Examples with RealVector org.hipparchus.linear.RealVector used on opensource projects

Example 6 with RealVector

use of org.hipparchus.linear.RealVector in project Orekit by CS-SI.

the class JacobianPropagatorConverterTest method doTestDerivatives.

private void doTestDerivatives(double tolP, double tolV, String... names) throws OrekitException {
    // we use a fixed step integrator on purpose
    // as the test is based on external differentiation using finite differences,
    // an adaptive step size integrator would introduce *lots* of numerical noise
    NumericalPropagatorBuilder builder = new NumericalPropagatorBuilder(OrbitType.CARTESIAN.convertType(orbit), new LutherIntegratorBuilder(10.0), PositionAngle.TRUE, dP);
    builder.setMass(200.0);
    builder.addForceModel(drag);
    builder.addForceModel(gravity);
    // retrieve a state slightly different from the initial state,
    // using normalized values different from 0.0 for the sake of generality
    RandomGenerator random = new Well19937a(0xe67f19c1a678d037l);
    List<ParameterDriver> all = new ArrayList<ParameterDriver>();
    for (final ParameterDriver driver : builder.getOrbitalParametersDrivers().getDrivers()) {
        all.add(driver);
    }
    for (final ParameterDriver driver : builder.getPropagationParametersDrivers().getDrivers()) {
        all.add(driver);
    }
    double[] normalized = new double[names.length];
    List<ParameterDriver> selected = new ArrayList<ParameterDriver>(names.length);
    int index = 0;
    for (final ParameterDriver driver : all) {
        boolean found = false;
        for (final String name : names) {
            if (name.equals(driver.getName())) {
                found = true;
                normalized[index++] = driver.getNormalizedValue() + (2 * random.nextDouble() - 1);
                selected.add(driver);
            }
        }
        driver.setSelected(found);
    }
    // create a one hour sample that starts 10 minutes after initial state
    // the 10 minutes offset implies even the first point is influenced by model parameters
    final List<SpacecraftState> sample = new ArrayList<SpacecraftState>();
    Propagator propagator = builder.buildPropagator(normalized);
    propagator.setMasterMode(60.0, new OrekitFixedStepHandler() {

        @Override
        public void handleStep(SpacecraftState currentState, boolean isLast) {
            sample.add(currentState);
        }
    });
    propagator.propagate(orbit.getDate().shiftedBy(600.0), orbit.getDate().shiftedBy(4200.0));
    JacobianPropagatorConverter fitter = new JacobianPropagatorConverter(builder, 1.0e-3, 5000);
    try {
        Method setSample = AbstractPropagatorConverter.class.getDeclaredMethod("setSample", List.class);
        setSample.setAccessible(true);
        setSample.invoke(fitter, sample);
    } catch (NoSuchMethodException | SecurityException | IllegalAccessException | IllegalArgumentException | InvocationTargetException e) {
        Assert.fail(e.getLocalizedMessage());
    }
    MultivariateVectorFunction f = fitter.getObjectiveFunction();
    Pair<RealVector, RealMatrix> p = fitter.getModel().value(new ArrayRealVector(normalized));
    // check derivatives
    // a h offset on normalized parameter represents a physical offset of h * scale
    RealMatrix m = p.getSecond();
    double h = 10.0;
    double[] shifted = normalized.clone();
    double maxErrorP = 0;
    double maxErrorV = 0;
    for (int j = 0; j < selected.size(); ++j) {
        shifted[j] = normalized[j] + 2.0 * h;
        double[] valueP2 = f.value(shifted);
        shifted[j] = normalized[j] + 1.0 * h;
        double[] valueP1 = f.value(shifted);
        shifted[j] = normalized[j] - 1.0 * h;
        double[] valueM1 = f.value(shifted);
        shifted[j] = normalized[j] - 2.0 * h;
        double[] valueM2 = f.value(shifted);
        shifted[j] = normalized[j];
        for (int i = 0; i < valueP2.length; ++i) {
            double d = (8 * (valueP1[i] - valueM1[i]) - (valueP2[i] - valueM2[i])) / (12 * h);
            if (i % 6 < 3) {
                // position
                maxErrorP = FastMath.max(maxErrorP, FastMath.abs(m.getEntry(i, j) - d));
            } else {
                // velocity
                maxErrorV = FastMath.max(maxErrorV, FastMath.abs(m.getEntry(i, j) - d));
            }
        }
    }
    Assert.assertEquals(0.0, maxErrorP, tolP);
    Assert.assertEquals(0.0, maxErrorV, tolV);
}

Example 7 with RealVector

use of org.hipparchus.linear.RealVector 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 8 with RealVector

use of org.hipparchus.linear.RealVector in project Orekit by CS-SI.

the class Model method predictState.

/**
 * Set the predicted normalized state vector.
 * The predicted/propagated orbit is used to update the state vector
 * @param predictedOrbit the predicted orbit at measurement date
 * @return predicted state
 * @throws OrekitException if the propagator builder could not be reset
 */
private RealVector predictState(final Orbit predictedOrbit) throws OrekitException {
    // First, update the builder with the predicted orbit
    // This updates the orbital drivers with the values of the predicted orbit
    builder.resetOrbit(predictedOrbit);
    // Predicted state is initialized to previous estimated state
    final RealVector predictedState = correctedEstimate.getState().copy();
    // The propagation & measurement parameters are not changed by the prediction (i.e. the propagation)
    if (nbOrbitalParameters > 0) {
        // As the propagator builder was previously updated with the predicted orbit,
        // the selected orbital drivers are already up to date with the prediction
        // Orbital parameters counter
        int jOrb = 0;
        for (DelegatingDriver orbitalDriver : builder.getOrbitalParametersDrivers().getDrivers()) {
            if (orbitalDriver.isSelected()) {
                predictedState.setEntry(jOrb++, orbitalDriver.getNormalizedValue());
            }
        }
    }
    return predictedState;
}