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

Example 46 with DSFactory

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

the class DragForceTest method accelerationDerivatives.

@Override
protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel, final AbsoluteDate date, final Frame frame, final FieldVector3D<DerivativeStructure> position, final FieldVector3D<DerivativeStructure> velocity, final FieldRotation<DerivativeStructure> rotation, final DerivativeStructure mass) throws OrekitException {
    try {
        java.lang.reflect.Field atmosphereField = DragForce.class.getDeclaredField("atmosphere");
        atmosphereField.setAccessible(true);
        Atmosphere atmosphere = (Atmosphere) atmosphereField.get(forceModel);
        java.lang.reflect.Field spacecraftField = DragForce.class.getDeclaredField("spacecraft");
        spacecraftField.setAccessible(true);
        DragSensitive spacecraft = (DragSensitive) spacecraftField.get(forceModel);
        // retrieve derivation properties
        final DSFactory factory = mass.getFactory();
        // get atmosphere properties in atmosphere own frame
        final Frame atmFrame = atmosphere.getFrame();
        final Transform toBody = frame.getTransformTo(atmFrame, date);
        final FieldVector3D<DerivativeStructure> posBodyDS = toBody.transformPosition(position);
        final Vector3D posBody = posBodyDS.toVector3D();
        final Vector3D vAtmBody = atmosphere.getVelocity(date, posBody, atmFrame);
        // to the Atmosphere interface
        if (factory.getCompiler().getOrder() > 1) {
            throw new OrekitException(OrekitMessages.OUT_OF_RANGE_DERIVATION_ORDER, factory.getCompiler().getOrder());
        }
        final double delta = 1.0;
        final double x = posBody.getX();
        final double y = posBody.getY();
        final double z = posBody.getZ();
        final double rho0 = atmosphere.getDensity(date, posBody, atmFrame);
        final double dRhodX = (atmosphere.getDensity(date, new Vector3D(x + delta, y, z), atmFrame) - rho0) / delta;
        final double dRhodY = (atmosphere.getDensity(date, new Vector3D(x, y + delta, z), atmFrame) - rho0) / delta;
        final double dRhodZ = (atmosphere.getDensity(date, new Vector3D(x, y, z + delta), atmFrame) - rho0) / delta;
        final double[] dXdQ = posBodyDS.getX().getAllDerivatives();
        final double[] dYdQ = posBodyDS.getY().getAllDerivatives();
        final double[] dZdQ = posBodyDS.getZ().getAllDerivatives();
        final double[] rhoAll = new double[dXdQ.length];
        rhoAll[0] = rho0;
        for (int i = 1; i < rhoAll.length; ++i) {
            rhoAll[i] = dRhodX * dXdQ[i] + dRhodY * dYdQ[i] + dRhodZ * dZdQ[i];
        }
        final DerivativeStructure rho = factory.build(rhoAll);
        // we consider that at first order the atmosphere velocity in atmosphere frame
        // does not depend on local position; however atmosphere velocity in inertial
        // frame DOES depend on position since the transform between the frames depends
        // on it, due to central body rotation rate and velocity composition.
        // So we use the transform to get the correct partial derivatives on vAtm
        final FieldVector3D<DerivativeStructure> vAtmBodyDS = new FieldVector3D<>(factory.constant(vAtmBody.getX()), factory.constant(vAtmBody.getY()), factory.constant(vAtmBody.getZ()));
        final FieldPVCoordinates<DerivativeStructure> pvAtmBody = new FieldPVCoordinates<>(posBodyDS, vAtmBodyDS);
        final FieldPVCoordinates<DerivativeStructure> pvAtm = toBody.getInverse().transformPVCoordinates(pvAtmBody);
        // now we can compute relative velocity, it takes into account partial derivatives with respect to position
        final FieldVector3D<DerivativeStructure> relativeVelocity = pvAtm.getVelocity().subtract(velocity);
        // compute acceleration with all its partial derivatives
        return spacecraft.dragAcceleration(new FieldAbsoluteDate<>(factory.getDerivativeField(), date), frame, position, rotation, mass, rho, relativeVelocity, forceModel.getParameters(factory.getDerivativeField()));
    } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
        return null;
    }
}

Example 47 with DSFactory

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

the class JB2008Test method testDensityGradient.

@Test
public void testDensityGradient() throws OrekitException {
    final Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
    final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, itrf);
    final JB2008 atm = new JB2008(new InputParams(), CelestialBodyFactory.getSun(), earth);
    final AbsoluteDate date = InputParams.TC[6];
    // Build the position
    final double alt = 400.;
    final double lat = 60.;
    final double lon = -70.;
    final GeodeticPoint point = new GeodeticPoint(FastMath.toRadians(lat), FastMath.toRadians(lon), alt * 1000.);
    final Vector3D pos = earth.transform(point);
    // Run
    DerivativeStructure zero = new DSFactory(1, 1).variable(0, 0.0);
    FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(5, 10.0);
    DerivativeStructure rhoX = differentiator.differentiate((double x) -> {
        try {
            return atm.getDensity(date, new Vector3D(1, pos, x, Vector3D.PLUS_I), itrf);
        } catch (OrekitException oe) {
            return Double.NaN;
        }
    }).value(zero);
    DerivativeStructure rhoY = differentiator.differentiate((double y) -> {
        try {
            return atm.getDensity(date, new Vector3D(1, pos, y, Vector3D.PLUS_J), itrf);
        } catch (OrekitException oe) {
            return Double.NaN;
        }
    }).value(zero);
    DerivativeStructure rhoZ = differentiator.differentiate((double z) -> {
        try {
            return atm.getDensity(date, new Vector3D(1, pos, z, Vector3D.PLUS_K), itrf);
        } catch (OrekitException oe) {
            return Double.NaN;
        }
    }).value(zero);
    DSFactory factory3 = new DSFactory(3, 1);
    Field<DerivativeStructure> field = factory3.getDerivativeField();
    final DerivativeStructure rhoDS = atm.getDensity(new FieldAbsoluteDate<>(field, date), new FieldVector3D<>(factory3.variable(0, pos.getX()), factory3.variable(1, pos.getY()), factory3.variable(2, pos.getZ())), itrf);
    Assert.assertEquals(rhoX.getValue(), rhoDS.getReal(), rhoX.getValue() * 2.0e-14);
    Assert.assertEquals(rhoY.getValue(), rhoDS.getReal(), rhoY.getValue() * 2.0e-14);
    Assert.assertEquals(rhoZ.getValue(), rhoDS.getReal(), rhoZ.getValue() * 2.0e-14);
    Assert.assertEquals(rhoX.getPartialDerivative(1), rhoDS.getPartialDerivative(1, 0, 0), FastMath.abs(6.0e-10 * rhoX.getPartialDerivative(1)));
    Assert.assertEquals(rhoY.getPartialDerivative(1), rhoDS.getPartialDerivative(0, 1, 0), FastMath.abs(6.0e-10 * rhoY.getPartialDerivative(1)));
    Assert.assertEquals(rhoZ.getPartialDerivative(1), rhoDS.getPartialDerivative(0, 0, 1), FastMath.abs(6.0e-10 * rhoY.getPartialDerivative(1)));
}

Example 48 with DSFactory

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

the class AbstractForceModelTest method checkParameterDerivative.

protected void checkParameterDerivative(SpacecraftState state, ForceModel forceModel, String name, double hFactor, double tol) throws OrekitException {
    final DSFactory factory11 = new DSFactory(1, 1);
    final Field<DerivativeStructure> field = factory11.getDerivativeField();
    final FieldSpacecraftState<DerivativeStructure> stateF = new FieldSpacecraftState<DerivativeStructure>(field, state);
    final ParameterDriver[] drivers = forceModel.getParametersDrivers();
    final DerivativeStructure[] parametersDS = new DerivativeStructure[drivers.length];
    for (int i = 0; i < parametersDS.length; ++i) {
        if (drivers[i].getName().equals(name)) {
            parametersDS[i] = factory11.variable(0, drivers[i].getValue());
        } else {
            parametersDS[i] = factory11.constant(drivers[i].getValue());
        }
    }
    FieldVector3D<DerivativeStructure> accDer = forceModel.acceleration(stateF, parametersDS);
    Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1), accDer.getY().getPartialDerivative(1), accDer.getZ().getPartialDerivative(1));
    int selected = -1;
    final double[] parameters = new double[drivers.length];
    for (int i = 0; i < drivers.length; ++i) {
        parameters[i] = drivers[i].getValue();
        if (drivers[i].getName().equals(name)) {
            selected = i;
        }
    }
    double p0 = parameters[selected];
    double hParam = hFactor * p0;
    drivers[selected].setValue(p0 - 1 * hParam);
    parameters[selected] = drivers[selected].getValue();
    Assert.assertEquals(p0 - 1 * hParam, parameters[selected], 1.0e-10);
    final Vector3D gammaM1h = forceModel.acceleration(state, parameters);
    drivers[selected].setValue(p0 + 1 * hParam);
    parameters[selected] = drivers[selected].getValue();
    Assert.assertEquals(p0 + 1 * hParam, parameters[selected], 1.0e-10);
    final Vector3D gammaP1h = forceModel.acceleration(state, parameters);
    drivers[selected].setValue(p0);
    final Vector3D reference = new Vector3D(1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
    final Vector3D delta = derivative.subtract(reference);
    Assert.assertEquals(0, delta.getNorm(), tol * reference.getNorm());
}

Example 49 with DSFactory

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

the class AbstractForceModelTest method checkStateJacobianVsFiniteDifferences.

protected void checkStateJacobianVsFiniteDifferences(final SpacecraftState state0, final ForceModel forceModel, final AttitudeProvider provider, final double dP, final double checkTolerance, final boolean print) throws OrekitException {
    double[][] finiteDifferencesJacobian = Differentiation.differentiate(state -> forceModel.acceleration(state, forceModel.getParameters()).toArray(), 3, provider, OrbitType.CARTESIAN, PositionAngle.MEAN, dP, 5).value(state0);
    DSFactory factory = new DSFactory(6, 1);
    Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state0.getDate());
    final Vector3D p = state0.getPVCoordinates().getPosition();
    final Vector3D v = state0.getPVCoordinates().getVelocity();
    final Vector3D a = state0.getPVCoordinates().getAcceleration();
    final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA = new TimeStampedFieldPVCoordinates<>(fDate, new FieldVector3D<>(factory.variable(0, p.getX()), factory.variable(1, p.getY()), factory.variable(2, p.getZ())), new FieldVector3D<>(factory.variable(3, v.getX()), factory.variable(4, v.getY()), factory.variable(5, v.getZ())), new FieldVector3D<>(factory.constant(a.getX()), factory.constant(a.getY()), factory.constant(a.getZ())));
    final TimeStampedFieldAngularCoordinates<DerivativeStructure> fAC = new TimeStampedFieldAngularCoordinates<>(fDate, new FieldRotation<>(field, state0.getAttitude().getRotation()), new FieldVector3D<>(field, state0.getAttitude().getSpin()), new FieldVector3D<>(field, state0.getAttitude().getRotationAcceleration()));
    final FieldSpacecraftState<DerivativeStructure> fState = new FieldSpacecraftState<>(new FieldCartesianOrbit<>(fPVA, state0.getFrame(), state0.getMu()), new FieldAttitude<>(state0.getFrame(), fAC), field.getZero().add(state0.getMass()));
    FieldVector3D<DerivativeStructure> dsJacobian = forceModel.acceleration(fState, forceModel.getParameters(fState.getDate().getField()));
    Vector3D dFdPXRef = new Vector3D(finiteDifferencesJacobian[0][0], finiteDifferencesJacobian[1][0], finiteDifferencesJacobian[2][0]);
    Vector3D dFdPXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(1, 0, 0, 0, 0, 0), dsJacobian.getY().getPartialDerivative(1, 0, 0, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
    Vector3D dFdPYRef = new Vector3D(finiteDifferencesJacobian[0][1], finiteDifferencesJacobian[1][1], finiteDifferencesJacobian[2][1]);
    Vector3D dFdPYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 1, 0, 0, 0, 0), dsJacobian.getY().getPartialDerivative(0, 1, 0, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
    Vector3D dFdPZRef = new Vector3D(finiteDifferencesJacobian[0][2], finiteDifferencesJacobian[1][2], finiteDifferencesJacobian[2][2]);
    Vector3D dFdPZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 1, 0, 0, 0), dsJacobian.getY().getPartialDerivative(0, 0, 1, 0, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
    Vector3D dFdVXRef = new Vector3D(finiteDifferencesJacobian[0][3], finiteDifferencesJacobian[1][3], finiteDifferencesJacobian[2][3]);
    Vector3D dFdVXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 1, 0, 0), dsJacobian.getY().getPartialDerivative(0, 0, 0, 1, 0, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
    Vector3D dFdVYRef = new Vector3D(finiteDifferencesJacobian[0][4], finiteDifferencesJacobian[1][4], finiteDifferencesJacobian[2][4]);
    Vector3D dFdVYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 1, 0), dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 1, 0), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
    Vector3D dFdVZRef = new Vector3D(finiteDifferencesJacobian[0][5], finiteDifferencesJacobian[1][5], finiteDifferencesJacobian[2][5]);
    Vector3D dFdVZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 0, 1), dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 0, 1), dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
    if (print) {
        System.out.println("dF/dPX ref: " + dFdPXRef.getX() + " " + dFdPXRef.getY() + " " + dFdPXRef.getZ());
        System.out.println("dF/dPX res: " + dFdPXRes.getX() + " " + dFdPXRes.getY() + " " + dFdPXRes.getZ());
        System.out.println("dF/dPY ref: " + dFdPYRef.getX() + " " + dFdPYRef.getY() + " " + dFdPYRef.getZ());
        System.out.println("dF/dPY res: " + dFdPYRes.getX() + " " + dFdPYRes.getY() + " " + dFdPYRes.getZ());
        System.out.println("dF/dPZ ref: " + dFdPZRef.getX() + " " + dFdPZRef.getY() + " " + dFdPZRef.getZ());
        System.out.println("dF/dPZ res: " + dFdPZRes.getX() + " " + dFdPZRes.getY() + " " + dFdPZRes.getZ());
        System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
        System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
        System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
        System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
        System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
        System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
    }
    checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
    checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
    checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
    checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
    checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
    checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
}

Example 50 with DSFactory

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

the class BoxAndSolarArraySpacecraftTest method testNormalOptimalRotationDS.

@Test
public void testNormalOptimalRotationDS() throws OrekitException {
    AbsoluteDate initialDate = propagator.getInitialState().getDate();
    CelestialBody sun = CelestialBodyFactory.getSun();
    BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(0, 0, 0, sun, 20.0, Vector3D.PLUS_J, 0.0, 1.0, 0.0);
    DSFactory factory = new DSFactory(1, 2);
    for (double dt = 0; dt < 4000; dt += 60) {
        AbsoluteDate date = initialDate.shiftedBy(dt);
        SpacecraftState state = propagator.propagate(date);
        FieldVector3D<DerivativeStructure> normal = s.getNormal(state.getDate(), state.getFrame(), new FieldVector3D<>(factory.getDerivativeField(), state.getPVCoordinates().getPosition()), new FieldRotation<>(factory.getDerivativeField(), state.getAttitude().getRotation()));
        Assert.assertEquals(0, FieldVector3D.dotProduct(normal, Vector3D.PLUS_J).getReal(), 1.0e-16);
    }
}