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

Example 46 with DerivativeStructure

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

the class DSConverter method getParameters.

/**
 * Get the force model parameters.
 * @param state state as returned by {@link #getState(ForceModel)}
 * @param forceModel force model associated with the parameters
 * @return force model parameters
 * @since 9.0
 */
public DerivativeStructure[] getParameters(final FieldSpacecraftState<DerivativeStructure> state, final ForceModel forceModel) {
    final DSFactory factory = state.getMass().getFactory();
    final ParameterDriver[] drivers = forceModel.getParametersDrivers();
    final DerivativeStructure[] parameters = new DerivativeStructure[drivers.length];
    int index = freeStateParameters;
    for (int i = 0; i < drivers.length; ++i) {
        parameters[i] = drivers[i].isSelected() ? factory.variable(index++, drivers[i].getValue()) : factory.constant(drivers[i].getValue());
    }
    return parameters;
}

Example 47 with DerivativeStructure

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

the class PartialDerivativesEquations method computeDerivatives.

/**
 * {@inheritDoc}
 */
public double[] computeDerivatives(final SpacecraftState s, final double[] pDot) throws OrekitException {
    // initialize acceleration Jacobians to zero
    final int paramDim = selected.getNbParams();
    final int dim = 3;
    final double[][] dAccdParam = new double[dim][paramDim];
    final double[][] dAccdPos = new double[dim][dim];
    final double[][] dAccdVel = new double[dim][dim];
    final DSConverter fullConverter = new DSConverter(s, 6, propagator.getAttitudeProvider());
    final DSConverter posOnlyConverter = new DSConverter(s, 3, propagator.getAttitudeProvider());
    // compute acceleration Jacobians, finishing with the largest force: Newtonian attraction
    for (final ForceModel forceModel : propagator.getAllForceModels()) {
        final DSConverter converter = forceModel.dependsOnPositionOnly() ? posOnlyConverter : fullConverter;
        final FieldSpacecraftState<DerivativeStructure> dsState = converter.getState(forceModel);
        final DerivativeStructure[] parameters = converter.getParameters(dsState, forceModel);
        final FieldVector3D<DerivativeStructure> acceleration = forceModel.acceleration(dsState, parameters);
        final double[] derivativesX = acceleration.getX().getAllDerivatives();
        final double[] derivativesY = acceleration.getY().getAllDerivatives();
        final double[] derivativesZ = acceleration.getZ().getAllDerivatives();
        // update Jacobians with respect to state
        addToRow(derivativesX, 0, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
        addToRow(derivativesY, 1, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
        addToRow(derivativesZ, 2, converter.getFreeStateParameters(), dAccdPos, dAccdVel);
        int index = converter.getFreeStateParameters();
        for (ParameterDriver driver : forceModel.getParametersDrivers()) {
            if (driver.isSelected()) {
                final int parameterIndex = map.get(driver);
                ++index;
                dAccdParam[0][parameterIndex] += derivativesX[index];
                dAccdParam[1][parameterIndex] += derivativesY[index];
                dAccdParam[2][parameterIndex] += derivativesZ[index];
            }
        }
    }
    // the variational equations of the complete state Jacobian matrix have the following form:
    // [        |        ]   [                 |                  ]   [     |     ]
    // [  Adot  |  Bdot  ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [  A  |  B  ]
    // [        |        ]   [                 |                  ]   [     |     ]
    // ---------+---------   ------------------+------------------- * ------+------
    // [        |        ]   [                 |                  ]   [     |     ]
    // [  Cdot  |  Ddot  ] = [    dAcc/dPos    |     dAcc/dVel    ]   [  C  |  D  ]
    // [        |        ]   [                 |                  ]   [     |     ]
    // The A, B, C and D sub-matrices and their derivatives (Adot ...) are 3x3 matrices
    // The expanded multiplication above can be rewritten to take into account
    // the fixed values found in the sub-matrices in the left factor. This leads to:
    // [ Adot ] = [ C ]
    // [ Bdot ] = [ D ]
    // [ Cdot ] = [ dAcc/dPos ] * [ A ] + [ dAcc/dVel ] * [ C ]
    // [ Ddot ] = [ dAcc/dPos ] * [ B ] + [ dAcc/dVel ] * [ D ]
    // The following loops compute these expressions taking care of the mapping of the
    // (A, B, C, D) matrices into the single dimension array p and of the mapping of the
    // (Adot, Bdot, Cdot, Ddot) matrices into the single dimension array pDot.
    // copy C and E into Adot and Bdot
    final int stateDim = 6;
    final double[] p = s.getAdditionalState(getName());
    System.arraycopy(p, dim * stateDim, pDot, 0, dim * stateDim);
    // compute Cdot and Ddot
    for (int i = 0; i < dim; ++i) {
        final double[] dAdPi = dAccdPos[i];
        final double[] dAdVi = dAccdVel[i];
        for (int j = 0; j < stateDim; ++j) {
            pDot[(dim + i) * stateDim + j] = dAdPi[0] * p[j] + dAdPi[1] * p[j + stateDim] + dAdPi[2] * p[j + 2 * stateDim] + dAdVi[0] * p[j + 3 * stateDim] + dAdVi[1] * p[j + 4 * stateDim] + dAdVi[2] * p[j + 5 * stateDim];
        }
    }
    for (int k = 0; k < paramDim; ++k) {
        // the variational equations of the parameters Jacobian matrix are computed
        // one column at a time, they have the following form:
        // [      ]   [                 |                  ]   [   ]   [                  ]
        // [ Edot ]   [  dVel/dPos = 0  |  dVel/dVel = Id  ]   [ E ]   [  dVel/dParam = 0 ]
        // [      ]   [                 |                  ]   [   ]   [                  ]
        // --------   ------------------+------------------- * ----- + --------------------
        // [      ]   [                 |                  ]   [   ]   [                  ]
        // [ Fdot ] = [    dAcc/dPos    |     dAcc/dVel    ]   [ F ]   [    dAcc/dParam   ]
        // [      ]   [                 |                  ]   [   ]   [                  ]
        // The E and F sub-columns and their derivatives (Edot, Fdot) are 3 elements columns.
        // The expanded multiplication and addition above can be rewritten to take into
        // account the fixed values found in the sub-matrices in the left factor. This leads to:
        // [ Edot ] = [ F ]
        // [ Fdot ] = [ dAcc/dPos ] * [ E ] + [ dAcc/dVel ] * [ F ] + [ dAcc/dParam ]
        // The following loops compute these expressions taking care of the mapping of the
        // (E, F) columns into the single dimension array p and of the mapping of the
        // (Edot, Fdot) columns into the single dimension array pDot.
        // copy F into Edot
        final int columnTop = stateDim * stateDim + k;
        pDot[columnTop] = p[columnTop + 3 * paramDim];
        pDot[columnTop + paramDim] = p[columnTop + 4 * paramDim];
        pDot[columnTop + 2 * paramDim] = p[columnTop + 5 * paramDim];
        // compute Fdot
        for (int i = 0; i < dim; ++i) {
            final double[] dAdPi = dAccdPos[i];
            final double[] dAdVi = dAccdVel[i];
            pDot[columnTop + (dim + i) * paramDim] = dAccdParam[i][k] + dAdPi[0] * p[columnTop] + dAdPi[1] * p[columnTop + paramDim] + dAdPi[2] * p[columnTop + 2 * paramDim] + dAdVi[0] * p[columnTop + 3 * paramDim] + dAdVi[1] * p[columnTop + 4 * paramDim] + dAdVi[2] * p[columnTop + 5 * paramDim];
        }
    }
    // these equations have no effect on the main state itself
    return null;
}

Example 48 with DerivativeStructure

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

the class ElevationExtremumDetector method g.

/**
 * Compute the value of the detection function.
 * <p>
 * The value is the spacecraft elevation first time derivative.
 * </p>
 * @param s the current state information: date, kinematics, attitude
 * @return spacecraft elevation first time derivative
 * @exception OrekitException if some specific error occurs
 */
public double g(final SpacecraftState s) throws OrekitException {
    // get position, velocity acceleration of spacecraft in topocentric frame
    final Transform inertToTopo = s.getFrame().getTransformTo(topo, s.getDate());
    final TimeStampedPVCoordinates pvTopo = inertToTopo.transformPVCoordinates(s.getPVCoordinates());
    // convert the coordinates to DerivativeStructure based vector
    // instead of having vector position, then vector velocity then vector acceleration
    // we get one vector and each coordinate is a DerivativeStructure containing
    // value, first time derivative (we don't need second time derivative here)
    final FieldVector3D<DerivativeStructure> pvDS = pvTopo.toDerivativeStructureVector(1);
    // compute elevation and its first time derivative
    final DerivativeStructure elevation = pvDS.getZ().divide(pvDS.getNorm()).asin();
    // return elevation first time derivative
    return elevation.getPartialDerivative(1);
}

Example 49 with DerivativeStructure

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

the class RelativityTest method RealFieldExpectErrorTest.

/**
 *Same test as the previous one but not adding the ForceModel to the NumericalPropagator
 *        it is a test to validate the previous test.
 *        (to test if the ForceModel it's actually
 *        doing something in the Propagator and the FieldPropagator)
 */
@Test
public void RealFieldExpectErrorTest() throws OrekitException {
    DSFactory factory = new DSFactory(6, 0);
    DerivativeStructure a_0 = factory.variable(0, 7e7);
    DerivativeStructure e_0 = factory.variable(1, 0.4);
    DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
    DerivativeStructure R_0 = factory.variable(3, 0.7);
    DerivativeStructure O_0 = factory.variable(4, 0.5);
    DerivativeStructure n_0 = factory.variable(5, 0.1);
    Field<DerivativeStructure> field = a_0.getField();
    DerivativeStructure zero = field.getZero();
    FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
    Frame EME = FramesFactory.getEME2000();
    FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0, PositionAngle.MEAN, EME, J2000, Constants.EIGEN5C_EARTH_MU);
    FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
    SpacecraftState iSR = initialState.toSpacecraftState();
    OrbitType type = OrbitType.KEPLERIAN;
    double[][] tolerance = NumericalPropagator.tolerances(0.001, FKO.toOrbit(), type);
    AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
    integrator.setInitialStepSize(zero.add(60));
    AdaptiveStepsizeIntegrator RIntegrator = new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
    RIntegrator.setInitialStepSize(60);
    FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
    FNP.setOrbitType(type);
    FNP.setInitialState(initialState);
    NumericalPropagator NP = new NumericalPropagator(RIntegrator);
    NP.setOrbitType(type);
    NP.setInitialState(iSR);
    final Relativity forceModel = new Relativity(Constants.EIGEN5C_EARTH_MU);
    FNP.addForceModel(forceModel);
    // NOT ADDING THE FORCE MODEL TO THE NUMERICAL PROPAGATOR   NP.addForceModel(forceModel);
    FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
    FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
    SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
    FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
    PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
    Assert.assertEquals(0, Vector3D.distance(finPVC_DS.toPVCoordinates().getPosition(), finPVC_R.getPosition()), 8.0e-13 * finPVC_R.getPosition().getNorm());
}

Example 50 with DerivativeStructure

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

the class ThirdBodyAttractionTest 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 bodyField = ThirdBodyAttraction.class.getDeclaredField("body");
        bodyField.setAccessible(true);
        CelestialBody body = (CelestialBody) bodyField.get(forceModel);
        double gm = forceModel.getParameterDriver(body.getName() + ThirdBodyAttraction.ATTRACTION_COEFFICIENT_SUFFIX).getValue();
        // compute bodies separation vectors and squared norm
        final Vector3D centralToBody = body.getPVCoordinates(date, frame).getPosition();
        final double r2Central = centralToBody.getNormSq();
        final FieldVector3D<DerivativeStructure> satToBody = position.subtract(centralToBody).negate();
        final DerivativeStructure r2Sat = satToBody.getNormSq();
        // compute relative acceleration
        final FieldVector3D<DerivativeStructure> satAcc = new FieldVector3D<>(r2Sat.sqrt().multiply(r2Sat).reciprocal().multiply(gm), satToBody);
        final Vector3D centralAcc = new Vector3D(gm / (r2Central * FastMath.sqrt(r2Central)), centralToBody);
        return satAcc.subtract(centralAcc);
    } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
        return null;
    }
}