Examples with OrekitExceptionWrapper org.orekit.errors.OrekitExceptionWrapper used on opensource projects

Example 26 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper in project Orekit by CS-SI.

the class Differentiation method differentiate.

/**
 * Differentiate a vector function using finite differences.
 * @param function function to differentiate
 * @param provider attitude provider to use for modified states
 * @param dimension dimension of the vector value of the function
 * @param orbitType type used to map the orbit to a one dimensional array
 * @param positionAngle type of the position angle used for orbit mapping to array
 * @param dP user specified position error, used for step size computation for finite differences
 * @param nbPoints number of points used for finite differences
 * @return matrix function evaluating to the Jacobian of the original function
 */
public static StateJacobian differentiate(final StateFunction function, final int dimension, final AttitudeProvider provider, final OrbitType orbitType, final PositionAngle positionAngle, final double dP, final int nbPoints) {
    return new StateJacobian() {

        @Override
        public double[][] value(final SpacecraftState state) throws OrekitException {
            try {
                final double[] tolerances = NumericalPropagator.tolerances(dP, state.getOrbit(), orbitType)[0];
                final double[][] jacobian = new double[dimension][6];
                for (int j = 0; j < 6; ++j) {
                    // compute partial derivatives with respect to state component j
                    final UnivariateVectorFunction componentJ = new StateComponentFunction(j, function, provider, state, orbitType, positionAngle);
                    final FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(nbPoints, tolerances[j]);
                    final UnivariateDifferentiableVectorFunction differentiatedJ = differentiator.differentiate(componentJ);
                    final DerivativeStructure[] c = differentiatedJ.value(FACTORY.variable(0, 0.0));
                    // populate the j-th column of the Jacobian
                    for (int i = 0; i < dimension; ++i) {
                        jacobian[i][j] = c[i].getPartialDerivative(1);
                    }
                }
                return jacobian;
            } catch (OrekitExceptionWrapper oew) {
                throw oew.getException();
            }
        }
    };
}

Example 27 with OrekitExceptionWrapper

use of org.orekit.errors.OrekitExceptionWrapper 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);
        }
    }
}