Examples with Well19937a org.hipparchus.random.Well19937a used on opensource projects

Example 46 with Well19937a

use of org.hipparchus.random.Well19937a in project Orekit by CS-SI.

the class NRLMSISE00Test method testgtd7SwitchesOff.

@Test
public void testgtd7SwitchesOff() throws OrekitException {
    RandomGenerator random = new Well19937a(0x3dc1f824e1033d1bl);
    NRLMSISE00 atm = new NRLMSISE00(null, null, null);
    for (int i = 1; i <= 23; ++i) {
        atm = atm.withSwitch(i, 0);
    }
    doTestVoidMethod(atm, random, "gtd7", 1.0e-50, 3.0e-14);
}

Example 47 with Well19937a

use of org.hipparchus.random.Well19937a in project Orekit by CS-SI.

the class RelativityTest method RealFieldTest.

/**
 *Testing if the propagation between the FieldPropagation and the propagation
 * is equivalent.
 * Also testing if propagating X+dX with the propagation is equivalent to
 * propagation X with the FieldPropagation and then applying the taylor
 * expansion of dX to the result.
 */
@Test
public void RealFieldTest() throws OrekitException {
    DSFactory factory = new DSFactory(6, 5);
    DerivativeStructure a_0 = factory.variable(0, 7e7);
    DerivativeStructure e_0 = factory.variable(1, 0.4);
    DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
    DerivativeStructure R_0 = factory.variable(3, 0.7);
    DerivativeStructure O_0 = factory.variable(4, 0.5);
    DerivativeStructure n_0 = factory.variable(5, 0.1);
    Field<DerivativeStructure> field = a_0.getField();
    DerivativeStructure zero = field.getZero();
    FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
    Frame EME = FramesFactory.getEME2000();
    FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0, PositionAngle.MEAN, EME, J2000, Constants.EIGEN5C_EARTH_MU);
    FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
    SpacecraftState iSR = initialState.toSpacecraftState();
    OrbitType type = OrbitType.KEPLERIAN;
    double[][] tolerance = NumericalPropagator.tolerances(0.001, FKO.toOrbit(), type);
    AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
    integrator.setInitialStepSize(zero.add(60));
    AdaptiveStepsizeIntegrator RIntegrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
    RIntegrator.setInitialStepSize(60);
    FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
    FNP.setOrbitType(type);
    FNP.setInitialState(initialState);
    NumericalPropagator NP = new NumericalPropagator(RIntegrator);
    NP.setOrbitType(type);
    NP.setInitialState(iSR);
    final Relativity forceModel = new Relativity(Constants.EIGEN5C_EARTH_MU);
    FNP.addForceModel(forceModel);
    NP.addForceModel(forceModel);
    FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
    FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
    SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
    FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
    PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
    long number = 23091991;
    RandomGenerator RG = new Well19937a(number);
    GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
    UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, new double[] { 1e3, 0.01, 0.01, 0.01, 0.01, 0.01 }, NGG);
    double a_R = a_0.getReal();
    double e_R = e_0.getReal();
    double i_R = i_0.getReal();
    double R_R = R_0.getReal();
    double O_R = O_0.getReal();
    double n_R = n_0.getReal();
    for (int ii = 0; ii < 1; ii++) {
        double[] rand_next = URVG.nextVector();
        double a_shift = a_R + rand_next[0];
        double e_shift = e_R + rand_next[1];
        double i_shift = i_R + rand_next[2];
        double R_shift = R_R + rand_next[3];
        double O_shift = O_R + rand_next[4];
        double n_shift = n_R + rand_next[5];
        KeplerianOrbit shiftedOrb = new KeplerianOrbit(a_shift, e_shift, i_shift, R_shift, O_shift, n_shift, PositionAngle.MEAN, EME, J2000.toAbsoluteDate(), Constants.EIGEN5C_EARTH_MU);
        SpacecraftState shift_iSR = new SpacecraftState(shiftedOrb);
        NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator);
        shift_NP.setInitialState(shift_iSR);
        shift_NP.addForceModel(forceModel);
        SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate());
        PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
        // position check
        FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
        double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        // System.out.println(pos_DS.getX().getPartialDerivative(1));
        double x = finPVC_shift.getPosition().getX();
        double y = finPVC_shift.getPosition().getY();
        double z = finPVC_shift.getPosition().getZ();
        Assert.assertEquals(x_DS, x, FastMath.abs(x - pos_DS.getX().getReal()) * 1e-8);
        Assert.assertEquals(y_DS, y, FastMath.abs(y - pos_DS.getY().getReal()) * 1e-8);
        Assert.assertEquals(z_DS, z, FastMath.abs(z - pos_DS.getZ().getReal()) * 1e-8);
        // velocity check
        FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
        double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vx = finPVC_shift.getVelocity().getX();
        double vy = finPVC_shift.getVelocity().getY();
        double vz = finPVC_shift.getVelocity().getZ();
        Assert.assertEquals(vx_DS, vx, FastMath.abs(vx) * 1e-9);
        Assert.assertEquals(vy_DS, vy, FastMath.abs(vy) * 1e-9);
        Assert.assertEquals(vz_DS, vz, FastMath.abs(vz) * 1e-9);
        // acceleration check
        FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
        double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double ax = finPVC_shift.getAcceleration().getX();
        double ay = finPVC_shift.getAcceleration().getY();
        double az = finPVC_shift.getAcceleration().getZ();
        Assert.assertEquals(ax_DS, ax, FastMath.abs(ax) * 1e-8);
        Assert.assertEquals(ay_DS, ay, FastMath.abs(ay) * 1e-8);
        Assert.assertEquals(az_DS, az, FastMath.abs(az) * 1e-8);
    }
}

Example 48 with Well19937a

use of org.hipparchus.random.Well19937a in project Orekit by CS-SI.

the class ThirdBodyAttractionTest method RealFieldTest.

/**
 *Testing if the propagation between the FieldPropagation and the propagation
 * is equivalent.
 * Also testing if propagating X+dX with the propagation is equivalent to
 * propagation X with the FieldPropagation and then applying the taylor
 * expansion of dX to the result.
 */
@Test
public void RealFieldTest() throws OrekitException {
    DSFactory factory = new DSFactory(6, 5);
    DerivativeStructure a_0 = factory.variable(0, 7e7);
    DerivativeStructure e_0 = factory.variable(1, 0.4);
    DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
    DerivativeStructure R_0 = factory.variable(3, 0.7);
    DerivativeStructure O_0 = factory.variable(4, 0.5);
    DerivativeStructure n_0 = factory.variable(5, 0.1);
    Field<DerivativeStructure> field = a_0.getField();
    DerivativeStructure zero = field.getZero();
    FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
    Frame EME = FramesFactory.getEME2000();
    FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0, PositionAngle.MEAN, EME, J2000, Constants.EIGEN5C_EARTH_MU);
    FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
    SpacecraftState iSR = initialState.toSpacecraftState();
    OrbitType type = OrbitType.KEPLERIAN;
    double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
    AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
    integrator.setInitialStepSize(zero.add(60));
    AdaptiveStepsizeIntegrator RIntegrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
    RIntegrator.setInitialStepSize(60);
    FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
    FNP.setOrbitType(type);
    FNP.setInitialState(initialState);
    NumericalPropagator NP = new NumericalPropagator(RIntegrator);
    NP.setOrbitType(type);
    NP.setInitialState(iSR);
    final ThirdBodyAttraction forceModel = new ThirdBodyAttraction(CelestialBodyFactory.getSun());
    FNP.addForceModel(forceModel);
    NP.addForceModel(forceModel);
    FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
    FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
    SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
    FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
    PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
    Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
    long number = 23091991;
    RandomGenerator RG = new Well19937a(number);
    GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
    UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, new double[] { 1e3, 0.01, 0.01, 0.01, 0.01, 0.01 }, NGG);
    double a_R = a_0.getReal();
    double e_R = e_0.getReal();
    double i_R = i_0.getReal();
    double R_R = R_0.getReal();
    double O_R = O_0.getReal();
    double n_R = n_0.getReal();
    double maxP = 0;
    double maxV = 0;
    double maxA = 0;
    for (int ii = 0; ii < 1; ii++) {
        double[] rand_next = URVG.nextVector();
        double a_shift = a_R + rand_next[0];
        double e_shift = e_R + rand_next[1];
        double i_shift = i_R + rand_next[2];
        double R_shift = R_R + rand_next[3];
        double O_shift = O_R + rand_next[4];
        double n_shift = n_R + rand_next[5];
        KeplerianOrbit shiftedOrb = new KeplerianOrbit(a_shift, e_shift, i_shift, R_shift, O_shift, n_shift, PositionAngle.MEAN, EME, J2000.toAbsoluteDate(), Constants.EIGEN5C_EARTH_MU);
        SpacecraftState shift_iSR = new SpacecraftState(shiftedOrb);
        NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator);
        shift_NP.setInitialState(shift_iSR);
        shift_NP.addForceModel(forceModel);
        SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate());
        PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
        // position check
        FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
        double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double x = finPVC_shift.getPosition().getX();
        double y = finPVC_shift.getPosition().getY();
        double z = finPVC_shift.getPosition().getZ();
        maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal())));
        maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal())));
        maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal())));
        // velocity check
        FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
        double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double vx = finPVC_shift.getVelocity().getX();
        double vy = finPVC_shift.getVelocity().getY();
        double vz = finPVC_shift.getVelocity().getZ();
        maxV = FastMath.max(maxV, FastMath.abs((vx_DS - vx) / vx));
        maxV = FastMath.max(maxV, FastMath.abs((vy_DS - vy) / vy));
        maxV = FastMath.max(maxV, FastMath.abs((vz_DS - vz) / vz));
        // acceleration check
        FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
        double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
        double ax = finPVC_shift.getAcceleration().getX();
        double ay = finPVC_shift.getAcceleration().getY();
        double az = finPVC_shift.getAcceleration().getZ();
        maxA = FastMath.max(maxA, FastMath.abs((ax_DS - ax) / ax));
        maxA = FastMath.max(maxA, FastMath.abs((ay_DS - ay) / ay));
        maxA = FastMath.max(maxA, FastMath.abs((az_DS - az) / az));
    }
    Assert.assertEquals(0, maxP, 5.0e-9);
    Assert.assertEquals(0, maxV, 3.0e-10);
    Assert.assertEquals(0, maxA, 8.0e-8);
}

Example 49 with Well19937a

use of org.hipparchus.random.Well19937a in project Orekit by CS-SI.

the class BiasTest method testEstimateBias.

@SuppressWarnings("unchecked")
@Test
public void testEstimateBias() throws OrekitException {
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    // create perfect range measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new RangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    // create range biases: one bias for each station
    final RandomGenerator random = new Well19937a(0x0c4b69da5d64b35al);
    final Bias<?>[] stationsRangeBiases = new Bias<?>[context.stations.size()];
    final double[] realStationsBiases = new double[context.stations.size()];
    for (int i = 0; i < context.stations.size(); ++i) {
        final TopocentricFrame base = context.stations.get(i).getBaseFrame();
        stationsRangeBiases[i] = new Bias<Range>(new String[] { base.getName() + " range bias" }, new double[] { 0.0 }, new double[] { 1.0 }, new double[] { Double.NEGATIVE_INFINITY }, new double[] { Double.POSITIVE_INFINITY });
        realStationsBiases[i] = 2 * random.nextDouble() - 1;
    }
    // create orbit estimator
    final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(), propagatorBuilder);
    // add the measurements, with both spacecraft and stations biases
    for (final ObservedMeasurement<?> measurement : measurements) {
        final Range range = (Range) measurement;
        for (int i = 0; i < context.stations.size(); ++i) {
            if (range.getStation() == context.stations.get(i)) {
                double biasedRange = range.getObservedValue()[0] + realStationsBiases[i];
                final Range m = new Range(range.getStation(), range.getDate(), biasedRange, range.getTheoreticalStandardDeviation()[0], range.getBaseWeight()[0]);
                m.addModifier((Bias<Range>) stationsRangeBiases[i]);
                estimator.addMeasurement(m);
            }
        }
    }
    estimator.setParametersConvergenceThreshold(1.0e-3);
    estimator.setMaxIterations(10);
    estimator.setMaxEvaluations(20);
    // we want to estimate the biases
    for (Bias<?> bias : stationsRangeBiases) {
        for (final ParameterDriver driver : bias.getParametersDrivers()) {
            driver.setSelected(true);
        }
    }
    EstimationTestUtils.checkFit(context, estimator, 2, 3, 0.0, 7.2e-7, 0.0, 2.1e-6, 0.0, 3.7e-7, 0.0, 1.7e-10);
    for (int i = 0; i < stationsRangeBiases.length; ++i) {
        Assert.assertEquals(realStationsBiases[i], stationsRangeBiases[i].getParametersDrivers().get(0).getValue(), 3.3e-6);
    }
}

Example 50 with Well19937a

use of org.hipparchus.random.Well19937a 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);
}