Example usage for org.apache.commons.math3.ode FirstOrderIntegrator addStepHandler

List of usage examples for org.apache.commons.math3.ode FirstOrderIntegrator addStepHandler

  1. HOME
  2. Java
  3. org.apache.commons
  4. org.apache.commons.math3.ode.*
  5. FirstOrderIntegrator
  6. addStepHandler

Introduction

In this page you can find the example usage for org.apache.commons.math3.ode FirstOrderIntegrator addStepHandler.

Prototype

void addStepHandler(StepHandler handler);

Source Link

Document

Add a step handler to this integrator.

Usage

From source file:jat.examples.TwoBodyExample.TwoBodyExample.java

public static void main(String[] args) {

    TwoBodyExample x = new TwoBodyExample();

    // create a TwoBody orbit using orbit elements
    TwoBodyAPL sat = new TwoBodyAPL(7000.0, 0.3, 0.0, 0.0, 0.0, 0.0);

    double[] y = sat.randv();

    ArrayRealVector v = new ArrayRealVector(y);

    DecimalFormat df2 = new DecimalFormat("#,###,###,##0.00");
    RealVectorFormat format = new RealVectorFormat(df2);
    System.out.println(format.format(v));

    // find out the period of the orbit
    double period = sat.period();

    // set the final time = one orbit period
    double tf = period;

    // set the initial time to zero
    double t0 = 0.0;

    // propagate the orbit
    FirstOrderIntegrator dp853 = new DormandPrince853Integrator(1.0e-8, 100.0, 1.0e-10, 1.0e-10);
    dp853.addStepHandler(sat.stepHandler);
    // double[] y = new double[] { 7000.0, 0, 0, .0, 8, 0 }; // initial
    // state// w w  w.ja va 2 s  .  co m

    dp853.integrate(sat, 0.0, y, 8000, y); // now y contains final state at
    // tf

    Double[] objArray = sat.time.toArray(new Double[sat.time.size()]);
    double[] timeArray = ArrayUtils.toPrimitive(objArray);
    double[] xsolArray = ArrayUtils.toPrimitive(sat.xsol.toArray(new Double[sat.time.size()]));
    double[] ysolArray = ArrayUtils.toPrimitive(sat.ysol.toArray(new Double[sat.time.size()]));

    double[][] XY = new double[timeArray.length][2];

    // int a=0;
    // System.arraycopy(timeArray,0,XY[a],0,timeArray.length);
    // System.arraycopy(ysolArray,0,XY[1],0,ysolArray.length);

    for (int i = 0; i < timeArray.length; i++) {
        XY[i][0] = xsolArray[i];
        XY[i][1] = ysolArray[i];
    }

    Plot2DPanel p = new Plot2DPanel();

    // Plot2DPanel p = new Plot2DPanel(min, max, axesScales, axesLabels);

    ScatterPlot s = new ScatterPlot("orbit", Color.RED, XY);
    // LinePlot l = new LinePlot("sin", Color.RED, XY);
    // l.closed_curve = false;
    // l.draw_dot = true;
    p.addPlot(s);
    p.setLegendOrientation(PlotPanel.SOUTH);
    double plotSize = 10000.;
    double[] min = { -plotSize, -plotSize };
    double[] max = { plotSize, plotSize };
    p.setFixedBounds(min, max);

    new FrameView(p).setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

    System.out.println("end");

}

From source file:jat.examples.ephemeris.DE405PropagateText.java

void doExample() {
    //double tf = 10000000.;
    double tf = 3600 * 24 * 500;
    double[] y = { 2e8, 0, 0, 0, 24.2, 0 }; // initial state

    PathUtil path = new PathUtil();
    DE405Plus Eph = new DE405Plus(path);
    Eph.setFrame(DE405Frame.frame.HEE);
    Eph.printSteps = true;/*from w  ww  .j  a v  a  2s .c om*/
    TimeAPL myTime = new TimeAPL(2003, 3, 1, 12, 0, 0);
    Eph.setIntegrationStartTime(myTime);
    Eph.bodyGravOnOff[body.SUN.ordinal()] = true;
    FirstOrderIntegrator dp853 = new DormandPrince853Integrator(1.0e-8, tf / 10.0, 1.0e-10, 1.0e-10);
    dp853.addStepHandler(Eph.stepHandler);
    FirstOrderDifferentialEquations ode = Eph;

    dp853.integrate(ode, 0.0, y, tf, y); // now y contains final state
    // at
    // time tf
    if (print) {
        String nf = "%10.3f ";
        String format = nf + nf + nf + nf + nf;
        System.out.printf(format, tf, y[0], y[1], y[2], Eph.energy(tf, y));
        System.out.println();
    }
}

From source file:jat.examples.ephemeris.DE405PropagatePlot.java

void doExample() {
    double tf = 3600 * 24 * 300;
    double[] y0 = { 2e8, 0, 0, 0, 24.2, 0 }; // initial state
    double[] y = new double[6];

    PathUtil path = new PathUtil();
    DE405Plus Eph = new DE405Plus(path);
    Eph.setFrame(DE405Frame.frame.HEE);
    Eph.printSteps = true;//from  w  w w.ja  v  a  2  s  .  c  o  m
    TimeAPL myTime = new TimeAPL(2003, 3, 1, 12, 0, 0);
    Eph.setIntegrationStartTime(myTime);
    Eph.bodyGravOnOff[body.SUN.ordinal()] = true;
    // Eph.planetOnOff[body.JUPITER.ordinal()] = true;

    FirstOrderIntegrator dp853 = new DormandPrince853Integrator(1.0e-8, tf / 10.0, 1.0e-10, 1.0e-10);
    dp853.addStepHandler(Eph.stepHandler);
    FirstOrderDifferentialEquations ode = Eph;

    dp853.integrate(ode, 0.0, y0, tf, y); // now y contains final state at
    // time tf
    if (print) {
        String nf = "%10.3f ";
        String format = nf + nf + nf + nf + nf;
        System.out.printf(format, tf, y[0], y[1], y[2], Eph.energy(tf, y));
        System.out.println();
    }

    Plot2DPanel p = new Plot2DPanel();
    LinePlot l1 = new LinePlot("Jup. off", Color.RED, getXYforPlot(Eph.xsol, Eph.ysol));
    l1.closed_curve = false;
    p.addPlot(l1);

    Eph.reset();
    Eph.bodyGravOnOff[body.JUPITER.ordinal()] = true;
    dp853.integrate(ode, 0.0, y0, tf, y); // now y contains final state at

    LinePlot l2 = new LinePlot("Jup. on", Color.BLUE, getXYforPlot(Eph.xsol, Eph.ysol));
    l2.closed_curve = false;
    p.addPlot(l2);

    VectorN EarthPos = null;
    try {
        EarthPos = Eph.get_planet_pos(body.EARTH, myTime);
    } catch (IOException e) {
        // TODO Auto-generated catch block
        e.printStackTrace();
    }
    addPoint(p, "Earth", java.awt.Color.BLUE, EarthPos.x[0], EarthPos.x[1]);

    p.setLegendOrientation(PlotPanel.SOUTH);
    double plotSize = 2e8;
    p.setFixedBounds(0, -plotSize, plotSize);
    p.setFixedBounds(1, -plotSize, plotSize);
    new FrameView(p).setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

}

From source file:jat.examples.CRTBP.CRTBPPlot.java

void doExample() {
    double mu = 0.15;
    double[] y0 = { .11, 0, 0, 1.35, 1.33, 0 }; // initial state

    // double mu = 0.1;
    // double mu = 3.035909999e-6;

    // double mu = 0.012277471;
    // double[] y0 = { .1, 0, 0, 2.69, 2.69, 0 }; // initial state

    // double mu = 0.2;

    CRTBP myCRTBP = new CRTBP(mu);
    FirstOrderIntegrator dp853 = new DormandPrince853Integrator(1.0e-8, 100.0, 1.0e-10, 1.0e-10);
    dp853.addStepHandler(myCRTBP.stepHandler);

    FirstOrderDifferentialEquations ode = myCRTBP;

    double tf;//w ww  .ja v a  2 s.  co  m
    double[] y = new double[6]; // initial state

    // for (int i = 1; i < 2; i++) {
    // tf = i * 20.;
    tf = 40.;
    System.arraycopy(y0, 0, y, 0, 6);

    dp853.integrate(ode, 0.0, y, tf, y); // now y contains final state
    // at
    // time tf
    if (print) {
        System.out.printf("%9.6f %9.6f %9.6f %9.6f %9.6f", tf, y[0], y[1], y[2], myCRTBP.JacobiIntegral(y));
        System.out.println();
    }

    int arraySize = myCRTBP.time.size();
    double[] timeArray = ArrayUtils.toPrimitive(myCRTBP.time.toArray(new Double[arraySize]));
    double[] xsolArray = ArrayUtils.toPrimitive(myCRTBP.xsol.toArray(new Double[arraySize]));
    double[] ysolArray = ArrayUtils.toPrimitive(myCRTBP.ysol.toArray(new Double[arraySize]));
    double[][] XY = new double[timeArray.length][2];
    for (int i = 0; i < timeArray.length; i++) {
        XY[i][0] = xsolArray[i];
        XY[i][1] = ysolArray[i];
    }

    Plot2DPanel p = new Plot2DPanel();
    LinePlot l = new LinePlot("spacecraft", Color.RED, XY);
    l.closed_curve = false;
    l.draw_dot = true;
    p.addPlot(l);
    double plotSize = 1.2;
    myCRTBP.findLibrationPoints();
    Color darkGreen = new java.awt.Color(0, 190, 0);

    addPoint(p, "Earth", java.awt.Color.BLUE, -mu, 0);
    addPoint(p, "Moon", java.awt.Color.gray, 1 - mu, 0);
    addPoint(p, "L1", darkGreen, myCRTBP.LibPoints[0].getX(), 0);
    addPoint(p, "L2", darkGreen, myCRTBP.LibPoints[1].getX(), 0);
    addPoint(p, "L3", darkGreen, myCRTBP.LibPoints[2].getX(), 0);

    String Labelmu = "mu = " + myCRTBP.mu;
    p.addLabel(Labelmu, java.awt.Color.black, 1, .9 * plotSize);
    String initial = "initial x,v = (" + y0[0] + "," + y0[1] + "),(" + y0[3] + "," + y0[4] + ")";
    p.addLabel(initial, java.awt.Color.black, 1, .8 * plotSize);
    String Jacobi = "spacecraft C = " + myCRTBP.C;
    p.addLabel(Jacobi, java.awt.Color.black, 1, .7 * plotSize);
    String L1C = "L1 C = " + myCRTBP.C1;
    p.addLabel(L1C, java.awt.Color.black, 1, .6 * plotSize);

    myCRTBP.findZeroVelocity();
    int size = myCRTBP.xzv.size();
    double[] xzvArray = ArrayUtils.toPrimitive(myCRTBP.xzv.toArray(new Double[size]));
    double[] yzvArray = ArrayUtils.toPrimitive(myCRTBP.yzv.toArray(new Double[size]));
    double[][] XYzv = new double[size][2];
    for (int i = 0; i < size; i++) {
        XYzv[i][0] = xzvArray[i];
        XYzv[i][1] = yzvArray[i];
    }
    LinePlot lzv = new LinePlot("zero vel", Color.blue, XYzv);
    lzv.closed_curve = false;
    lzv.draw_dot = true;
    p.addPlot(lzv);

    p.setLegendOrientation(PlotPanel.SOUTH);
    p.setFixedBounds(0, -plotSize, plotSize);
    p.setFixedBounds(1, -plotSize, plotSize);
    new FrameView(p).setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

}

From source file:jat.application.DE405Propagator.DE405PropagatorPlot.java

public void add_scene() {

    // Update Ephemeris to current user parameters
    for (body b : body.values()) {
        Eph.bodyGravOnOff[b.ordinal()] = dpMain.dpParam.bodyGravOnOff[b.ordinal()];
    }/*  w w w  .jav a 2 s.  c om*/
    Eph.setIntegrationStartTime(dpMain.dpParam.simulationDate);
    try {
        Eph.setEarthMoonPlaneNormal(dpMain.dpParam.simulationDate);
    } catch (IOException e1) {
        e1.printStackTrace();
    }
    Eph.setFrame(dpMain.dpParam.Frame);
    Eph.reset();

    // Spacecraft Trajectory
    FirstOrderIntegrator dp853 = new DormandPrince853Integrator(1.0e-8, dpMain.dpParam.tf, 1.0e-10, 1.0e-10);
    dp853.addStepHandler(Eph.stepHandler);
    FirstOrderDifferentialEquations ode = Eph;
    double[] y = new double[6];
    dp853.integrate(ode, 0.0, dpMain.dpParam.y0, dpMain.dpParam.tf, y);
    if (print) {
        String nf = "%10.3f ";
        String format = nf + nf + nf + nf + nf;
        System.out.printf(format, dpMain.dpParam.tf, y[0], y[1], y[2], Eph.energy(dpMain.dpParam.tf, y));
        System.out.println();
    }

    LinePlot l1 = new LinePlot("spacecraft", Color.RED, getXYZforPlot(Eph.xsol, Eph.ysol, Eph.zsol));
    l1.closed_curve = false;
    plot.addPlot(l1);

    addBodies();

    // Vector3D y0v=new Vector3D(dpParam.y0[0],dpParam.y0[1],dpParam.y0[2]);
    // double plotBounds = 2*y0v.getNorm();
    plot.setFixedBounds(0, -plotBounds, plotBounds);
    plot.setFixedBounds(1, -plotBounds, plotBounds);
    plot.setFixedBounds(2, -plotBounds, plotBounds);
    plot.setLegendOrientation(PlotPanel.SOUTH);
}

From source file:edu.gcsc.vrl.commons.math.ode.ODESolver.java

public Trajectory solve(@ParamInfo(name = "Label", options = "value=\"Label 1\"") String label,
        @ParamInfo(name = "x0", options = "value=0.0D") double t0,
        @ParamInfo(name = "xn", options = "value=3.0D") double tn,
        @ParamInfo(name = "y0", options = "value=0.0") double y0,
        @ParamInfo(name = "Min Step", options = "value=1e-6D") double minStep,
        @ParamInfo(name = "Max Step", options = "value=1e-2D") double maxStep,
        @ParamInfo(name = "Abs.Tol.", options = "value=1e-10") double absTol,
        @ParamInfo(name = "Rel.Tol.", options = "value=1e-10") double relTol,
        @ParamInfo(name = "RHS") FirstOrderDifferentialEquations rhs) {

    FirstOrderIntegrator integrator = new DormandPrince853Integrator(minStep, maxStep, absTol, relTol);

    final Trajectory result = new Trajectory(label);

    StepHandler stepHandler = new StepHandler() {
        @Override/* w w  w . jav a2  s. c  o m*/
        public void init(double t0, double[] y0, double t) {
            result.add(t, y0[0]);
        }

        @Override
        public void handleStep(StepInterpolator interpolator, boolean isLast) {
            double t = interpolator.getCurrentTime();
            double[] y = interpolator.getInterpolatedState();
            result.add(t, y[0]);
        }
    };

    integrator.addStepHandler(stepHandler);

    double[] y = new double[] { y0 }; // initial state
    integrator.integrate(rhs, t0, y, tn, y);

    return result;
}

From source file:de.uni_erlangen.lstm.modelaccess.Model.java

/**
 * Run the model using set parameters/*  ww  w  . j  av  a2  s . c om*/
 */
public void simulate() {
    finished = false;
    /*
     * Integrator selection 
     */
    //FirstOrderIntegrator integrator = new HighamHall54Integrator(1.0e-8, 100.0, 1.0e-10, 1.0e-10);
    //FirstOrderIntegrator integrator = new DormandPrince54Integrator(1.0e-12, 100.0, 1.0e-12, 1.0e-12);
    //FirstOrderIntegrator integrator = new DormandPrince853Integrator(1.0e-8, 100.0, 1.0e-10, 1.0e-10);
    //FirstOrderIntegrator integrator = new GraggBulirschStoerIntegrator(1.0e-8, 100.0, 1.0e-10, 1.0e-10);
    FirstOrderIntegrator integrator = new AdamsBashforthIntegrator(2, 1.0e-14, 100.0, 1.0e-10, 1.0e-10);
    //FirstOrderIntegrator integrator = new AdamsMoultonIntegrator(2, 1.0e-8, 100.0, 1.0e-10, 1.0e-10);

    // influent values, digester parameters, S_H_ion, dae system
    final DAEModel ode = new DAEModel(u, param, S_H_ion, dae, fix_pH);
    //FirstOrderDifferentialEquations ode = model; 

    // Records progress
    StepHandler progHandler = new StepHandler() {
        public void init(double t0, double[] y0, double t) {
        }

        public void handleStep(StepInterpolator interpolator, boolean isLast) {
            progress = interpolator.getCurrentTime();
        }
    };
    integrator.addStepHandler(progHandler);

    /*
     * Continuous model recorded in CSV
     */
    if (onlineRecord) {
        final CSVWriter writer = new CSVWriter();
        StepHandler stepHandler = new StepHandler() {
            double prevT = 0.0;

            public void init(double t0, double[] y0, double t) {
            }

            public void handleStep(StepInterpolator interpolator, boolean isLast) {
                double t = interpolator.getCurrentTime();
                if (t - prevT > resolution) {
                    // Add time to the beginning of the array
                    double[] timemodel = new double[ode.getDimensions().length + 1];
                    timemodel[0] = t;

                    // We need to pull variables (S_h2 and acid-base) directly from the model if using DAE
                    for (int i = 1; i < timemodel.length; i++) {
                        timemodel[i] = ode.getDimensions()[i - 1];
                    }

                    writer.WriteArray(output_file, timemodel, true);
                    prevT = t;
                }
            }
        };
        integrator.addStepHandler(stepHandler);
    }

    /*
     * Add event handlers for discrete events
     * maxCheck - maximal time interval between switching function checks (this interval prevents missing sign changes in case the integration steps becomes very large)
      * conv - convergence threshold in the event time search
      * maxIt - upper limit of the iteration count in the event time search
     */
    if (events.size() > 0) {
        for (DiscreteEvent event : events) {
            double maxCheck = Double.POSITIVE_INFINITY;
            double conv = 1.0e-20;
            int maxIt = 100;
            integrator.addEventHandler(event, maxCheck, conv, maxIt);
        }
    }

    integrator.integrate(ode, start, x, end, x);

    /*
     * Return the time that the discrete event occurred
     */
    if (events.size() > 0) {
        for (DiscreteEvent event : events) {
            if (event.getTime() < end) {
                end = event.getTime();
            }
        }
    }

    // We need to pull variables (S_h2 and acid-base) directly from the model
    x = ode.getDimensions();

    finished = true;
}

From source file:nl.rivm.cib.episim.model.disease.infection.MSEIRSTest.java

public static Observable<Map.Entry<Double, double[]>> deterministic(final SIRConfig config,
        final Supplier<FirstOrderIntegrator> integrators) {
    return Observable.create(sub -> {
        final double gamma = 1. / config.recovery();
        final double beta = gamma * config.reproduction();
        final double[] y0 = Arrays.stream(config.population()).mapToDouble(n -> n).toArray();
        final double[] t = config.t();

        try {//ww  w.  j a  v a 2  s  .  c  o  m
            final FirstOrderIntegrator integrator = integrators.get();

            integrator.addStepHandler(new StepHandler() {
                @Override
                public void init(final double t0, final double[] y0, final double t) {
                    publishCopy(sub, t0, y0);
                }

                @Override
                public void handleStep(final StepInterpolator interpolator, final boolean isLast)
                        throws MaxCountExceededException {
                    publishCopy(sub, interpolator.getInterpolatedTime(), interpolator.getInterpolatedState());
                    if (isLast)
                        sub.onComplete();
                }
            });

            integrator.integrate(new FirstOrderDifferentialEquations() {
                @Override
                public int getDimension() {
                    return y0.length;
                }

                @Override
                public void computeDerivatives(final double t, final double[] y, final double[] yp) {
                    // SIR terms (flow rates)
                    final double n = y[0] + y[1] + y[2], flow_si = beta * y[0] * y[1] / n,
                            flow_ir = gamma * y[1];

                    yp[0] = -flow_si;
                    yp[1] = flow_si - flow_ir;
                    yp[2] = flow_ir;
                }
            }, t[0], y0, t[1], y0);
        } catch (final Exception e) {
            sub.onError(e);
        }
    });
}

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;
        }/*from  w ww  .j a  v  a  2 s  .co 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;
        }//from ww  w .jav a2 s.  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 };

}