Examples with TimeStampedFieldPVCoordinates org.orekit.utils.TimeStampedFieldPVCoordinates used on opensource projects

Example 11 with TimeStampedFieldPVCoordinates

use of org.orekit.utils.TimeStampedFieldPVCoordinates in project Orekit by CS-SI.

the class FieldEclipseDetectorTest method doTestInsideOcculted.

private <T extends RealFieldElement<T>> void doTestInsideOcculted(Field<T> field) throws OrekitException {
    T zero = field.getZero();
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(-6142438.668), zero.add(3492467.560), zero.add(-25767.25680));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(505.8479685), zero.add(942.7809215), zero.add(7435.922231));
    FieldAbsoluteDate<T> iniDate = new FieldAbsoluteDate<>(field, 1969, 7, 28, 4, 0, 0.0, TimeScalesFactory.getTT());
    final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(new FieldPVCoordinates<>(position, velocity), FramesFactory.getGCRF(), iniDate, mu);
    FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(orbit);
    double[] absTolerance = { 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001 };
    double[] relTolerance = { 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7 };
    AdaptiveStepsizeFieldIntegrator<T> integrator = new DormandPrince853FieldIntegrator<>(field, 0.001, 1000, absTolerance, relTolerance);
    integrator.setInitialStepSize(field.getZero().add(60));
    FieldNumericalPropagator<T> propagator = new FieldNumericalPropagator<>(field, integrator);
    propagator.setOrbitType(OrbitType.EQUINOCTIAL);
    propagator.setInitialState(initialState);
    sun = CelestialBodyFactory.getSun();
    earth = CelestialBodyFactory.getEarth();
    sunRadius = 696000000.;
    earthRadius = 6400000.;
    FieldEclipseDetector<T> e = new FieldEclipseDetector<>(field.getZero().add(60.), field.getZero().add(1.e-3), sun, sunRadius, earth, earthRadius);
    Vector3D p = sun.getPVCoordinates(AbsoluteDate.J2000_EPOCH, FramesFactory.getGCRF()).getPosition();
    FieldSpacecraftState<T> s = new FieldSpacecraftState<>(new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(FieldAbsoluteDate.getJ2000Epoch(field), new FieldPVCoordinates<>(new FieldVector3D<>(field.getOne(), field.getZero(), field.getZero()).add(p), new FieldVector3D<>(field.getZero(), field.getZero(), field.getOne()))), FramesFactory.getGCRF(), mu));
    Assert.assertEquals(FastMath.PI, e.g(s).getReal(), 1.0e-15);
}

Example 12 with TimeStampedFieldPVCoordinates

use of org.orekit.utils.TimeStampedFieldPVCoordinates 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 13 with TimeStampedFieldPVCoordinates

use of org.orekit.utils.TimeStampedFieldPVCoordinates in project Orekit by CS-SI.

the class SolidTidesTest 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 attractionModelField = SolidTides.class.getDeclaredField("attractionModel");
        attractionModelField.setAccessible(true);
        ForceModel attractionModel = (ForceModel) attractionModelField.get(forceModel);
        double mu = GravityFieldFactory.getConstantNormalizedProvider(5, 5).getMu();
        Field<DerivativeStructure> field = position.getX().getField();
        FieldAbsoluteDate<DerivativeStructure> dsDate = new FieldAbsoluteDate<>(field, date);
        FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
        FieldSpacecraftState<DerivativeStructure> dState = new FieldSpacecraftState<>(new FieldCartesianOrbit<>(new TimeStampedFieldPVCoordinates<>(dsDate, position, velocity, zero), frame, mu), new FieldAttitude<>(frame, new TimeStampedFieldAngularCoordinates<>(dsDate, rotation, zero, zero)), mass);
        return attractionModel.acceleration(dState, attractionModel.getParameters(field));
    } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
        return null;
    }
}

Example 14 with TimeStampedFieldPVCoordinates

use of org.orekit.utils.TimeStampedFieldPVCoordinates 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 15 with TimeStampedFieldPVCoordinates

use of org.orekit.utils.TimeStampedFieldPVCoordinates in project Orekit by CS-SI.

the class FieldNumericalPropagatorTest method createHyperbolicOrbit.

private static <T extends RealFieldElement<T>> FieldCartesianOrbit<T> createHyperbolicOrbit(Field<T> field) throws OrekitException {
    T zero = field.getZero();
    final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, "2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
    final FieldVector3D<T> position = new FieldVector3D<>(zero.add(224267911.905821), zero.add(290251613.109399), zero.add(45534292.777492));
    final FieldVector3D<T> velocity = new FieldVector3D<>(zero.add(-1494.068165293), zero.add(1124.771027677), zero.add(526.915286134));
    final TimeStampedFieldPVCoordinates<T> pv = new TimeStampedFieldPVCoordinates<>(date, position, velocity, FieldVector3D.getZero(field));
    final Frame frame = FramesFactory.getEME2000();
    final double mu = Constants.EIGEN5C_EARTH_MU;
    return new FieldCartesianOrbit<>(pv, frame, mu);
}