Examples with ParameterDriver org.orekit.utils.ParameterDriver used on opensource projects

Example 61 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class TurnAroundRange method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Turn around range derivatives are computed with respect to:
    // - Spacecraft state in inertial frame
    // - Master station parameters
    // - Slave station parameters
    // --------------------------
    // 
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - stations' parameters (stations' offsets, pole, prime meridian...)
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        // as one set only (they are combined together by the estimation engine)
        if (driver.isSelected() && !indices.containsKey(driver.getName())) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Place the derivative structures in a time-stamped PV
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // The path of the signal is divided in two legs.
    // Leg1: Emission from master station to satellite in masterTauU seconds
    // + Reflection from satellite to slave station in slaveTauD seconds
    // Leg2: Reflection from slave station to satellite in slaveTauU seconds
    // + Reflection from satellite to master station in masterTaudD seconds
    // The measurement is considered to be time stamped at reception on ground
    // by the master station. All times are therefore computed as backward offsets
    // with respect to this reception time.
    // 
    // Two intermediate spacecraft states are defined:
    // - transitStateLeg2: State of the satellite when it bounced back the signal
    // from slave station to master station during the 2nd leg
    // - transitStateLeg1: State of the satellite when it bounced back the signal
    // from master station to slave station during the 1st leg
    // Compute propagation time for the 2nd leg of the signal path
    // --
    // Time difference between t (date of the measurement) and t' (date tagged in spacecraft state)
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta = masterTauD + slaveTauU)
    final AbsoluteDate measurementDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> measurementDateDS = new FieldAbsoluteDate<>(field, measurementDate);
    final double delta = measurementDate.durationFrom(state.getDate());
    // transform between master station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    final FieldTransform<DerivativeStructure> masterToInert = masterStation.getOffsetToInertial(state.getFrame(), measurementDateDS, factory, indices);
    // Master station PV in inertial frame at measurement date
    final TimeStampedFieldPVCoordinates<DerivativeStructure> masterArrival = masterToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
    // Compute propagation times
    final DerivativeStructure masterTauD = signalTimeOfFlight(pvaDS, masterArrival.getPosition(), measurementDateDS);
    // Elapsed time between state date t' and signal arrival to the transit state of the 2nd leg
    final DerivativeStructure dtLeg2 = masterTauD.negate().add(delta);
    // Transit state where the satellite reflected the signal from slave to master station
    final SpacecraftState transitStateLeg2 = state.shiftedBy(dtLeg2.getValue());
    // Transit state pv of leg2 (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateLeg2PV = pvaDS.shiftedBy(dtLeg2);
    // transform between slave station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    // The components of slave station's position in offset frame are the 3 last derivative parameters
    final FieldAbsoluteDate<DerivativeStructure> approxReboundDate = measurementDateDS.shiftedBy(-delta);
    final FieldTransform<DerivativeStructure> slaveToInertApprox = slaveStation.getOffsetToInertial(state.getFrame(), approxReboundDate, factory, indices);
    // Slave station PV in inertial frame at approximate rebound date on slave station
    final TimeStampedFieldPVCoordinates<DerivativeStructure> QSlaveApprox = slaveToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxReboundDate, zero, zero, zero));
    // Uplink time of flight from slave station to transit state of leg2
    final DerivativeStructure slaveTauU = signalTimeOfFlight(QSlaveApprox, transitStateLeg2PV.getPosition(), transitStateLeg2PV.getDate());
    // Total time of flight for leg 2
    final DerivativeStructure tauLeg2 = masterTauD.add(slaveTauU);
    // Compute propagation time for the 1st leg of the signal path
    // --
    // Absolute date of rebound of the signal to slave station
    final FieldAbsoluteDate<DerivativeStructure> reboundDateDS = measurementDateDS.shiftedBy(tauLeg2.negate());
    final FieldTransform<DerivativeStructure> slaveToInert = slaveStation.getOffsetToInertial(state.getFrame(), reboundDateDS, factory, indices);
    // Slave station PV in inertial frame at rebound date on slave station
    final TimeStampedFieldPVCoordinates<DerivativeStructure> slaveRebound = slaveToInert.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(reboundDateDS, FieldPVCoordinates.getZero(field)));
    // Downlink time of flight from transitStateLeg1 to slave station at rebound date
    final DerivativeStructure slaveTauD = signalTimeOfFlight(transitStateLeg2PV, slaveRebound.getPosition(), reboundDateDS);
    // Elapsed time between state date t' and signal arrival to the transit state of the 1st leg
    final DerivativeStructure dtLeg1 = dtLeg2.subtract(slaveTauU).subtract(slaveTauD);
    // Transit state pv of leg2 (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateLeg1PV = pvaDS.shiftedBy(dtLeg1);
    // transform between master station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    // The components of master station's position in offset frame are the 3 third derivative parameters
    final FieldAbsoluteDate<DerivativeStructure> approxEmissionDate = measurementDateDS.shiftedBy(-2 * (slaveTauU.getValue() + masterTauD.getValue()));
    final FieldTransform<DerivativeStructure> masterToInertApprox = masterStation.getOffsetToInertial(state.getFrame(), approxEmissionDate, factory, indices);
    // Master station PV in inertial frame at approximate emission date
    final TimeStampedFieldPVCoordinates<DerivativeStructure> QMasterApprox = masterToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxEmissionDate, zero, zero, zero));
    // Uplink time of flight from master station to transit state of leg1
    final DerivativeStructure masterTauU = signalTimeOfFlight(QMasterApprox, transitStateLeg1PV.getPosition(), transitStateLeg1PV.getDate());
    // Master station PV in inertial frame at exact emission date
    final AbsoluteDate emissionDate = transitStateLeg1PV.getDate().toAbsoluteDate().shiftedBy(-masterTauU.getValue());
    final TimeStampedPVCoordinates masterDeparture = masterToInertApprox.shiftedBy(emissionDate.durationFrom(masterToInertApprox.getDate())).transformPVCoordinates(new TimeStampedPVCoordinates(emissionDate, PVCoordinates.ZERO)).toTimeStampedPVCoordinates();
    // Total time of flight for leg 1
    final DerivativeStructure tauLeg1 = slaveTauD.add(masterTauU);
    // --
    // Evaluate the turn-around range value and its derivatives
    // --------------------------------------------------------
    // The state we use to define the estimated measurement is a middle ground between the two transit states
    // This is done to avoid calling "SpacecraftState.shiftedBy" function on long duration
    // Thus we define the state at the date t" = date of rebound of the signal at the slave station
    // Or t" = t -masterTauD -slaveTauU
    // The iterative process in the estimation ensures that, after several iterations, the date stamped in the
    // state S in input of this function will be close to t"
    // Therefore we will shift state S by:
    // - +slaveTauU to get transitStateLeg2
    // - -slaveTauD to get transitStateLeg1
    final EstimatedMeasurement<TurnAroundRange> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitStateLeg2.shiftedBy(-slaveTauU.getValue()) }, new TimeStampedPVCoordinates[] { masterDeparture, transitStateLeg1PV.toTimeStampedPVCoordinates(), slaveRebound.toTimeStampedPVCoordinates(), transitStateLeg2.getPVCoordinates(), masterArrival.toTimeStampedPVCoordinates() });
    // Turn-around range value = Total time of flight for the 2 legs divided by 2 and multiplied by c
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final DerivativeStructure turnAroundRange = (tauLeg2.add(tauLeg1)).multiply(cOver2);
    estimated.setEstimatedValue(turnAroundRange.getValue());
    // Turn-around range partial derivatives with respect to state
    final double[] derivatives = turnAroundRange.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    return estimated;
}

Example 62 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class Bias method modify.

/**
 * {@inheritDoc}
 */
@Override
public void modify(final EstimatedMeasurement<T> estimated) {
    // apply the bias to the measurement value
    final double[] value = estimated.getEstimatedValue();
    for (int i = 0; i < drivers.size(); ++i) {
        final ParameterDriver driver = drivers.get(i);
        value[i] += driver.getValue();
        if (driver.isSelected()) {
            // add the partial derivatives
            estimated.setParameterDerivatives(driver, derivatives[i]);
        }
    }
    estimated.setEstimatedValue(value);
}

Example 63 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class BatchLSEstimator method getPhysicalCovariances.

/**
 * Get the covariances matrix in space flight dynamics physical units.
 * <p>
 * This method retrieve the {@link
 * org.hipparchus.optim.nonlinear.vector.leastsquares.LeastSquaresProblem.Evaluation#getCovariances(double)
 * covariances} from the [@link {@link #getOptimum() optimum} and applies the scaling factors
 * to it in order to convert it from raw normalized values back to physical values.
 * </p>
 * @param threshold threshold to identify matrix singularity
 * @return covariances matrix in space flight dynamics physical units
 * @exception OrekitException if the covariance matrix cannot be computed (singular problem).
 * @since 9.1
 */
public RealMatrix getPhysicalCovariances(final double threshold) throws OrekitException {
    final RealMatrix covariances;
    try {
        // get the normalized matrix
        covariances = optimum.getCovariances(threshold).copy();
    } catch (MathIllegalArgumentException miae) {
        // the problem is singular
        throw new OrekitException(miae);
    }
    // retrieve the scaling factors
    final double[] scale = new double[covariances.getRowDimension()];
    int index = 0;
    for (final ParameterDriver driver : getOrbitalParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : getPropagatorParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : getMeasurementsParametersDrivers(true).getDrivers()) {
        scale[index++] = driver.getScale();
    }
    // unnormalize the matrix, to retrieve physical covariances
    for (int i = 0; i < covariances.getRowDimension(); ++i) {
        for (int j = 0; j < covariances.getColumnDimension(); ++j) {
            covariances.setEntry(i, j, scale[i] * scale[j] * covariances.getEntry(i, j));
        }
    }
    return covariances;
}

Example 64 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class BatchLSEstimator method getMeasurementsParametersDrivers.

/**
 * Get the measurements parameters supported by this estimator (including measurements and modifiers).
 * @param estimatedOnly if true, only estimated parameters are returned
 * @return measurements parameters supported by this estimator
 * @exception OrekitException if different parameters have the same name
 */
public ParameterDriversList getMeasurementsParametersDrivers(final boolean estimatedOnly) throws OrekitException {
    final ParameterDriversList parameters = new ParameterDriversList();
    for (final ObservedMeasurement<?> measurement : measurements) {
        for (final ParameterDriver driver : measurement.getParametersDrivers()) {
            if ((!estimatedOnly) || driver.isSelected()) {
                parameters.add(driver);
            }
        }
    }
    parameters.sort();
    return parameters;
}

Example 65 with ParameterDriver

use of org.orekit.utils.ParameterDriver in project Orekit by CS-SI.

the class AngularAzEl method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<AngularAzEl> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Azimuth/elevation derivatives are computed with respect to spacecraft state in inertial frame
    // and station parameters
    // ----------------------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    // Get the number of parameters used for derivation
    // Place the selected drivers into a map
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // Transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position/velocity in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Station topocentric frame (east-north-zenith) in inertial frame expressed as DerivativeStructures
    final FieldVector3D<DerivativeStructure> east = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusI(field));
    final FieldVector3D<DerivativeStructure> north = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusJ(field));
    final FieldVector3D<DerivativeStructure> zenith = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusK(field));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
    // Station-satellite vector expressed in inertial frame
    final FieldVector3D<DerivativeStructure> staSat = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
    // Compute azimuth/elevation
    final DerivativeStructure baseAzimuth = DerivativeStructure.atan2(staSat.dotProduct(east), staSat.dotProduct(north));
    final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) - baseAzimuth.getReal();
    final DerivativeStructure azimuth = baseAzimuth.add(twoPiWrap);
    final DerivativeStructure elevation = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin();
    // Prepare the estimation
    final EstimatedMeasurement<AngularAzEl> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { transitStateDS.toTimeStampedPVCoordinates(), stationDownlink.toTimeStampedPVCoordinates() });
    // azimuth - elevation values
    estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue());
    // Partial derivatives of azimuth/elevation with respect to state
    // (beware element at index 0 is the value, not a derivative)
    final double[] azDerivatives = azimuth.getAllDerivatives();
    final double[] elDerivatives = elevation.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(azDerivatives, 1, 7), Arrays.copyOfRange(elDerivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, azDerivatives[index + 1], elDerivatives[index + 1]);
        }
    }
    return estimated;
}