Examples with FiniteDifferencesDifferentiator org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator used on opensource projects

Example 21 with FiniteDifferencesDifferentiator

use of org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator in project Orekit by CS-SI.

the class FramesFactoryTest method doTestDerivatives.

private void doTestDerivatives(AbsoluteDate ref, double duration, double step, boolean forbidInterpolation, double cartesianTolerance, double cartesianDotTolerance, double cartesianDotDotTolerance, double rodriguesTolerance, double rodriguesDotTolerance, double rodriguesDotDotTolerance) throws OrekitException {
    final DSFactory factory = new DSFactory(1, 2);
    final FieldAbsoluteDate<DerivativeStructure> refDS = new FieldAbsoluteDate<>(factory.getDerivativeField(), ref);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 60.0);
    for (final Predefined predefined : Predefined.values()) {
        final Frame frame = FramesFactory.getFrame(predefined);
        final Frame parent = frame.getParent();
        if (parent != null) {
            UnivariateDifferentiableVectorFunction dCartesian = differentiator.differentiate(new UnivariateVectorFunction() {

                @Override
                public double[] value(double t) {
                    try {
                        return forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getTranslation().toArray() : parent.getTransformTo(frame, ref.shiftedBy(t)).getTranslation().toArray();
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            UnivariateDifferentiableVectorFunction dOrientation = differentiator.differentiate(new UnivariateVectorFunction() {

                double sign = +1.0;

                Rotation previous = Rotation.IDENTITY;

                @Override
                public double[] value(double t) {
                    try {
                        AngularCoordinates ac = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, ref.shiftedBy(t)).getAngular() : parent.getTransformTo(frame, ref.shiftedBy(t)).getAngular();
                        final double dot = MathArrays.linearCombination(ac.getRotation().getQ0(), previous.getQ0(), ac.getRotation().getQ1(), previous.getQ1(), ac.getRotation().getQ2(), previous.getQ2(), ac.getRotation().getQ3(), previous.getQ3());
                        sign = FastMath.copySign(1.0, dot * sign);
                        previous = ac.getRotation();
                        return ac.getModifiedRodrigues(sign)[0];
                    } catch (OrekitException oe) {
                        throw new OrekitExceptionWrapper(oe);
                    }
                }
            });
            double maxCartesianError = 0;
            double maxCartesianDotError = 0;
            double maxCartesianDotDotError = 0;
            double maxRodriguesError = 0;
            double maxRodriguesDotError = 0;
            double maxRodriguesDotDotError = 0;
            for (double dt = 0; dt < duration; dt += step) {
                final DerivativeStructure dtDS = factory.variable(0, dt);
                final FieldTransform<DerivativeStructure> tDS = forbidInterpolation ? FramesFactory.getNonInterpolatingTransform(parent, frame, refDS.shiftedBy(dtDS)) : parent.getTransformTo(frame, refDS.shiftedBy(dtDS));
                final DerivativeStructure[] refCart = dCartesian.value(dtDS);
                final DerivativeStructure[] fieldCart = tDS.getTranslation().toArray();
                for (int i = 0; i < 3; ++i) {
                    maxCartesianError = FastMath.max(maxCartesianError, FastMath.abs(refCart[i].getValue() - fieldCart[i].getValue()));
                    maxCartesianDotError = FastMath.max(maxCartesianDotError, FastMath.abs(refCart[i].getPartialDerivative(1) - fieldCart[i].getPartialDerivative(1)));
                    maxCartesianDotDotError = FastMath.max(maxCartesianDotDotError, FastMath.abs(refCart[i].getPartialDerivative(2) - fieldCart[i].getPartialDerivative(2)));
                }
                final DerivativeStructure[] refOr = dOrientation.value(dtDS);
                DerivativeStructure[] fieldOr = tDS.getAngular().getModifiedRodrigues(1.0)[0];
                final double dot = refOr[0].linearCombination(refOr, fieldOr).getReal();
                if (dot < 0 || Double.isNaN(dot)) {
                    fieldOr = tDS.getAngular().getModifiedRodrigues(-1.0)[0];
                }
                for (int i = 0; i < 3; ++i) {
                    maxRodriguesError = FastMath.max(maxRodriguesError, FastMath.abs(refOr[i].getValue() - fieldOr[i].getValue()));
                    maxRodriguesDotError = FastMath.max(maxRodriguesDotError, FastMath.abs(refOr[i].getPartialDerivative(1) - fieldOr[i].getPartialDerivative(1)));
                    maxRodriguesDotDotError = FastMath.max(maxRodriguesDotDotError, FastMath.abs(refOr[i].getPartialDerivative(2) - fieldOr[i].getPartialDerivative(2)));
                }
            }
            Assert.assertEquals(frame.getName(), 0, maxCartesianError, cartesianTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotError, cartesianDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxCartesianDotDotError, cartesianDotDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesError, rodriguesTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotError, rodriguesDotTolerance);
            Assert.assertEquals(frame.getName(), 0, maxRodriguesDotDotError, rodriguesDotDotTolerance);
        }
    }
}

Example 22 with FiniteDifferencesDifferentiator

use of org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator in project Orekit by CS-SI.

the class EquinoctialOrbitTest method differentiate.

private <S extends Function<EquinoctialOrbit, Double>> double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new EquinoctialOrbit(pv.shiftedBy(dt), frame, mu));
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 23 with FiniteDifferencesDifferentiator

use of org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator in project Orekit by CS-SI.

the class FieldCartesianOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldCartesianOrbit<T>, T>> double differentiate(TimeStampedFieldPVCoordinates<T> pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new FieldCartesianOrbit<>(pv.shiftedBy(dt), frame, mu)).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 24 with FiniteDifferencesDifferentiator

use of org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator in project Orekit by CS-SI.

the class FieldKeplerianOrbitTest method differentiate.

private <T extends RealFieldElement<T>, S extends Function<FieldKeplerianOrbit<T>, T>> double differentiate(FieldKeplerianOrbit<T> orbit, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(orbit.shiftedBy(orbit.getDate().getField().getZero().add(dt))).getReal();
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}

Example 25 with FiniteDifferencesDifferentiator

use of org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator in project Orekit by CS-SI.

the class KeplerianOrbitTest method differentiate.

private <S extends Function<KeplerianOrbit, Double>> double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
    final DSFactory factory = new DSFactory(1, 1);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
    UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {

        public double value(double dt) {
            return picker.apply(new KeplerianOrbit(pv.shiftedBy(dt), frame, mu));
        }
    });
    return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
}