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

Example 66 with ParameterDriver

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

the class Model method getEvolution.

/**
 * {@inheritDoc}
 */
@Override
public NonLinearEvolution getEvolution(final double previousTime, final RealVector previousState, final MeasurementDecorator measurement) throws OrekitExceptionWrapper {
    try {
        // Set a reference date for all measurements parameters that lack one (including the not estimated ones)
        final ObservedMeasurement<?> observedMeasurement = measurement.getObservedMeasurement();
        for (final ParameterDriver driver : observedMeasurement.getParametersDrivers()) {
            if (driver.getReferenceDate() == null) {
                driver.setReferenceDate(builder.getInitialOrbitDate());
            }
        }
        ++currentMeasurementNumber;
        currentDate = measurement.getObservedMeasurement().getDate();
        // Note:
        // - N = size of the current measurement
        // Example:
        // * 1 for Range, RangeRate and TurnAroundRange
        // * 2 for Angular (Azimuth/Elevation or Right-ascension/Declination)
        // * 6 for Position/Velocity
        // - M = size of the state vector. N = nbOrb + nbPropag + nbMeas
        // Propagate the reference trajectory to measurement date
        predictedSpacecraftStates[0] = referenceTrajectory.propagate(observedMeasurement.getDate());
        // Predict the state vector (Mx1)
        final RealVector predictedState = predictState(predictedSpacecraftStates[0].getOrbit());
        // Get the error state transition matrix (MxM)
        final RealMatrix stateTransitionMatrix = getErrorStateTransitionMatrix(predictedSpacecraftStates[0]);
        // Predict the measurement based on predicted spacecraft state
        // Compute the innovations (i.e. residuals of the predicted measurement)
        // ------------------------------------------------------------
        // Predicted measurement
        // Note: here the "iteration/evaluation" formalism from the batch LS method
        // is twisted to fit the need of the Kalman filter.
        // The number of "iterations" is actually the number of measurements processed by the filter
        // so far. We use this to be able to apply the OutlierFilter modifiers on the predicted measurement.
        predictedMeasurement = observedMeasurement.estimate(currentMeasurementNumber, currentMeasurementNumber, predictedSpacecraftStates);
        // Normalized measurement matrix (NxM)
        final RealMatrix measurementMatrix = getMeasurementMatrix(predictedSpacecraftStates[0]);
        // compute process noise matrix
        final SpacecraftState previous = correctedSpacecraftStates[0];
        final RealMatrix physicalProcessNoise = processNoiseMatrixProvider.getProcessNoiseMatrix(previous, predictedSpacecraftStates[0]);
        final RealMatrix normalizedProcessNoise = normalizeCovarianceMatrix(physicalProcessNoise);
        return new NonLinearEvolution(measurement.getTime(), predictedState, stateTransitionMatrix, normalizedProcessNoise, measurementMatrix);
    } catch (OrekitException oe) {
        throw new OrekitExceptionWrapper(oe);
    }
}

Example 67 with ParameterDriver

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

the class Model method getParametersScale.

/**
 * Gather the different scaling factors of the estimated parameters in an array.
 * @return the array of scales (i.e. scaling factors)
 */
private double[] getParametersScale() {
    // Retrieve the scale factors
    final double[] scale = new double[nbOrbitalParameters + nbPropagationParameters + nbMeasurementsParameters];
    int index = 0;
    for (final ParameterDriver driver : estimatedOrbitalParameters.getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : estimatedPropagationParameters.getDrivers()) {
        scale[index++] = driver.getScale();
    }
    for (final ParameterDriver driver : estimatedMeasurementsParameters.getDrivers()) {
        scale[index++] = driver.getScale();
    }
    return scale;
}

Example 68 with ParameterDriver

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

the class RangeRateTroposphericDelayModifier method modify.

/**
 * {@inheritDoc}
 */
@Override
public void modify(final EstimatedMeasurement<RangeRate> estimated) throws OrekitException {
    final RangeRate measurement = estimated.getObservedMeasurement();
    final GroundStation station = measurement.getStation();
    final SpacecraftState state = estimated.getStates()[0];
    final double[] oldValue = estimated.getEstimatedValue();
    final double delay = rangeRateErrorTroposphericModel(station, state);
    // update estimated value taking into account the tropospheric delay.
    // The tropospheric 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 = rangeRateErrorJacobianState(station, state, delay);
    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 += rangeRateErrorParameterDerivative(station, driver, state, delay);
            estimated.setParameterDerivatives(driver, parameterDerivative);
        }
    }
}

Example 69 with ParameterDriver

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

the class TurnAroundRangeIonosphericDelayModifier 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
 * @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) 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 70 with ParameterDriver

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

the class RangeRateIonosphericDelayModifier method modify.

/**
 * {@inheritDoc}
 */
@Override
public void modify(final EstimatedMeasurement<RangeRate> estimated) throws OrekitException {
    final RangeRate measurement = estimated.getObservedMeasurement();
    final GroundStation station = measurement.getStation();
    final SpacecraftState state = estimated.getStates()[0];
    final double[] oldValue = estimated.getEstimatedValue();
    final double delay = rangeRateErrorIonosphericModel(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 += rangeRateErrorParameterDerivative(station, driver, state, delay);
            estimated.setParameterDerivatives(driver, parameterDerivative);
        }
    }
}