List of usage examples for org.apache.commons.math3.ode.sampling StepNormalizer StepNormalizer
public StepNormalizer(final double h, final FixedStepHandler handler)
From source file:org.orekit.utils.AngularCoordinatesTest.java
@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 FirstOrderDifferentialEquations ode = new FirstOrderDifferentialEquations() { public int getDimension() { return 4; }/* w w w . j a v a 2s. c o m*/ public void computeDerivatives(final double t, final double[] q, final double[] qDot) { 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(); qDot[0] = 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ); qDot[1] = 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ); qDot[2] = 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ); qDot[3] = 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ); } }; FirstOrderIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12); integrator.addStepHandler(new StepNormalizer(dt / n, new FixedStepHandler() { public void init(double t0, double[] y0, double t) { } public void handleStep(double t, double[] y, double[] yDot, boolean isLast) { 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, 0, y, dt, y); }
From source file:org.orekit.utils.TimeStampedAngularCoordinatesTest.java
private double[] interpolationErrors(final TimeStampedAngularCoordinates reference, double dt) throws OrekitException { final FirstOrderDifferentialEquations ode = new FirstOrderDifferentialEquations() { public int getDimension() { return 4; }// w w w.jav a 2s. c om public void computeDerivatives(final double t, final double[] q, final double[] qDot) { 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(); qDot[0] = 0.5 * MathArrays.linearCombination(-q[1], omegaX, -q[2], omegaY, -q[3], omegaZ); qDot[1] = 0.5 * MathArrays.linearCombination(q[0], omegaX, -q[3], omegaY, q[2], omegaZ); qDot[2] = 0.5 * MathArrays.linearCombination(q[3], omegaX, q[0], omegaY, -q[1], omegaZ); qDot[3] = 0.5 * MathArrays.linearCombination(-q[2], omegaX, q[1], omegaY, q[0], omegaZ); } }; final List<TimeStampedAngularCoordinates> complete = new ArrayList<TimeStampedAngularCoordinates>(); FirstOrderIntegrator integrator = new DormandPrince853Integrator(1.0e-6, 1.0, 1.0e-12, 1.0e-12); integrator.addStepHandler(new StepNormalizer(dt / 2000, new FixedStepHandler() { public void init(double t0, double[] y0, double t) { } public void handleStep(double t, double[] y, double[] yDot, boolean isLast) { complete.add( new TimeStampedAngularCoordinates(reference.getDate().shiftedBy(t), new Rotation(y[0], y[1], y[2], y[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, 0, y, dt, y); 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 }; }