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

Example 21 with ParameterDriver

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

the class Model method getMeasurementMatrix.

/**
 * Get the normalized measurement matrix H.
 * H contains the partial derivatives of the measurement with respect to the state.
 * H is an NxM matrix where N is the size of the measurement vector and M the size of the state vector.
 * @param predictedSpacecraftState the spacecraft state associated with the measurement
 * @return the normalized measurement matrix H
 * @throws OrekitException if Jacobians cannot be computed
 */
private RealMatrix getMeasurementMatrix(final SpacecraftState predictedSpacecraftState) throws OrekitException {
    // Number of parameters
    final int nbOrb = estimatedOrbitalParameters.getNbParams();
    final int nbPropag = estimatedPropagationParameters.getNbParams();
    final int nbMeas = estimatedMeasurementsParameters.getNbParams();
    // Initialize measurement matrix H: N x M
    // N: Number of measurements in current measurement
    // M: State vector size
    final RealMatrix measurementMatrix = MatrixUtils.createRealMatrix(predictedMeasurement.getEstimatedValue().length, nbOrb + nbPropag + nbMeas);
    // Predicted orbit
    final Orbit predictedOrbit = predictedSpacecraftState.getOrbit();
    // Observed measurement characteristics
    final ObservedMeasurement<?> observedMeasurement = predictedMeasurement.getObservedMeasurement();
    final double[] sigma = observedMeasurement.getTheoreticalStandardDeviation();
    // Measurement matrix's columns related to orbital parameters
    // ----------------------------------------------------------
    // Partial derivatives of the current Cartesian coordinates with respect to current orbital state
    final double[][] aCY = new double[6][6];
    // dC/dY
    predictedOrbit.getJacobianWrtParameters(builder.getPositionAngle(), aCY);
    final RealMatrix dCdY = new Array2DRowRealMatrix(aCY, false);
    // Jacobian of the measurement with respect to current Cartesian coordinates
    final RealMatrix dMdC = new Array2DRowRealMatrix(predictedMeasurement.getStateDerivatives(0), false);
    // Jacobian of the measurement with respect to current orbital state
    final RealMatrix dMdY = dMdC.multiply(dCdY);
    // Fill the normalized measurement matrix's columns related to estimated orbital parameters
    if (nbOrb > 0) {
        for (int i = 0; i < dMdY.getRowDimension(); ++i) {
            int jOrb = 0;
            for (int j = 0; j < dMdY.getColumnDimension(); ++j) {
                final DelegatingDriver delegating = builder.getOrbitalParametersDrivers().getDrivers().get(j);
                if (delegating.isSelected()) {
                    measurementMatrix.setEntry(i, jOrb++, dMdY.getEntry(i, j) / sigma[i] * delegating.getScale());
                }
            }
        }
    }
    // Jacobian of the measurement with respect to propagation parameters
    if (nbPropag > 0) {
        final double[][] aYPp = new double[6][nbPropag];
        jacobiansMapper.getParametersJacobian(predictedSpacecraftState, aYPp);
        final RealMatrix dYdPp = new Array2DRowRealMatrix(aYPp, false);
        final RealMatrix dMdPp = dMdY.multiply(dYdPp);
        for (int i = 0; i < dMdPp.getRowDimension(); ++i) {
            for (int j = 0; j < nbPropag; ++j) {
                final DelegatingDriver delegating = estimatedPropagationParameters.getDrivers().get(j);
                measurementMatrix.setEntry(i, nbOrb + j, dMdPp.getEntry(i, j) / sigma[i] * delegating.getScale());
            }
        }
    }
    // Jacobian of the measurement with respect to measurement parameters
    if (nbMeas > 0) {
        // Gather the measurement parameters linked to current measurement
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.isSelected()) {
                // Derivatives of current measurement w/r to selected measurement parameter
                final double[] aMPm = predictedMeasurement.getParameterDerivatives(driver);
                // Check that the measurement parameter is managed by the filter
                if (measurementParameterColumns.get(driver.getName()) != null) {
                    // Column of the driver in the measurement matrix
                    final int driverColumn = measurementParameterColumns.get(driver.getName());
                    // Fill the corresponding indexes of the measurement matrix
                    for (int i = 0; i < aMPm.length; ++i) {
                        measurementMatrix.setEntry(i, driverColumn, aMPm[i] / sigma[i] * driver.getScale());
                    }
                }
            }
        }
    }
    // Return the normalized measurement matrix
    return measurementMatrix;
}

Example 22 with ParameterDriver

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

the class RangeIonosphericDelayModifier method modify.

@Override
public void modify(final EstimatedMeasurement<Range> estimated) throws OrekitException {
    final Range measurement = estimated.getObservedMeasurement();
    final GroundStation station = measurement.getStation();
    final SpacecraftState state = estimated.getStates()[0];
    final double[] oldValue = estimated.getEstimatedValue();
    final double delay = rangeErrorIonosphericModel(station, state);
    // update estimated value taking into account the ionospheric delay.
    // The ionospheric delay is directly added to the range.
    final double[] newValue = oldValue.clone();
    newValue[0] = newValue[0] + delay;
    estimated.setEstimatedValue(newValue);
    // update estimated derivatives with Jacobian of the measure wrt state
    final double[][] djac = rangeErrorJacobianState(station, 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] += djac[irow][jcol];
        }
    }
    estimated.setStateDerivatives(0, stateDerivatives);
    for (final ParameterDriver driver : Arrays.asList(station.getEastOffsetDriver(), station.getNorthOffsetDriver(), station.getZenithOffsetDriver())) {
        if (driver.isSelected()) {
            // update estimated derivatives with derivative of the modification wrt station parameters
            double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
            parameterDerivative += rangeErrorParameterDerivative(station, driver, state, delay);
            estimated.setParameterDerivatives(driver, parameterDerivative);
        }
    }
}

Example 23 with ParameterDriver

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

the class RangeIonosphericDelayModifier method rangeErrorParameterDerivative.

/**
 * Compute the derivative of the delay term wrt parameters.
 *
 * @param station ground station
 * @param driver driver for the station offset parameter
 * @param state spacecraft state
 * @param delay current ionospheric delay
 * @return derivative of the delay wrt station offset parameter
 * @throws OrekitException  if frames transformations cannot be computed
 */
private double rangeErrorParameterDerivative(final GroundStation station, final ParameterDriver driver, final SpacecraftState state, final double delay) throws OrekitException {
    final ParameterFunction rangeError = new ParameterFunction() {

        /**
         * {@inheritDoc}
         */
        @Override
        public double value(final ParameterDriver parameterDriver) throws OrekitException {
            return rangeErrorIonosphericModel(station, state);
        }
    };
    final ParameterFunction rangeErrorDerivative = Differentiation.differentiate(rangeError, driver, 3, 10.0);
    return rangeErrorDerivative.value(driver);
}

Example 24 with ParameterDriver

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

the class RangeRateTroposphericDelayModifier method rangeRateErrorParameterDerivative.

/**
 * Compute the derivative of the delay term wrt parameters.
 *
 * @param station ground station
 * @param driver driver for the station offset parameter
 * @param state spacecraft state
 * @param delay current ionospheric delay
 * @return derivative of the delay wrt station offset parameter
 * @throws OrekitException  if frames transformations cannot be computed
 */
private double rangeRateErrorParameterDerivative(final GroundStation station, final ParameterDriver driver, final SpacecraftState state, final double delay) throws OrekitException {
    final ParameterFunction rangeError = new ParameterFunction() {

        /**
         * {@inheritDoc}
         */
        @Override
        public double value(final ParameterDriver parameterDriver) throws OrekitException {
            return rangeRateErrorTroposphericModel(station, state);
        }
    };
    final ParameterFunction rangeErrorDerivative = Differentiation.differentiate(rangeError, driver, 3, 10.0);
    return rangeErrorDerivative.value(driver);
}

Example 25 with ParameterDriver

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

the class TurnAroundRangeIonosphericDelayModifier method modify.

@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 ionospheric delay.
    // The ionospheric delay is directly added to the TurnAroundRange.
    final double masterDelay = rangeErrorIonosphericModel(masterStation, state);
    final double slaveDelay = rangeErrorIonosphericModel(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);
        }
    }
}