Examples with NumericalPropagator org.orekit.propagation.numerical.NumericalPropagator used on opensource projects

Example 16 with NumericalPropagator

use of org.orekit.propagation.numerical.NumericalPropagator in project Orekit by CS-SI.

the class KalmanEstimatorTest method testKeplerianRangeAndRangeRate.

/**
 * Perfect range and range rate measurements with a perfect start
 * @throws OrekitException
 */
@Test
public void testKeplerianRangeAndRangeRate() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.KEPLERIAN;
    final PositionAngle positionAngle = PositionAngle.TRUE;
    final boolean perfectStart = true;
    final double minStep = 1.e-6;
    final double maxStep = 60.;
    final double dP = 1.;
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
    // Create perfect range & range rate measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurementsRange = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    final List<ObservedMeasurement<?>> measurementsRangeRate = EstimationTestUtils.createMeasurements(propagator, new RangeRateMeasurementCreator(context, false), 1.0, 3.0, 300.0);
    // Concatenate measurements
    final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
    measurements.addAll(measurementsRange);
    measurements.addAll(measurementsRangeRate);
    // Reference propagator for estimation performances
    final NumericalPropagator referencePropagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
    // Reference position/velocity at last measurement date
    final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
    // Cartesian covariance matrix initialization
    // 100m on position / 1e-2m/s on velocity
    final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-2, 1e-2, 1e-2, 1e-8, 1e-8, 1e-8 });
    // Jacobian of the orbital parameters w/r to Cartesian
    final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
    final double[][] dYdC = new double[6][6];
    initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
    final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
    // Keplerian initial covariance matrix
    final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
    // Process noise matrix
    final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10 });
    final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
    // Build the Kalman filter
    final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
    kalmanBuilder.builder(propagatorBuilder);
    kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
    kalmanBuilder.initialCovarianceMatrix(initialP);
    kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
    final KalmanEstimator kalman = kalmanBuilder.build();
    // Filter the measurements and check the results
    final double expectedDeltaPos = 0.;
    final double posEps = 5.96e-3;
    final double expectedDeltaVel = 0.;
    final double velEps = 2.06e-6;
    final double[] expectedSigmasPos = { 0.341538, 8.175281, 4.634384 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 1.167838e-3, 1.036437e-3, 2.834385e-3 };
    final double sigmaVelEps = 1e-9;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedSigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}

Example 17 with NumericalPropagator

use of org.orekit.propagation.numerical.NumericalPropagator in project Orekit by CS-SI.

the class KalmanEstimatorTest method testKeplerianPV.

/**
 * Perfect PV measurements with a perfect start
 * Keplerian formalism
 * @throws OrekitException
 */
@Test
public void testKeplerianPV() throws OrekitException {
    // Create context
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Create initial orbit and propagator builder
    final OrbitType orbitType = OrbitType.KEPLERIAN;
    final PositionAngle positionAngle = PositionAngle.TRUE;
    final boolean perfectStart = true;
    final double minStep = 1.e-6;
    final double maxStep = 60.;
    final double dP = 1.;
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart, minStep, maxStep, dP);
    // Create perfect PV measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new PVMeasurementCreator(), 0.0, 3.0, 300.0);
    // Reference propagator for estimation performances
    final NumericalPropagator referencePropagator = propagatorBuilder.buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
    // Reference position/velocity at last measurement date
    final Orbit refOrbit = referencePropagator.propagate(measurements.get(measurements.size() - 1).getDate()).getOrbit();
    // Covariance matrix initialization
    final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[] { 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5 });
    // Process noise matrix
    RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double[] { 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8 });
    // Build the Kalman filter
    final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
    kalmanBuilder.builder(propagatorBuilder);
    kalmanBuilder.estimatedMeasurementsParameters(new ParameterDriversList());
    kalmanBuilder.initialCovarianceMatrix(initialP);
    kalmanBuilder.processNoiseMatrixProvider(new ConstantProcessNoise(Q));
    final KalmanEstimator kalman = kalmanBuilder.build();
    // Filter the measurements and check the results
    final double expectedDeltaPos = 0.;
    final double posEps = 5.80e-8;
    final double expectedDeltaVel = 0.;
    final double velEps = 2.28e-11;
    final double[] expectedsigmasPos = { 0.998872, 0.933655, 0.997516 };
    final double sigmaPosEps = 1e-6;
    final double[] expectedSigmasVel = { 9.478853e-4, 9.910788e-4, 5.0438709e-4 };
    final double sigmaVelEps = 1e-10;
    EstimationTestUtils.checkKalmanFit(context, kalman, measurements, refOrbit, positionAngle, expectedDeltaPos, posEps, expectedDeltaVel, velEps, expectedsigmasPos, sigmaPosEps, expectedSigmasVel, sigmaVelEps);
}

Example 18 with NumericalPropagator

use of org.orekit.propagation.numerical.NumericalPropagator in project Orekit by CS-SI.

the class EstimationTestUtils method checkKalmanFit.

/**
 * Checker for Kalman estimator validation
 * @param context context used for the test
 * @param kalman Kalman filter
 * @param measurements List of observed measurements to be processed by the Kalman
 * @param refOrbit Reference orbit at last measurement date
 * @param expectedDeltaPos Expected position difference between estimated orbit and reference orbit
 * @param posEps Tolerance on expected position difference
 * @param expectedDeltaVel Expected velocity difference between estimated orbit and reference orbit
 * @param velEps Tolerance on expected velocity difference
 * @param expectedSigmasPos Expected values for covariance matrix on position
 * @param sigmaPosEps Tolerance on expected covariance matrix on position
 * @param expectedSigmasVel Expected values for covariance matrix on velocity
 * @param sigmaVelEps Tolerance on expected covariance matrix on velocity
 * @throws OrekitException
 */
public static void checkKalmanFit(final Context context, final KalmanEstimator kalman, final List<ObservedMeasurement<?>> measurements, final Orbit refOrbit, final PositionAngle positionAngle, final double expectedDeltaPos, final double posEps, final double expectedDeltaVel, final double velEps, final double[] expectedSigmasPos, final double sigmaPosEps, final double[] expectedSigmasVel, final double sigmaVelEps) throws OrekitException {
    // Add the measurements to the Kalman filter
    NumericalPropagator estimated = kalman.processMeasurements(measurements);
    // Check the number of measurements processed by the filter
    Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
    // Get the last estimation
    final Orbit estimatedOrbit = estimated.getInitialState().getOrbit();
    final Vector3D estimatedPosition = estimatedOrbit.getPVCoordinates().getPosition();
    final Vector3D estimatedVelocity = estimatedOrbit.getPVCoordinates().getVelocity();
    // Get the last covariance matrix estimation
    final RealMatrix estimatedP = kalman.getPhysicalEstimatedCovarianceMatrix();
    // Convert the orbital part to Cartesian formalism
    // Assuming all 6 orbital parameters are estimated by the filter
    final double[][] dCdY = new double[6][6];
    estimatedOrbit.getJacobianWrtParameters(positionAngle, dCdY);
    final RealMatrix Jacobian = MatrixUtils.createRealMatrix(dCdY);
    final RealMatrix estimatedCartesianP = Jacobian.multiply(estimatedP.getSubMatrix(0, 5, 0, 5)).multiply(Jacobian.transpose());
    // Get the final sigmas (ie.sqrt of the diagonal of the Cartesian orbital covariance matrix)
    final double[] sigmas = new double[6];
    for (int i = 0; i < 6; i++) {
        sigmas[i] = FastMath.sqrt(estimatedCartesianP.getEntry(i, i));
    }
    // // FIXME: debug
    // final double dPos = Vector3D.distance(refOrbit.getPVCoordinates().getPosition(), estimatedPosition);
    // final double dVel = Vector3D.distance(refOrbit.getPVCoordinates().getVelocity(), estimatedVelocity);
    // System.out.println("Nb Meas = " + kalman.getCurrentMeasurementNumber());
    // System.out.println("dPos    = " + dPos + " m");
    // System.out.println("dVel    = " + dVel + " m/s");
    // System.out.println("sigmas  = " + sigmas[0] + " "
    // + sigmas[1] + " "
    // + sigmas[2] + " "
    // + sigmas[3] + " "
    // + sigmas[4] + " "
    // + sigmas[5]);
    // //debug
    // Check the final orbit estimation & PV sigmas
    Assert.assertEquals(expectedDeltaPos, Vector3D.distance(refOrbit.getPVCoordinates().getPosition(), estimatedPosition), posEps);
    Assert.assertEquals(expectedDeltaVel, Vector3D.distance(refOrbit.getPVCoordinates().getVelocity(), estimatedVelocity), velEps);
    for (int i = 0; i < 3; i++) {
        Assert.assertEquals(expectedSigmasPos[i], sigmas[i], sigmaPosEps);
        Assert.assertEquals(expectedSigmasVel[i], sigmas[i + 3], sigmaVelEps);
    }
}

Example 19 with NumericalPropagator

use of org.orekit.propagation.numerical.NumericalPropagator in project Orekit by CS-SI.

the class SecularAndHarmonicTest method createPropagator.

private NumericalPropagator createPropagator(CircularOrbit orbit) throws OrekitException {
    OrbitType type = OrbitType.CIRCULAR;
    double[][] tolerances = NumericalPropagator.tolerances(0.1, orbit, type);
    DormandPrince853Integrator integrator = new DormandPrince853Integrator(1.0, 600, tolerances[0], tolerances[1]);
    integrator.setInitialStepSize(60.0);
    NumericalPropagator propagator = new NumericalPropagator(integrator);
    propagator.addForceModel(new HolmesFeatherstoneAttractionModel(earth.getBodyFrame(), gravityField));
    propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun()));
    propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon()));
    propagator.setOrbitType(type);
    propagator.resetInitialState(new SpacecraftState(orbit));
    return propagator;
}

Example 20 with NumericalPropagator

use of org.orekit.propagation.numerical.NumericalPropagator in project Orekit by CS-SI.

the class OrekitStepHandlerTest method testIsInterpolated.

/**
 * Check {@link OrekitStepInterpolator#isPreviousStateInterpolated()} and {@link
 * OrekitStepInterpolator#isCurrentStateInterpolated()}.
 *
 * @throws OrekitException on error.
 */
@Test
public void testIsInterpolated() throws OrekitException {
    // setup
    NumericalPropagator propagator = new NumericalPropagator(new ClassicalRungeKuttaIntegrator(60));
    AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
    Frame eci = FramesFactory.getGCRF();
    SpacecraftState ic = new SpacecraftState(new KeplerianOrbit(6378137 + 500e3, 1e-3, 0, 0, 0, 0, PositionAngle.TRUE, eci, date, Constants.EIGEN5C_EARTH_MU));
    propagator.setInitialState(ic);
    propagator.setOrbitType(OrbitType.CARTESIAN);
    // detector triggers half way through second step
    DateDetector detector = new DateDetector(date.shiftedBy(90)).withHandler(new ContinueOnEvent<>());
    propagator.addEventDetector(detector);
    // action and verify
    Queue<Boolean> expected = new ArrayDeque<>(Arrays.asList(false, false, false, true, true, false));
    propagator.setMasterMode(new OrekitStepHandler() {

        @Override
        public void handleStep(OrekitStepInterpolator interpolator, boolean isLast) {
            assertEquals(expected.poll(), interpolator.isPreviousStateInterpolated());
            assertEquals(expected.poll(), interpolator.isCurrentStateInterpolated());
        }
    });
    final AbsoluteDate end = date.shiftedBy(120);
    assertEquals(end, propagator.propagate(end).getDate());
}