Examples with NonLinearEvolution org.hipparchus.filtering.kalman.extended.NonLinearEvolution used on opensource projects

Example 1 with NonLinearEvolution

use of org.hipparchus.filtering.kalman.extended.NonLinearEvolution 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);
    }
}