Examples with TurnAroundRange org.orekit.estimation.measurements.TurnAroundRange used on opensource projects

Example 6 with TurnAroundRange

use of org.orekit.estimation.measurements.TurnAroundRange in project Orekit by CS-SI.

the class TurnAroundRangeTroposphericDelayModifier method modify.

/**
 * {@inheritDoc}
 */
@Override
public void modify(final EstimatedMeasurement<TurnAroundRange> estimated) throws OrekitException {
    final TurnAroundRange measurement = estimated.getObservedMeasurement();
    final GroundStation masterStation = measurement.getMasterStation();
    final GroundStation slaveStation = measurement.getSlaveStation();
    final SpacecraftState state = estimated.getStates()[0];
    final double[] oldValue = estimated.getEstimatedValue();
    // Update estimated value taking into account the tropospheric delay.
    // The tropospheric delay is directly added to the TurnAroundRange.
    final double masterDelay = rangeErrorTroposphericModel(masterStation, state);
    final double slaveDelay = rangeErrorTroposphericModel(slaveStation, state);
    final double[] newValue = oldValue.clone();
    newValue[0] = newValue[0] + masterDelay + slaveDelay;
    estimated.setEstimatedValue(newValue);
    // Update estimated derivatives with Jacobian of the measure wrt state
    final double[][] masterDjac = rangeErrorJacobianState(masterStation, state);
    final double[][] slaveDjac = rangeErrorJacobianState(slaveStation, state);
    final double[][] stateDerivatives = estimated.getStateDerivatives(0);
    for (int irow = 0; irow < stateDerivatives.length; ++irow) {
        for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
            stateDerivatives[irow][jcol] += masterDjac[irow][jcol] + slaveDjac[irow][jcol];
        }
    }
    estimated.setStateDerivatives(0, stateDerivatives);
    // Update derivatives with respect to master station position
    for (final ParameterDriver driver : Arrays.asList(masterStation.getEastOffsetDriver(), masterStation.getNorthOffsetDriver(), masterStation.getZenithOffsetDriver())) {
        if (driver.isSelected()) {
            double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
            parameterDerivative += rangeErrorParameterDerivative(masterStation, driver, state);
            estimated.setParameterDerivatives(driver, parameterDerivative);
        }
    }
    // Update derivatives with respect to slave station position
    for (final ParameterDriver driver : Arrays.asList(slaveStation.getEastOffsetDriver(), slaveStation.getNorthOffsetDriver(), slaveStation.getZenithOffsetDriver())) {
        if (driver.isSelected()) {
            double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
            parameterDerivative += rangeErrorParameterDerivative(slaveStation, driver, state);
            estimated.setParameterDerivatives(driver, parameterDerivative);
        }
    }
}