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

Example 56 with DSFactory

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

the class ThirdBodyAttractionTest 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, 5);
    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 ThirdBodyAttraction forceModel = new ThirdBodyAttraction(CelestialBodyFactory.getSun());
    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.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
    Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
    Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
}

Example 57 with DSFactory

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

the class RangeAnalytic method theoreticalEvaluationValidation.

/**
 * Added for validation
 * Compares directly numeric and analytic computations
 * @param iteration
 * @param evaluation
 * @param state
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<Range> theoreticalEvaluationValidation(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
    // Station & DSFactory attributes from parent Range class
    final GroundStation groundStation = getStation();
    // get the number of parameters used for derivation
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory dsFactory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = dsFactory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Range derivatives are computed with respect to spacecraft state in inertial frame
    // and station position in station's offset frame
    // -------
    // 
    // Parameters:
    // - 0..2 - Px, Py, Pz   : Position of the spacecraft in inertial frame
    // - 3..5 - Vx, Vy, Vz   : Velocity of the spacecraft in inertial frame
    // - 6..8 - QTx, QTy, QTz: Position of the station in station's offset frame
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, dsFactory);
    // transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDateDS, dsFactory, indices);
    // Station position in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have offset == downlinkDelay and transitState will be
    // the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure tauDMDelta = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(tauDMDelta.getValue());
    // Transit state position (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(tauDMDelta);
    // Station at transit state date (derivatives of tauD taken into account)
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationAtTransitDate = stationDownlink.shiftedBy(tauD.negate());
    // Uplink delay
    final DerivativeStructure tauU = signalTimeOfFlight(stationAtTransitDate, transitStateDS.getPosition(), transitStateDS.getDate());
    // Prepare the evaluation
    final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, null);
    // Range value
    final DerivativeStructure tau = tauD.add(tauU);
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final DerivativeStructure range = tau.multiply(cOver2);
    estimated.setEstimatedValue(range.getValue());
    // Range partial derivatives with respect to state
    final double[] derivatives = range.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    // ----------
    // VALIDATION
    // -----------
    // Computation of the value without DS
    // ----------------------------------
    // Time difference between t (date of the measurement) and t' (date tagged in spacecraft state)
    // Station position at signal arrival
    final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
    final PVCoordinates QDownlink = topoToInertDownlink.transformPVCoordinates(PVCoordinates.ZERO);
    // Downlink time of flight from spacecraft to station
    final double td = signalTimeOfFlight(state.getPVCoordinates(), QDownlink.getPosition(), downlinkDate);
    final double dt = delta - td;
    // Transit state position
    final AbsoluteDate transitT = state.getDate().shiftedBy(dt);
    final SpacecraftState transit = state.shiftedBy(dt);
    final Vector3D transitP = transitState.getPVCoordinates().getPosition();
    // Station position at signal departure
    // First guess
    // AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-getObservedValue()[0] / cOver2);
    // final Transform topoToInertUplink =
    // station.getOffsetFrame().getTransformTo(state.getFrame(), uplinkDate);
    // TimeStampedPVCoordinates QUplink = topoToInertUplink.
    // transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Station position at transit state date
    final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitT);
    TimeStampedPVCoordinates QAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitT, PVCoordinates.ZERO));
    // Uplink time of flight
    final double tu = signalTimeOfFlight(QAtTransitDate, transitP, transitT);
    // Total time of flight
    final double t = td + tu;
    // Real date and position of station at signal departure
    AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-t);
    TimeStampedPVCoordinates QUplink = topoToInertDownlink.shiftedBy(-t).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
    // Range value
    double r = t * cOver2;
    double dR = r - range.getValue();
    // td derivatives / state
    // -----------------------
    // Qt = Master station position at tmeas = t = signal arrival at master station
    final Vector3D vel = state.getPVCoordinates().getVelocity();
    final Vector3D Qt_V = QDownlink.getVelocity();
    final Vector3D Ptr = transit.getPVCoordinates().getPosition();
    final Vector3D Ptr_Qt = QDownlink.getPosition().subtract(Ptr);
    final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * td - Vector3D.dotProduct(Ptr_Qt, vel);
    // Derivatives of the downlink time of flight
    final double dtddPx = -Ptr_Qt.getX() / dDown;
    final double dtddPy = -Ptr_Qt.getY() / dDown;
    final double dtddPz = -Ptr_Qt.getZ() / dDown;
    final double dtddVx = dtddPx * dt;
    final double dtddVy = dtddPy * dt;
    final double dtddVz = dtddPz * dt;
    // From the DS
    final double dtddPxDS = tauD.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPyDS = tauD.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtddPzDS = tauD.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtddVxDS = tauD.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtddVyDS = tauD.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtddVzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtddPx = dtddPxDS - dtddPx;
    final double d_dtddPy = dtddPyDS - dtddPy;
    final double d_dtddPz = dtddPzDS - dtddPz;
    final double d_dtddVx = dtddVxDS - dtddVx;
    final double d_dtddVy = dtddVyDS - dtddVy;
    final double d_dtddVz = dtddVzDS - dtddVz;
    // tu derivatives / state
    // -----------------------
    final Vector3D Qt2_Ptr = Ptr.subtract(QUplink.getPosition());
    final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu - Vector3D.dotProduct(Qt2_Ptr, Qt_V);
    // test
    // // Speed of the station at tmeas-t
    // // Note: Which one to use in the calculation of dUp ???
    // final Vector3D Qt2_V    = QUplink.getVelocity();
    // final double   dUp      = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tu -
    // Vector3D.dotProduct(Qt2_Ptr, Qt2_V);
    // test
    // tu derivatives
    final double dtudPx = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddPx)));
    final double dtudPy = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddPy)));
    final double dtudPz = 1. / dUp * Qt2_Ptr.dotProduct(Vector3D.PLUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddPz)));
    final double dtudVx = dtudPx * dt;
    final double dtudVy = dtudPy * dt;
    final double dtudVz = dtudPz * dt;
    // From the DS
    final double dtudPxDS = tauU.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPyDS = tauU.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtudPzDS = tauU.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    final double dtudVxDS = tauU.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    final double dtudVyDS = tauU.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    final double dtudVzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Difference
    final double d_dtudPx = dtudPxDS - dtudPx;
    final double d_dtudPy = dtudPyDS - dtudPy;
    final double d_dtudPz = dtudPzDS - dtudPz;
    final double d_dtudVx = dtudVxDS - dtudVx;
    final double d_dtudVy = dtudVyDS - dtudVy;
    final double d_dtudVz = dtudVzDS - dtudVz;
    // Range derivatives / state
    // -----------------------
    // R = Range
    double dRdPx = (dtddPx + dtudPx) * cOver2;
    double dRdPy = (dtddPy + dtudPy) * cOver2;
    double dRdPz = (dtddPz + dtudPz) * cOver2;
    double dRdVx = (dtddVx + dtudVx) * cOver2;
    double dRdVy = (dtddVy + dtudVy) * cOver2;
    double dRdVz = (dtddVz + dtudVz) * cOver2;
    // With DS
    double dRdPxDS = range.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdPyDS = range.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dRdPzDS = range.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0);
    double dRdVxDS = range.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dRdVyDS = range.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dRdVzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0);
    // Diff
    final double d_dRdPx = dRdPxDS - dRdPx;
    final double d_dRdPy = dRdPyDS - dRdPy;
    final double d_dRdPz = dRdPzDS - dRdPz;
    final double d_dRdVx = dRdVxDS - dRdVx;
    final double d_dRdVy = dRdVyDS - dRdVy;
    final double d_dRdVz = dRdVzDS - dRdVz;
    // td derivatives / station
    // -----------------------
    final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
    final Rotation rotTopoToInert = ac.getRotation();
    final Vector3D omega = ac.getRotationRate();
    final Vector3D dtddQI = Ptr_Qt.scalarMultiply(1. / dDown);
    final double dtddQIx = dtddQI.getX();
    final double dtddQIy = dtddQI.getY();
    final double dtddQIz = dtddQI.getZ();
    final Vector3D dtddQ = rotTopoToInert.applyTo(dtddQI);
    // With DS
    double dtddQxDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtddQyDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtddQzDS = tauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtddQx = dtddQxDS - dtddQ.getX();
    final double d_dtddQy = dtddQyDS - dtddQ.getY();
    final double d_dtddQz = dtddQzDS - dtddQ.getZ();
    // tu derivatives / station
    // -----------------------
    // Inertial frame
    final double dtudQIx = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_I.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIy = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_J.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(t)));
    final double dtudQIz = 1 / dUp * Qt2_Ptr.dotProduct(Vector3D.MINUS_K.add((Qt_V.subtract(vel)).scalarMultiply(dtddQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(t)));
    // // test
    // final double dtudQIx = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_I);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIx));
    // .dotProduct(Vector3D.MINUS_I.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIy = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_J);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIy));
    // .dotProduct(Vector3D.MINUS_J.crossProduct(omega).scalarMultiply(t));
    // final double dtudQIz = 1/dUp*Qt2_Ptr
    // //                        .dotProduct(Vector3D.MINUS_K);
    // //                                    .dotProduct((Qt_V.subtract(vel)).scalarMultiply(dtddQIz));
    // .dotProduct(Vector3D.MINUS_K.crossProduct(omega).scalarMultiply(t));
    // 
    // double dtu_dQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    // double dtu_dQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    // double dtu_dQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // final Vector3D dtudQDS = new Vector3D(dtu_dQxDS, dtu_dQyDS, dtu_dQzDS);
    // final Vector3D dtudQIDS = rotTopoToInert.applyInverseTo(dtudQDS);
    // double dtudQIxDS = dtudQIDS.getX();
    // double dtudQIyDS = dtudQIDS.getY();
    // double dtudQIxzS = dtudQIDS.getZ();
    // // test
    // Topocentric frame
    final Vector3D dtudQI = new Vector3D(dtudQIx, dtudQIy, dtudQIz);
    final Vector3D dtudQ = rotTopoToInert.applyTo(dtudQI);
    // With DS
    double dtudQxDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtudQyDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtudQzDS = tauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtudQx = dtudQxDS - dtudQ.getX();
    final double d_dtudQy = dtudQyDS - dtudQ.getY();
    final double d_dtudQz = dtudQzDS - dtudQ.getZ();
    // Range derivatives / station
    // -----------------------
    double dRdQx = (dtddQ.getX() + dtudQ.getX()) * cOver2;
    double dRdQy = (dtddQ.getY() + dtudQ.getY()) * cOver2;
    double dRdQz = (dtddQ.getZ() + dtudQ.getZ()) * cOver2;
    // With DS
    double dRdQxDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dRdQyDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dRdQzDS = range.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dRdQx = dRdQxDS - dRdQx;
    final double d_dRdQy = dRdQyDS - dRdQy;
    final double d_dRdQz = dRdQzDS - dRdQz;
    // Print results to avoid warning
    final boolean printResults = false;
    if (printResults) {
        System.out.println("dR = " + dR);
        System.out.println("d_dtddPx = " + d_dtddPx);
        System.out.println("d_dtddPy = " + d_dtddPy);
        System.out.println("d_dtddPz = " + d_dtddPz);
        System.out.println("d_dtddVx = " + d_dtddVx);
        System.out.println("d_dtddVy = " + d_dtddVy);
        System.out.println("d_dtddVz = " + d_dtddVz);
        System.out.println("d_dtudPx = " + d_dtudPx);
        System.out.println("d_dtudPy = " + d_dtudPy);
        System.out.println("d_dtudPz = " + d_dtudPz);
        System.out.println("d_dtudVx = " + d_dtudVx);
        System.out.println("d_dtudVy = " + d_dtudVy);
        System.out.println("d_dtudVz = " + d_dtudVz);
        System.out.println("d_dRdPx = " + d_dRdPx);
        System.out.println("d_dRdPy = " + d_dRdPy);
        System.out.println("d_dRdPz = " + d_dRdPz);
        System.out.println("d_dRdVx = " + d_dRdVx);
        System.out.println("d_dRdVy = " + d_dRdVy);
        System.out.println("d_dRdVz = " + d_dRdVz);
        System.out.println("d_dtddQx = " + d_dtddQx);
        System.out.println("d_dtddQy = " + d_dtddQy);
        System.out.println("d_dtddQz = " + d_dtddQz);
        System.out.println("d_dtudQx = " + d_dtudQx);
        System.out.println("d_dtudQy = " + d_dtudQy);
        System.out.println("d_dtudQz = " + d_dtudQz);
        System.out.println("d_dRdQx = " + d_dRdQx);
        System.out.println("d_dRdQy = " + d_dRdQy);
        System.out.println("d_dRdQz = " + d_dRdQz);
    }
    // Dummy return
    return estimated;
}

Example 58 with DSFactory

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

the class AngularRaDec method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<AngularRaDec> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Right Ascension/elevation (in reference frame )derivatives are computed with respect to spacecraft state in inertial frame
    // and station parameters
    // ----------------------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    // Get the number of parameters used for derivation
    // Place the selected drivers into a map
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // Transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position/velocity in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateDS = pvaDS.shiftedBy(deltaMTauD);
    // Station-satellite vector expressed in inertial frame
    final FieldVector3D<DerivativeStructure> staSatInertial = transitStateDS.getPosition().subtract(stationDownlink.getPosition());
    // Field transform from inertial to reference frame at station's reception date
    final FieldTransform<DerivativeStructure> inertialToReferenceDownlink = state.getFrame().getTransformTo(referenceFrame, downlinkDateDS);
    // Station-satellite vector in reference frame
    final FieldVector3D<DerivativeStructure> staSatReference = inertialToReferenceDownlink.transformPosition(staSatInertial);
    // Compute right ascension and declination
    final DerivativeStructure baseRightAscension = staSatReference.getAlpha();
    final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension.getReal(), getObservedValue()[0]) - baseRightAscension.getReal();
    final DerivativeStructure rightAscension = baseRightAscension.add(twoPiWrap);
    final DerivativeStructure declination = staSatReference.getDelta();
    // Prepare the estimation
    final EstimatedMeasurement<AngularRaDec> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { transitStateDS.toTimeStampedPVCoordinates(), stationDownlink.toTimeStampedPVCoordinates() });
    // azimuth - elevation values
    estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue());
    // Partial derivatives of right ascension/declination in reference frame with respect to state
    // (beware element at index 0 is the value, not a derivative)
    final double[] raDerivatives = rightAscension.getAllDerivatives();
    final double[] decDerivatives = declination.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(raDerivatives, 1, 7), Arrays.copyOfRange(decDerivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, raDerivatives[index + 1], decDerivatives[index + 1]);
        }
    }
    return estimated;
}

Example 59 with DSFactory

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

the class RangeRate method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<RangeRate> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Range-rate derivatives are computed with respect to spacecraft state in inertial frame
    // and station position in station's offset frame
    // -------
    // 
    // Parameters:
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - station parameters (station offsets, pole, prime meridian...)
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        if (driver.isSelected()) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // transform between station and inertial frame, expressed as a derivative structure
    // The components of station's position in offset frame are the 3 last derivative parameters
    final AbsoluteDate downlinkDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> downlinkDateDS = new FieldAbsoluteDate<>(field, downlinkDate);
    final FieldTransform<DerivativeStructure> offsetToInertialDownlink = station.getOffsetToInertial(state.getFrame(), downlinkDateDS, factory, indices);
    // Station position in inertial frame at end of the downlink leg
    final TimeStampedFieldPVCoordinates<DerivativeStructure> stationDownlink = offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, zero, zero, zero));
    // Compute propagation times
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta == tauD and transitState will be the same as state)
    // Downlink delay
    final DerivativeStructure tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS);
    // Transit state
    final double delta = downlinkDate.durationFrom(state.getDate());
    final DerivativeStructure deltaMTauD = tauD.negate().add(delta);
    final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue());
    // Transit state (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitPV = pvaDS.shiftedBy(deltaMTauD);
    // one-way (downlink) range-rate
    final EstimatedMeasurement<RangeRate> evalOneWay1 = oneWayTheoreticalEvaluation(iteration, evaluation, true, stationDownlink, transitPV, transitState, indices);
    final EstimatedMeasurement<RangeRate> estimated;
    if (twoway) {
        // one-way (uplink) light time correction
        final AbsoluteDate approxUplinkDate = downlinkDate.shiftedBy(-2 * tauD.getValue());
        final FieldAbsoluteDate<DerivativeStructure> approxUplinkDateDS = new FieldAbsoluteDate<>(field, approxUplinkDate);
        final FieldTransform<DerivativeStructure> offsetToInertialApproxUplink = station.getOffsetToInertial(state.getFrame(), approxUplinkDateDS, factory, indices);
        final TimeStampedFieldPVCoordinates<DerivativeStructure> stationApproxUplink = offsetToInertialApproxUplink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxUplinkDateDS, zero, zero, zero));
        final DerivativeStructure tauU = signalTimeOfFlight(stationApproxUplink, transitPV.getPosition(), transitPV.getDate());
        final TimeStampedFieldPVCoordinates<DerivativeStructure> stationUplink = stationApproxUplink.shiftedBy(transitPV.getDate().durationFrom(approxUplinkDateDS).subtract(tauU));
        final EstimatedMeasurement<RangeRate> evalOneWay2 = oneWayTheoreticalEvaluation(iteration, evaluation, false, stationUplink, transitPV, transitState, indices);
        // combine uplink and downlink values
        estimated = new EstimatedMeasurement<>(this, iteration, evaluation, evalOneWay1.getStates(), new TimeStampedPVCoordinates[] { evalOneWay2.getParticipants()[0], evalOneWay1.getParticipants()[0], evalOneWay1.getParticipants()[1] });
        estimated.setEstimatedValue(0.5 * (evalOneWay1.getEstimatedValue()[0] + evalOneWay2.getEstimatedValue()[0]));
        // combine uplink and downlink partial derivatives with respect to state
        final double[][] sd1 = evalOneWay1.getStateDerivatives(0);
        final double[][] sd2 = evalOneWay2.getStateDerivatives(0);
        final double[][] sd = new double[sd1.length][sd1[0].length];
        for (int i = 0; i < sd.length; ++i) {
            for (int j = 0; j < sd[0].length; ++j) {
                sd[i][j] = 0.5 * (sd1[i][j] + sd2[i][j]);
            }
        }
        estimated.setStateDerivatives(0, sd);
        // combine uplink and downlink partial derivatives with respect to parameters
        evalOneWay1.getDerivativesDrivers().forEach(driver -> {
            final double[] pd1 = evalOneWay1.getParameterDerivatives(driver);
            final double[] pd2 = evalOneWay2.getParameterDerivatives(driver);
            final double[] pd = new double[pd1.length];
            for (int i = 0; i < pd.length; ++i) {
                pd[i] = 0.5 * (pd1[i] + pd2[i]);
            }
            estimated.setParameterDerivatives(driver, pd);
        });
    } else {
        estimated = evalOneWay1;
    }
    return estimated;
}

Example 60 with DSFactory

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

the class TurnAroundRange method theoreticalEvaluation.

/**
 * {@inheritDoc}
 */
@Override
protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluation(final int iteration, final int evaluation, final SpacecraftState[] states) throws OrekitException {
    final SpacecraftState state = states[getPropagatorsIndices().get(0)];
    // Turn around range derivatives are computed with respect to:
    // - Spacecraft state in inertial frame
    // - Master station parameters
    // - Slave station parameters
    // --------------------------
    // 
    // - 0..2 - Position of the spacecraft in inertial frame
    // - 3..5 - Velocity of the spacecraft in inertial frame
    // - 6..n - stations' parameters (stations' offsets, pole, prime meridian...)
    int nbParams = 6;
    final Map<String, Integer> indices = new HashMap<>();
    for (ParameterDriver driver : getParametersDrivers()) {
        // as one set only (they are combined together by the estimation engine)
        if (driver.isSelected() && !indices.containsKey(driver.getName())) {
            indices.put(driver.getName(), nbParams++);
        }
    }
    final DSFactory factory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = factory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Place the derivative structures in a time-stamped PV
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, factory);
    // The path of the signal is divided in two legs.
    // Leg1: Emission from master station to satellite in masterTauU seconds
    // + Reflection from satellite to slave station in slaveTauD seconds
    // Leg2: Reflection from slave station to satellite in slaveTauU seconds
    // + Reflection from satellite to master station in masterTaudD seconds
    // The measurement is considered to be time stamped at reception on ground
    // by the master station. All times are therefore computed as backward offsets
    // with respect to this reception time.
    // 
    // Two intermediate spacecraft states are defined:
    // - transitStateLeg2: State of the satellite when it bounced back the signal
    // from slave station to master station during the 2nd leg
    // - transitStateLeg1: State of the satellite when it bounced back the signal
    // from master station to slave station during the 1st leg
    // Compute propagation time for the 2nd leg of the signal path
    // --
    // Time difference between t (date of the measurement) and t' (date tagged in spacecraft state)
    // (if state has already been set up to pre-compensate propagation delay,
    // we will have delta = masterTauD + slaveTauU)
    final AbsoluteDate measurementDate = getDate();
    final FieldAbsoluteDate<DerivativeStructure> measurementDateDS = new FieldAbsoluteDate<>(field, measurementDate);
    final double delta = measurementDate.durationFrom(state.getDate());
    // transform between master station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    final FieldTransform<DerivativeStructure> masterToInert = masterStation.getOffsetToInertial(state.getFrame(), measurementDateDS, factory, indices);
    // Master station PV in inertial frame at measurement date
    final TimeStampedFieldPVCoordinates<DerivativeStructure> masterArrival = masterToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
    // Compute propagation times
    final DerivativeStructure masterTauD = signalTimeOfFlight(pvaDS, masterArrival.getPosition(), measurementDateDS);
    // Elapsed time between state date t' and signal arrival to the transit state of the 2nd leg
    final DerivativeStructure dtLeg2 = masterTauD.negate().add(delta);
    // Transit state where the satellite reflected the signal from slave to master station
    final SpacecraftState transitStateLeg2 = state.shiftedBy(dtLeg2.getValue());
    // Transit state pv of leg2 (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateLeg2PV = pvaDS.shiftedBy(dtLeg2);
    // transform between slave station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    // The components of slave station's position in offset frame are the 3 last derivative parameters
    final FieldAbsoluteDate<DerivativeStructure> approxReboundDate = measurementDateDS.shiftedBy(-delta);
    final FieldTransform<DerivativeStructure> slaveToInertApprox = slaveStation.getOffsetToInertial(state.getFrame(), approxReboundDate, factory, indices);
    // Slave station PV in inertial frame at approximate rebound date on slave station
    final TimeStampedFieldPVCoordinates<DerivativeStructure> QSlaveApprox = slaveToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxReboundDate, zero, zero, zero));
    // Uplink time of flight from slave station to transit state of leg2
    final DerivativeStructure slaveTauU = signalTimeOfFlight(QSlaveApprox, transitStateLeg2PV.getPosition(), transitStateLeg2PV.getDate());
    // Total time of flight for leg 2
    final DerivativeStructure tauLeg2 = masterTauD.add(slaveTauU);
    // Compute propagation time for the 1st leg of the signal path
    // --
    // Absolute date of rebound of the signal to slave station
    final FieldAbsoluteDate<DerivativeStructure> reboundDateDS = measurementDateDS.shiftedBy(tauLeg2.negate());
    final FieldTransform<DerivativeStructure> slaveToInert = slaveStation.getOffsetToInertial(state.getFrame(), reboundDateDS, factory, indices);
    // Slave station PV in inertial frame at rebound date on slave station
    final TimeStampedFieldPVCoordinates<DerivativeStructure> slaveRebound = slaveToInert.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(reboundDateDS, FieldPVCoordinates.getZero(field)));
    // Downlink time of flight from transitStateLeg1 to slave station at rebound date
    final DerivativeStructure slaveTauD = signalTimeOfFlight(transitStateLeg2PV, slaveRebound.getPosition(), reboundDateDS);
    // Elapsed time between state date t' and signal arrival to the transit state of the 1st leg
    final DerivativeStructure dtLeg1 = dtLeg2.subtract(slaveTauU).subtract(slaveTauD);
    // Transit state pv of leg2 (re)computed with derivative structures
    final TimeStampedFieldPVCoordinates<DerivativeStructure> transitStateLeg1PV = pvaDS.shiftedBy(dtLeg1);
    // transform between master station topocentric frame (east-north-zenith) and inertial frame expressed as DerivativeStructures
    // The components of master station's position in offset frame are the 3 third derivative parameters
    final FieldAbsoluteDate<DerivativeStructure> approxEmissionDate = measurementDateDS.shiftedBy(-2 * (slaveTauU.getValue() + masterTauD.getValue()));
    final FieldTransform<DerivativeStructure> masterToInertApprox = masterStation.getOffsetToInertial(state.getFrame(), approxEmissionDate, factory, indices);
    // Master station PV in inertial frame at approximate emission date
    final TimeStampedFieldPVCoordinates<DerivativeStructure> QMasterApprox = masterToInertApprox.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxEmissionDate, zero, zero, zero));
    // Uplink time of flight from master station to transit state of leg1
    final DerivativeStructure masterTauU = signalTimeOfFlight(QMasterApprox, transitStateLeg1PV.getPosition(), transitStateLeg1PV.getDate());
    // Master station PV in inertial frame at exact emission date
    final AbsoluteDate emissionDate = transitStateLeg1PV.getDate().toAbsoluteDate().shiftedBy(-masterTauU.getValue());
    final TimeStampedPVCoordinates masterDeparture = masterToInertApprox.shiftedBy(emissionDate.durationFrom(masterToInertApprox.getDate())).transformPVCoordinates(new TimeStampedPVCoordinates(emissionDate, PVCoordinates.ZERO)).toTimeStampedPVCoordinates();
    // Total time of flight for leg 1
    final DerivativeStructure tauLeg1 = slaveTauD.add(masterTauU);
    // --
    // Evaluate the turn-around range value and its derivatives
    // --------------------------------------------------------
    // The state we use to define the estimated measurement is a middle ground between the two transit states
    // This is done to avoid calling "SpacecraftState.shiftedBy" function on long duration
    // Thus we define the state at the date t" = date of rebound of the signal at the slave station
    // Or t" = t -masterTauD -slaveTauU
    // The iterative process in the estimation ensures that, after several iterations, the date stamped in the
    // state S in input of this function will be close to t"
    // Therefore we will shift state S by:
    // - +slaveTauU to get transitStateLeg2
    // - -slaveTauD to get transitStateLeg1
    final EstimatedMeasurement<TurnAroundRange> estimated = new EstimatedMeasurement<>(this, iteration, evaluation, new SpacecraftState[] { transitStateLeg2.shiftedBy(-slaveTauU.getValue()) }, new TimeStampedPVCoordinates[] { masterDeparture, transitStateLeg1PV.toTimeStampedPVCoordinates(), slaveRebound.toTimeStampedPVCoordinates(), transitStateLeg2.getPVCoordinates(), masterArrival.toTimeStampedPVCoordinates() });
    // Turn-around range value = Total time of flight for the 2 legs divided by 2 and multiplied by c
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final DerivativeStructure turnAroundRange = (tauLeg2.add(tauLeg1)).multiply(cOver2);
    estimated.setEstimatedValue(turnAroundRange.getValue());
    // Turn-around range partial derivatives with respect to state
    final double[] derivatives = turnAroundRange.getAllDerivatives();
    estimated.setStateDerivatives(0, Arrays.copyOfRange(derivatives, 1, 7));
    // (beware element at index 0 is the value, not a derivative)
    for (final ParameterDriver driver : getParametersDrivers()) {
        final Integer index = indices.get(driver.getName());
        if (index != null) {
            estimated.setParameterDerivatives(driver, derivatives[index + 1]);
        }
    }
    return estimated;
}