Examples with StepNormalizer org.hipparchus.ode.sampling.StepNormalizer used on opensource projects

Example 1 with StepNormalizer

use of org.hipparchus.ode.sampling.StepNormalizer in project Orekit by CS-SI.

the class TimeStampedAngularCoordinatesTest method interpolationErrors.

private double[] interpolationErrors(final TimeStampedAngularCoordinates reference, double dt) throws OrekitException {
    final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {

        public int getDimension() {
            return 4;
        }

        public double[] computeDerivatives(final double t, final double[] q) {
            final double omegaX = reference.getRotationRate().getX() + t * reference.getRotationAcceleration().getX();
            final double omegaY = reference.getRotationRate().getY() + t * reference.getRotationAcceleration().getY();
            final double omegaZ = reference.getRotationRate().getZ() + t * reference.getRotationAcceleration().getZ();
            return new double[] { 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ), 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ), 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ), 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ) };
        }
    };
    final List<TimeStampedAngularCoordinates> complete = new ArrayList<TimeStampedAngularCoordinates>();
    ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
    integrator.addStepHandler(new StepNormalizer(dt / 2000, new ODEFixedStepHandler() {

        public void handleStep(ODEStateAndDerivative state, boolean isLast) {
            final double t = state.getTime();
            final double[] q = state.getPrimaryState();
            complete.add(new TimeStampedAngularCoordinates(reference.getDate().shiftedBy(t), new Rotation(q[0], q[1], q[2], q[3], true), new Vector3D(1, reference.getRotationRate(), t, reference.getRotationAcceleration()), reference.getRotationAcceleration()));
        }
    }));
    double[] y = new double[] { reference.getRotation().getQ0(), reference.getRotation().getQ1(), reference.getRotation().getQ2(), reference.getRotation().getQ3() };
    integrator.integrate(ode, new ODEState(0, y), dt);
    List<TimeStampedAngularCoordinates> sample = new ArrayList<TimeStampedAngularCoordinates>();
    sample.add(complete.get(0));
    sample.add(complete.get(complete.size() / 2));
    sample.add(complete.get(complete.size() - 1));
    double maxRotationError = 0;
    double maxRateError = 0;
    double maxAccelerationError = 0;
    for (TimeStampedAngularCoordinates acRef : complete) {
        TimeStampedAngularCoordinates interpolated = TimeStampedAngularCoordinates.interpolate(acRef.getDate(), AngularDerivativesFilter.USE_RRA, sample);
        double rotationError = Rotation.distance(acRef.getRotation(), interpolated.getRotation());
        double rateError = Vector3D.distance(acRef.getRotationRate(), interpolated.getRotationRate());
        double accelerationError = Vector3D.distance(acRef.getRotationAcceleration(), interpolated.getRotationAcceleration());
        maxRotationError = FastMath.max(maxRotationError, rotationError);
        maxRateError = FastMath.max(maxRateError, rateError);
        maxAccelerationError = FastMath.max(maxAccelerationError, accelerationError);
    }
    return new double[] { maxRotationError, maxRateError, maxAccelerationError };
}

Example 2 with StepNormalizer

use of org.hipparchus.ode.sampling.StepNormalizer in project Orekit by CS-SI.

the class AngularCoordinatesTest method testShiftWithAcceleration.

@Test
public void testShiftWithAcceleration() throws OrekitException {
    double rate = 2 * FastMath.PI / (12 * 60);
    double acc = 0.001;
    double dt = 1.0;
    int n = 2000;
    final AngularCoordinates quadratic = new AngularCoordinates(Rotation.IDENTITY, new Vector3D(rate, Vector3D.PLUS_K), new Vector3D(acc, Vector3D.PLUS_J));
    final AngularCoordinates linear = new AngularCoordinates(quadratic.getRotation(), quadratic.getRotationRate(), Vector3D.ZERO);
    final OrdinaryDifferentialEquation ode = new OrdinaryDifferentialEquation() {

        public int getDimension() {
            return 4;
        }

        public double[] computeDerivatives(final double t, final double[] q) {
            final double omegaX = quadratic.getRotationRate().getX() + t * quadratic.getRotationAcceleration().getX();
            final double omegaY = quadratic.getRotationRate().getY() + t * quadratic.getRotationAcceleration().getY();
            final double omegaZ = quadratic.getRotationRate().getZ() + t * quadratic.getRotationAcceleration().getZ();
            return new double[] { 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ), 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ), 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ), 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ) };
        }
    };
    ODEIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12);
    integrator.addStepHandler(new StepNormalizer(dt / n, new ODEFixedStepHandler() {

        public void handleStep(ODEStateAndDerivative s, boolean isLast) {
            final double t = s.getTime();
            final double[] y = s.getPrimaryState();
            Rotation reference = new Rotation(y[0], y[1], y[2], y[3], true);
            // the error in shiftedBy taking acceleration into account is cubic
            double expectedCubicError = 1.4544e-6 * t * t * t;
            Assert.assertEquals(expectedCubicError, Rotation.distance(reference, quadratic.shiftedBy(t).getRotation()), 0.0001 * expectedCubicError);
            // the error in shiftedBy not taking acceleration into account is quadratic
            double expectedQuadraticError = 5.0e-4 * t * t;
            Assert.assertEquals(expectedQuadraticError, Rotation.distance(reference, linear.shiftedBy(t).getRotation()), 0.00001 * expectedQuadraticError);
        }
    }));
    double[] y = new double[] { quadratic.getRotation().getQ0(), quadratic.getRotation().getQ1(), quadratic.getRotation().getQ2(), quadratic.getRotation().getQ3() };
    integrator.integrate(ode, new ODEState(0, y), dt);
}