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

Example 1 with TimeStampedPVCoordinates

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

the class BoxAndSolarArraySpacecraftTest method testNormalSunAlignedDouble.

@Test
public void testNormalSunAlignedDouble() throws OrekitException {
    BoxAndSolarArraySpacecraft s = new BoxAndSolarArraySpacecraft(0, 0, 0, (date, frame) -> new TimeStampedPVCoordinates(date, new Vector3D(0, 1e6, 0), Vector3D.ZERO), 20.0, Vector3D.PLUS_J, 0.0, 1.0, 0.0);
    Vector3D normal = s.getNormal(AbsoluteDate.J2000_EPOCH, FramesFactory.getEME2000(), Vector3D.ZERO, Rotation.IDENTITY);
    Assert.assertEquals(0, Vector3D.dotProduct(normal, Vector3D.PLUS_J), 1.0e-16);
}

Example 2 with TimeStampedPVCoordinates

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

the class TurnAroundRangeAnalytic method theoreticalEvaluationValidation.

/**
 * Added for validation
 * @param iteration
 * @param evaluation
 * @param state
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluationValidation(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
    // Stations & DSFactory attributes from parent TurnArounsRange class
    final GroundStation masterGroundStation = getMasterStation();
    final GroundStation slaveGroundStation = getSlaveStation();
    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 dsFactory = new DSFactory(nbParams, 1);
    final Field<DerivativeStructure> field = dsFactory.getDerivativeField();
    final FieldVector3D<DerivativeStructure> zero = FieldVector3D.getZero(field);
    // Coordinates of the spacecraft expressed as a derivative structure
    final TimeStampedFieldPVCoordinates<DerivativeStructure> pvaDS = getCoordinates(state, 0, dsFactory);
    // 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
    // The components of master station's position in offset frame are the 3 third derivative parameters
    final FieldTransform<DerivativeStructure> masterToInert = masterGroundStation.getOffsetToInertial(state.getFrame(), measurementDateDS, dsFactory, indices);
    // Master station PV in inertial frame at measurement date
    final FieldVector3D<DerivativeStructure> QMaster = masterToInert.transformPosition(zero);
    // Compute propagation times
    final DerivativeStructure masterTauD = signalTimeOfFlight(pvaDS, QMaster, 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 = slaveGroundStation.getOffsetToInertial(state.getFrame(), approxReboundDate, dsFactory, 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 = slaveGroundStation.getOffsetToInertial(state.getFrame(), reboundDateDS, dsFactory, indices);
    // Slave station PV in inertial frame at rebound date on slave station
    final FieldVector3D<DerivativeStructure> QSlave = slaveToInert.transformPosition(zero);
    // Downlink time of flight from transitStateLeg1 to slave station at rebound date
    final DerivativeStructure slaveTauD = signalTimeOfFlight(transitStateLeg2PV, QSlave, 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 = masterGroundStation.getOffsetToInertial(state.getFrame(), approxEmissionDate, dsFactory, 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());
    // 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()) }, null);
    // 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]);
        }
    }
    // ----------
    // VALIDATION: Using analytical version to compare
    // -----------
    // Computation of the value without DS
    // ----------------------------------
    // 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)
    // Master station PV at measurement date
    final Transform masterTopoToInert = masterGroundStation.getOffsetToInertial(state.getFrame(), measurementDate);
    final TimeStampedPVCoordinates QMt = masterTopoToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
    // Slave station PV at measurement date
    final Transform slaveTopoToInert = slaveGroundStation.getOffsetToInertial(state.getFrame(), measurementDate);
    final TimeStampedPVCoordinates QSt = slaveTopoToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
    // Downlink time of flight from master station at t to spacecraft at t'
    final double tMd = signalTimeOfFlight(state.getPVCoordinates(), QMt.getPosition(), measurementDate);
    // Transit state from which the satellite reflected the signal from slave to master station
    final SpacecraftState state2 = state.shiftedBy(delta - tMd);
    final AbsoluteDate transitDateLeg2 = transitStateLeg2.getDate();
    // Slave station PV at transit state leg2 date
    final Transform slaveTopoToInertTransitLeg2 = slaveGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg2);
    final TimeStampedPVCoordinates QSdate2PV = slaveTopoToInertTransitLeg2.transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg2, PVCoordinates.ZERO));
    // Uplink time of flight from slave station to transit state leg2
    final double tSu = signalTimeOfFlight(QSdate2PV, state2.getPVCoordinates().getPosition(), transitDateLeg2);
    // Total time of flight for leg 2
    final double t2 = tMd + tSu;
    // Compute propagation time for the 1st leg of the signal path
    // --
    // Absolute date of arrival of the signal to slave station
    final AbsoluteDate tQSA = measurementDate.shiftedBy(-t2);
    // Slave station position in inertial frame at date tQSA
    final Transform slaveTopoToInertArrivalDate = slaveGroundStation.getOffsetToInertial(state.getFrame(), tQSA);
    final Vector3D QSA = slaveTopoToInertArrivalDate.transformPosition(Vector3D.ZERO);
    // Dowlink time of flight from transitStateLeg1 to slave station at slaveStationArrivalDate
    final double tSd = signalTimeOfFlight(state2.getPVCoordinates(), QSA, tQSA);
    // Transit state from which the satellite reflected the signal from master to slave station
    final SpacecraftState state1 = state.shiftedBy(delta - tMd - tSu - tSd);
    final AbsoluteDate transitDateLeg1 = transitStateLeg1PV.getDate().toAbsoluteDate();
    // Master station PV at transit state date of leg1
    final Transform masterTopoToInertTransitLeg1 = masterGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg1);
    final TimeStampedPVCoordinates QMdate1PV = masterTopoToInertTransitLeg1.transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg1, PVCoordinates.ZERO));
    // Uplink time of flight from master station to transit state leg1
    final double tMu = signalTimeOfFlight(QMdate1PV, state1.getPVCoordinates().getPosition(), transitDateLeg1);
    // Total time of flight for leg 1
    final double t1 = tSd + tMu;
    // Total time of flight
    final double t = t1 + t2;
    // Turn-around range value
    final double TAR = t * cOver2;
    // Diff with DS
    final double dTAR = turnAroundRange.getValue() - TAR;
    // tMd derivatives / state
    // -----------------------
    // QMt_PV = Master station PV at tmeas = t = signal arrival at master station
    final Vector3D vel = state.getPVCoordinates().getVelocity();
    final PVCoordinates QMt_PV = masterTopoToInert.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QMt_V = QMt_PV.getVelocity();
    final Vector3D pos2 = state2.getPVCoordinates().getPosition();
    final Vector3D P2_QMt = QMt_PV.getPosition().subtract(pos2);
    final double dMDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMd - Vector3D.dotProduct(P2_QMt, vel);
    // derivatives of the downlink time of flight
    final double dtMddPx = -P2_QMt.getX() / dMDown;
    final double dtMddPy = -P2_QMt.getY() / dMDown;
    final double dtMddPz = -P2_QMt.getZ() / dMDown;
    final double dt = delta - tMd;
    final double dtMddVx = dtMddPx * dt;
    final double dtMddVy = dtMddPy * dt;
    final double dtMddVz = dtMddPz * dt;
    // From the DS
    final double dtMddPxDS = masterTauD.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMddPyDS = masterTauD.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMddPzDS = masterTauD.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMddVxDS = masterTauD.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMddVyDS = masterTauD.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtMddVzDS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Difference
    final double d_dtMddPx = dtMddPxDS - dtMddPx;
    final double d_dtMddPy = dtMddPyDS - dtMddPy;
    final double d_dtMddPz = dtMddPzDS - dtMddPz;
    final double d_dtMddVx = dtMddVxDS - dtMddVx;
    final double d_dtMddVy = dtMddVyDS - dtMddVy;
    final double d_dtMddVz = dtMddVzDS - dtMddVz;
    // tSu derivatives / state
    // -----------------------
    // QSt = slave station PV at tmeas = t = signal arrival at master station
    // final Transform FSt     = slaveStation.getOffsetFrame().getTransformTo(state.getFrame(), measurementDate);
    // final PVCoordinates QSt = FSt.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QSt_V = QSt.getVelocity();
    // QSt2 = slave station PV at t-t2 = signal arrival at slave station
    final PVCoordinates QSt2 = slaveTopoToInertArrivalDate.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QSt2_P2 = pos2.subtract(QSt2.getPosition());
    final double dSUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSu - Vector3D.dotProduct(QSt2_P2, QSt_V);
    final double alphaSu = 1. / dSUp * QSt2_P2.dotProduct(QSt_V.subtract(vel));
    final double dtSudPx = 1. / dSUp * QSt2_P2.getX() + alphaSu * dtMddPx;
    final double dtSudPy = 1. / dSUp * QSt2_P2.getY() + alphaSu * dtMddPy;
    final double dtSudPz = 1. / dSUp * QSt2_P2.getZ() + alphaSu * dtMddPz;
    final double dtSudVx = dtSudPx * dt;
    final double dtSudVy = dtSudPy * dt;
    final double dtSudVz = dtSudPz * dt;
    // From the DS
    final double dtSudPxDS = slaveTauU.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSudPyDS = slaveTauU.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSudPzDS = slaveTauU.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSudVxDS = slaveTauU.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSudVyDS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtSudVzDS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Difference
    final double d_dtSudPx = dtSudPxDS - dtSudPx;
    final double d_dtSudPy = dtSudPyDS - dtSudPy;
    final double d_dtSudPz = dtSudPzDS - dtSudPz;
    final double d_dtSudVx = dtSudVxDS - dtSudVx;
    final double d_dtSudVy = dtSudVyDS - dtSudVy;
    final double d_dtSudVz = dtSudVzDS - dtSudVz;
    // t2 derivatives / state
    // -----------------------
    // t2 = Time leg 2
    double dt2dPx = dtSudPx + dtMddPx;
    double dt2dPy = dtSudPy + dtMddPy;
    double dt2dPz = dtSudPz + dtMddPz;
    double dt2dVx = dtSudVx + dtMddVx;
    double dt2dVy = dtSudVy + dtMddVy;
    double dt2dVz = dtSudVz + dtMddVz;
    // With DS
    double dt2dPxDS = tauLeg2.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt2dPyDS = tauLeg2.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt2dPzDS = tauLeg2.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt2dVxDS = tauLeg2.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt2dVyDS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dt2dVzDS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Diff
    final double d_dt2dPx = dt2dPxDS - dt2dPx;
    final double d_dt2dPy = dt2dPyDS - dt2dPy;
    final double d_dt2dPz = dt2dPzDS - dt2dPz;
    final double d_dt2dVx = dt2dVxDS - dt2dVx;
    final double d_dt2dVy = dt2dVyDS - dt2dVy;
    final double d_dt2dVz = dt2dVzDS - dt2dVz;
    // tSd derivatives / state
    // -----------------------
    final Vector3D pos1 = state1.getPVCoordinates().getPosition();
    final Vector3D P1_QSt2 = QSt2.getPosition().subtract(pos1);
    final double dSDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSd - Vector3D.dotProduct(P1_QSt2, vel);
    // derivatives w/r to state
    final double alphaSd = 1. / dSDown * P1_QSt2.dotProduct(vel.subtract(QSt_V));
    final double dtSddPx = -1. / dSDown * P1_QSt2.getX() + alphaSd * dt2dPx;
    final double dtSddPy = -1. / dSDown * P1_QSt2.getY() + alphaSd * dt2dPy;
    final double dtSddPz = -1. / dSDown * P1_QSt2.getZ() + alphaSd * dt2dPz;
    final double dt2 = delta - t2 - tSd;
    final double dtSddVx = -dt2 / dSDown * P1_QSt2.getX() + alphaSd * dt2dVx;
    final double dtSddVy = -dt2 / dSDown * P1_QSt2.getY() + alphaSd * dt2dVy;
    final double dtSddVz = -dt2 / dSDown * P1_QSt2.getZ() + alphaSd * dt2dVz;
    // From the DS
    final double dtSddPxDS = slaveTauD.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSddPyDS = slaveTauD.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSddPzDS = slaveTauD.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSddVxDS = slaveTauD.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtSddVyDS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtSddVzDS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Difference
    final double d_dtSddPx = dtSddPxDS - dtSddPx;
    final double d_dtSddPy = dtSddPyDS - dtSddPy;
    final double d_dtSddPz = dtSddPzDS - dtSddPz;
    final double d_dtSddVx = dtSddVxDS - dtSddVx;
    final double d_dtSddVy = dtSddVyDS - dtSddVy;
    final double d_dtSddVz = dtSddVzDS - dtSddVz;
    // tMu derivatives / state
    // -----------------------
    // QMt1 = Master station position at t1 = t - tau = signal departure from master station
    final Transform FMt1 = masterGroundStation.getOffsetToInertial(state.getFrame(), measurementDate.shiftedBy(-t1 - t2));
    final PVCoordinates QMt1 = FMt1.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QMt1_P1 = pos1.subtract(QMt1.getPosition());
    final double dMUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMu - Vector3D.dotProduct(QMt1_P1, QMt_V);
    // derivatives w/r to state
    final double alphaMu = 1. / dMUp * QMt1_P1.dotProduct(QMt_V.subtract(vel));
    final double dtMudPx = 1. / dMUp * QMt1_P1.getX() + alphaMu * (dt2dPx + dtSddPx);
    final double dtMudPy = 1. / dMUp * QMt1_P1.getY() + alphaMu * (dt2dPy + dtSddPy);
    final double dtMudPz = 1. / dMUp * QMt1_P1.getZ() + alphaMu * (dt2dPz + dtSddPz);
    final double dtMudVx = dt2 / dMUp * QMt1_P1.getX() + alphaMu * (dt2dVx + dtSddVx);
    final double dtMudVy = dt2 / dMUp * QMt1_P1.getY() + alphaMu * (dt2dVy + dtSddVy);
    final double dtMudVz = dt2 / dMUp * QMt1_P1.getZ() + alphaMu * (dt2dVz + dtSddVz);
    // From the DS
    final double dtMudPxDS = masterTauU.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMudPyDS = masterTauU.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMudPzDS = masterTauU.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMudVxDS = masterTauU.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    final double dtMudVyDS = masterTauU.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    final double dtMudVzDS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Difference
    final double d_dtMudPx = dtMudPxDS - dtMudPx;
    final double d_dtMudPy = dtMudPyDS - dtMudPy;
    final double d_dtMudPz = dtMudPzDS - dtMudPz;
    final double d_dtMudVx = dtMudVxDS - dtMudVx;
    final double d_dtMudVy = dtMudVyDS - dtMudVy;
    final double d_dtMudVz = dtMudVzDS - dtMudVz;
    // t1 derivatives / state
    // -----------------------
    // t1 = Time leg 1
    double dt1dPx = dtSddPx + dtMudPx;
    double dt1dPy = dtSddPy + dtMudPy;
    double dt1dPz = dtSddPz + dtMudPz;
    double dt1dVx = dtSddVx + dtMudVx;
    double dt1dVy = dtSddVy + dtMudVy;
    double dt1dVz = dtSddVz + dtMudVz;
    // With DS
    double dt1dPxDS = tauLeg1.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt1dPyDS = tauLeg1.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt1dPzDS = tauLeg1.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt1dVxDS = tauLeg1.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dt1dVyDS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dt1dVzDS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0);
    // Diff
    final double d_dt1dPx = dt1dPxDS - dt1dPx;
    final double d_dt1dPy = dt1dPyDS - dt1dPy;
    final double d_dt1dPz = dt1dPzDS - dt1dPz;
    final double d_dt1dVx = dt1dVxDS - dt1dVx;
    final double d_dt1dVy = dt1dVyDS - dt1dVy;
    final double d_dt1dVz = dt1dVzDS - dt1dVz;
    // TAR derivatives / state
    // -----------------------
    // R = TAR
    double dRdPx = (dt1dPx + dt2dPx) * cOver2;
    double dRdPy = (dt1dPy + dt2dPy) * cOver2;
    double dRdPz = (dt1dPz + dt2dPz) * cOver2;
    double dRdVx = (dt1dVx + dt2dVx) * cOver2;
    double dRdVy = (dt1dVy + dt2dVy) * cOver2;
    double dRdVz = (dt1dVz + dt2dVz) * cOver2;
    // With DS
    double dRdPxDS = turnAroundRange.getPartialDerivative(1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdPyDS = turnAroundRange.getPartialDerivative(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdPzDS = turnAroundRange.getPartialDerivative(0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdVxDS = turnAroundRange.getPartialDerivative(0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0);
    double dRdVyDS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0);
    double dRdVzDS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 1, 0, 0, 0, 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;
    // tMd derivatives / stations
    // --------------------------
    // Master station rotation and angular speed at tmeas
    final AngularCoordinates acM = masterTopoToInert.getAngular().revert();
    final Rotation rotationMasterTopoToInert = acM.getRotation();
    final Vector3D OmegaM = acM.getRotationRate();
    // Slave station rotation and angular speed at tmeas
    final AngularCoordinates acS = slaveTopoToInert.getAngular().revert();
    final Rotation rotationSlaveTopoToInert = acS.getRotation();
    final Vector3D OmegaS = acS.getRotationRate();
    // Master station - Inertial frame
    final double dtMddQMx_I = P2_QMt.getX() / dMDown;
    final double dtMddQMy_I = P2_QMt.getY() / dMDown;
    final double dtMddQMz_I = P2_QMt.getZ() / dMDown;
    // Slave station - Inertial frame
    final double dtMddQSx_I = 0.;
    final double dtMddQSy_I = 0.;
    final double dtMddQSz_I = 0.;
    // Topo frames
    final Vector3D dtMddQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtMddQMx_I, dtMddQMy_I, dtMddQMz_I));
    final Vector3D dtMddQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtMddQSx_I, dtMddQSy_I, dtMddQSz_I));
    // With DS
    double dtMddQMx_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dtMddQMy_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dtMddQMz_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dtMddQSx_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtMddQSy_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtMddQSz_DS = masterTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtMddQMx = dtMddQMx_DS - dtMddQM.getX();
    final double d_dtMddQMy = dtMddQMy_DS - dtMddQM.getY();
    final double d_dtMddQMz = dtMddQMz_DS - dtMddQM.getZ();
    final double d_dtMddQSx = dtMddQSx_DS - dtMddQS.getX();
    final double d_dtMddQSy = dtMddQSy_DS - dtMddQS.getY();
    final double d_dtMddQSz = dtMddQSz_DS - dtMddQS.getZ();
    // tSu derivatives / stations
    // --------------------------
    // Master station - Inertial frame
    final double dtSudQMx_I = dtMddQMx_I * alphaSu;
    final double dtSudQMy_I = dtMddQMy_I * alphaSu;
    final double dtSudQMz_I = dtMddQMz_I * alphaSu;
    // Slave station - Inertial frame
    final double dtSudQSx_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_I.add(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
    final double dtSudQSy_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_J.add(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
    final double dtSudQSz_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_K.add(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
    // Topo frames
    final Vector3D dtSudQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtSudQMx_I, dtSudQMy_I, dtSudQMz_I));
    final Vector3D dtSudQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtSudQSx_I, dtSudQSy_I, dtSudQSz_I));
    // With DS
    double dtSudQMx_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dtSudQMy_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dtSudQMz_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dtSudQSx_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtSudQSy_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtSudQSz_DS = slaveTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtSudQMx = dtSudQMx_DS - dtSudQM.getX();
    final double d_dtSudQMy = dtSudQMy_DS - dtSudQM.getY();
    final double d_dtSudQMz = dtSudQMz_DS - dtSudQM.getZ();
    final double d_dtSudQSx = dtSudQSx_DS - dtSudQS.getX();
    final double d_dtSudQSy = dtSudQSy_DS - dtSudQS.getY();
    final double d_dtSudQSz = dtSudQSz_DS - dtSudQS.getZ();
    // t2 derivatives / stations
    // --------------------------
    final double dt2dQMx_I = dtMddQMx_I + dtSudQMx_I;
    final double dt2dQMy_I = dtMddQMy_I + dtSudQMy_I;
    final double dt2dQMz_I = dtMddQMz_I + dtSudQMz_I;
    final double dt2dQSx_I = dtMddQSx_I + dtSudQSx_I;
    final double dt2dQSy_I = dtMddQSy_I + dtSudQSy_I;
    final double dt2dQSz_I = dtMddQSz_I + dtSudQSz_I;
    final Vector3D dt2dQM = dtSudQM.add(dtMddQM);
    final Vector3D dt2dQS = dtSudQS.add(dtMddQS);
    // With DS
    double dt2dQMx_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dt2dQMy_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dt2dQMz_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dt2dQSx_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dt2dQSy_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dt2dQSz_DS = tauLeg2.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dt2dQMx = dt2dQMx_DS - dt2dQM.getX();
    final double d_dt2dQMy = dt2dQMy_DS - dt2dQM.getY();
    final double d_dt2dQMz = dt2dQMz_DS - dt2dQM.getZ();
    final double d_dt2dQSx = dt2dQSx_DS - dt2dQS.getX();
    final double d_dt2dQSy = dt2dQSy_DS - dt2dQS.getY();
    final double d_dt2dQSz = dt2dQSz_DS - dt2dQS.getZ();
    // tSd derivatives / stations
    // --------------------------
    // Master station - Inertial frame
    final double dtSddQMx_I = dt2dQMx_I * alphaSd;
    final double dtSddQMy_I = dt2dQMy_I * alphaSd;
    final double dtSddQMz_I = dt2dQMz_I * alphaSd;
    // Slave station - Inertial frame
    final double dtSddQSx_I = dt2dQSx_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_I.subtract(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
    final double dtSddQSy_I = dt2dQSy_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_J.subtract(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
    final double dtSddQSz_I = dt2dQSz_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_K.subtract(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
    // Topo frames
    final Vector3D dtSddQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtSddQMx_I, dtSddQMy_I, dtSddQMz_I));
    final Vector3D dtSddQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtSddQSx_I, dtSddQSy_I, dtSddQSz_I));
    // With DS
    double dtSddQMx_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dtSddQMy_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dtSddQMz_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dtSddQSx_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtSddQSy_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtSddQSz_DS = slaveTauD.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtSddQMx = dtSddQMx_DS - dtSddQM.getX();
    final double d_dtSddQMy = dtSddQMy_DS - dtSddQM.getY();
    final double d_dtSddQMz = dtSddQMz_DS - dtSddQM.getZ();
    final double d_dtSddQSx = dtSddQSx_DS - dtSddQS.getX();
    final double d_dtSddQSy = dtSddQSy_DS - dtSddQS.getY();
    final double d_dtSddQSz = dtSddQSz_DS - dtSddQS.getZ();
    // tMu derivatives / stations
    // --------------------------
    // Master station - Inertial frame
    final double dtMudQMx_I = -QMt1_P1.getX() / dMUp + alphaMu * (dt2dQMx_I + dtSddQMx_I) + t / dMUp * QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_I));
    final double dtMudQMy_I = -QMt1_P1.getY() / dMUp + alphaMu * (dt2dQMy_I + dtSddQMy_I) + t / dMUp * QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_J));
    final double dtMudQMz_I = -QMt1_P1.getZ() / dMUp + alphaMu * (dt2dQMz_I + dtSddQMz_I) + t / dMUp * QMt1_P1.dotProduct(OmegaM.crossProduct(Vector3D.PLUS_K));
    // Slave station - Inertial frame
    final double dtMudQSx_I = alphaMu * (dt2dQSx_I + dtSddQSx_I);
    final double dtMudQSy_I = alphaMu * (dt2dQSy_I + dtSddQSy_I);
    final double dtMudQSz_I = alphaMu * (dt2dQSz_I + dtSddQSz_I);
    // Topo frames
    final Vector3D dtMudQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtMudQMx_I, dtMudQMy_I, dtMudQMz_I));
    final Vector3D dtMudQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtMudQSx_I, dtMudQSy_I, dtMudQSz_I));
    // With DS
    double dtMudQMx_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dtMudQMy_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dtMudQMz_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dtMudQSx_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dtMudQSy_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dtMudQSz_DS = masterTauU.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dtMudQMx = dtMudQMx_DS - dtMudQM.getX();
    final double d_dtMudQMy = dtMudQMy_DS - dtMudQM.getY();
    final double d_dtMudQMz = dtMudQMz_DS - dtMudQM.getZ();
    final double d_dtMudQSx = dtMudQSx_DS - dtMudQS.getX();
    final double d_dtMudQSy = dtMudQSy_DS - dtMudQS.getY();
    final double d_dtMudQSz = dtMudQSz_DS - dtMudQS.getZ();
    // t1 derivatives / stations
    // --------------------------
    final Vector3D dt1dQM = dtMudQM.add(dtSddQM);
    final Vector3D dt1dQS = dtMudQS.add(dtSddQS);
    // With DS
    double dt1dQMx_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dt1dQMy_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dt1dQMz_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dt1dQSx_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dt1dQSy_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dt1dQSz_DS = tauLeg1.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dt1dQMx = dt1dQMx_DS - dt1dQM.getX();
    final double d_dt1dQMy = dt1dQMy_DS - dt1dQM.getY();
    final double d_dt1dQMz = dt1dQMz_DS - dt1dQM.getZ();
    final double d_dt1dQSx = dt1dQSx_DS - dt1dQS.getX();
    final double d_dt1dQSy = dt1dQSy_DS - dt1dQS.getY();
    final double d_dt1dQSz = dt1dQSz_DS - dt1dQS.getZ();
    // TAR derivatives / stations
    // --------------------------
    final Vector3D dRdQM = (dt1dQM.add(dt2dQM)).scalarMultiply(cOver2);
    final Vector3D dRdQS = (dt1dQS.add(dt2dQS)).scalarMultiply(cOver2);
    // With DS
    double dRdQMx_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0);
    double dRdQMy_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0);
    double dRdQMz_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0);
    double dRdQSx_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0);
    double dRdQSy_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0);
    double dRdQSz_DS = turnAroundRange.getPartialDerivative(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1);
    // Diff
    final double d_dRdQMx = dRdQMx_DS - dRdQM.getX();
    final double d_dRdQMy = dRdQMy_DS - dRdQM.getY();
    final double d_dRdQMz = dRdQMz_DS - dRdQM.getZ();
    final double d_dRdQSx = dRdQSx_DS - dRdQS.getX();
    final double d_dRdQSy = dRdQSy_DS - dRdQS.getY();
    final double d_dRdQSz = dRdQSz_DS - dRdQS.getZ();
    // Print results to avoid warning
    final boolean printResults = false;
    if (printResults) {
        System.out.println("dTAR = " + dTAR);
        System.out.println("d_dtMddPx = " + d_dtMddPx);
        System.out.println("d_dtMddPy = " + d_dtMddPy);
        System.out.println("d_dtMddPz = " + d_dtMddPz);
        System.out.println("d_dtMddVx = " + d_dtMddVx);
        System.out.println("d_dtMddVy = " + d_dtMddVy);
        System.out.println("d_dtMddVz = " + d_dtMddVz);
        System.out.println("d_dtSudPx = " + d_dtSudPx);
        System.out.println("d_dtSudPy = " + d_dtSudPy);
        System.out.println("d_dtSudPz = " + d_dtSudPz);
        System.out.println("d_dtSudVx = " + d_dtSudVx);
        System.out.println("d_dtSudVy = " + d_dtSudVy);
        System.out.println("d_dtSudVz = " + d_dtSudVz);
        System.out.println("d_dt2dPx = " + d_dt2dPx);
        System.out.println("d_dt2dPy = " + d_dt2dPy);
        System.out.println("d_dt2dPz = " + d_dt2dPz);
        System.out.println("d_dt2dVx = " + d_dt2dVx);
        System.out.println("d_dt2dVy = " + d_dt2dVy);
        System.out.println("d_dt2dVz = " + d_dt2dVz);
        System.out.println("d_dtSddPx = " + d_dtSddPx);
        System.out.println("d_dtSddPy = " + d_dtSddPy);
        System.out.println("d_dtSddPz = " + d_dtSddPz);
        System.out.println("d_dtSddVx = " + d_dtSddVx);
        System.out.println("d_dtSddVy = " + d_dtSddVy);
        System.out.println("d_dtSddVz = " + d_dtSddVz);
        System.out.println("d_dtMudPx = " + d_dtMudPx);
        System.out.println("d_dtMudPy = " + d_dtMudPy);
        System.out.println("d_dtMudPz = " + d_dtMudPz);
        System.out.println("d_dtMudVx = " + d_dtMudVx);
        System.out.println("d_dtMudVy = " + d_dtMudVy);
        System.out.println("d_dtMudVz = " + d_dtMudVz);
        System.out.println("d_dt1dPx = " + d_dt1dPx);
        System.out.println("d_dt1dPy = " + d_dt1dPy);
        System.out.println("d_dt1dPz = " + d_dt1dPz);
        System.out.println("d_dt1dVx = " + d_dt1dVx);
        System.out.println("d_dt1dVy = " + d_dt1dVy);
        System.out.println("d_dt1dVz = " + d_dt1dVz);
        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_dtMddQMx = " + d_dtMddQMx);
        System.out.println("d_dtMddQMy = " + d_dtMddQMy);
        System.out.println("d_dtMddQMz = " + d_dtMddQMz);
        System.out.println("d_dtMddQSx = " + d_dtMddQSx);
        System.out.println("d_dtMddQSy = " + d_dtMddQSy);
        System.out.println("d_dtMddQSz = " + d_dtMddQSz);
        System.out.println("d_dtSudQMx = " + d_dtSudQMx);
        System.out.println("d_dtSudQMy = " + d_dtSudQMy);
        System.out.println("d_dtSudQMz = " + d_dtSudQMz);
        System.out.println("d_dtSudQSx = " + d_dtSudQSx);
        System.out.println("d_dtSudQSy = " + d_dtSudQSy);
        System.out.println("d_dtSudQSz = " + d_dtSudQSz);
        System.out.println("d_dt2dQMx = " + d_dt2dQMx);
        System.out.println("d_dt2dQMy = " + d_dt2dQMy);
        System.out.println("d_dt2dQMz = " + d_dt2dQMz);
        System.out.println("d_dt2dQSx = " + d_dt2dQSx);
        System.out.println("d_dt2dQSy = " + d_dt2dQSy);
        System.out.println("d_dt2dQSz = " + d_dt2dQSz);
        System.out.println("d_dtSddQMx = " + d_dtSddQMx);
        System.out.println("d_dtSddQMy = " + d_dtSddQMy);
        System.out.println("d_dtSddQMz = " + d_dtSddQMz);
        System.out.println("d_dtSddQSx = " + d_dtSddQSx);
        System.out.println("d_dtSddQSy = " + d_dtSddQSy);
        System.out.println("d_dtSddQSz = " + d_dtSddQSz);
        System.out.println("d_dtMudQMx = " + d_dtMudQMx);
        System.out.println("d_dtMudQMy = " + d_dtMudQMy);
        System.out.println("d_dtMudQMz = " + d_dtMudQMz);
        System.out.println("d_dtMudQSx = " + d_dtMudQSx);
        System.out.println("d_dtMudQSy = " + d_dtMudQSy);
        System.out.println("d_dtMudQSz = " + d_dtMudQSz);
        System.out.println("d_dt1dQMx = " + d_dt1dQMx);
        System.out.println("d_dt1dQMy = " + d_dt1dQMy);
        System.out.println("d_dt1dQMz = " + d_dt1dQMz);
        System.out.println("d_dt1dQSx = " + d_dt1dQSx);
        System.out.println("d_dt1dQSy = " + d_dt1dQSy);
        System.out.println("d_dt1dQSz = " + d_dt1dQSz);
        System.out.println("d_dRdQMx = " + d_dRdQMx);
        System.out.println("d_dRdQMy = " + d_dRdQMy);
        System.out.println("d_dRdQMz = " + d_dRdQMz);
        System.out.println("d_dRdQSx = " + d_dRdQSx);
        System.out.println("d_dRdQSy = " + d_dRdQSy);
        System.out.println("d_dRdQSz = " + d_dRdQSz);
    }
    // Dummy return
    return estimated;
}

Example 3 with TimeStampedPVCoordinates

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

the class TurnAroundRangeAnalytic method theoreticalEvaluationAnalytic.

/**
 * Analytical version of the function theoreticalEvalution in TurnAroundRange class
 * The derivative structures are not used
 * For now only the value of turn-around range and not its derivatives are available
 * @param iteration
 * @param evaluation
 * @param initialState
 * @param state
 * @return
 * @throws OrekitException
 */
protected EstimatedMeasurement<TurnAroundRange> theoreticalEvaluationAnalytic(final int iteration, final int evaluation, final SpacecraftState initialState, final SpacecraftState state) throws OrekitException {
    // Stations attributes from parent Range class
    final GroundStation masterGroundStation = this.getMasterStation();
    final GroundStation slaveGroundStation = this.getSlaveStation();
    // Compute propagation times:
    // 
    // 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
    // --
    // Master station PV at measurement date
    final AbsoluteDate measurementDate = this.getDate();
    final Transform masterTopoToInert = masterGroundStation.getOffsetToInertial(state.getFrame(), measurementDate);
    final TimeStampedPVCoordinates masterArrival = masterTopoToInert.transformPVCoordinates(new TimeStampedPVCoordinates(measurementDate, PVCoordinates.ZERO));
    // Downlink time of flight from master station at t to spacecraft at t'
    final double tMd = signalTimeOfFlight(state.getPVCoordinates(), masterArrival.getPosition(), measurementDate);
    // 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, delta = masterTauD + slaveTauU)
    final double delta = getDate().durationFrom(state.getDate());
    // Transit state from which the satellite reflected the signal from slave to master station
    final SpacecraftState transitStateLeg2 = state.shiftedBy(delta - tMd);
    final AbsoluteDate transitDateLeg2 = transitStateLeg2.getDate();
    // Slave station PV at transit state leg2 date
    final Transform slaveTopoToInertTransitLeg2 = slaveGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg2);
    final TimeStampedPVCoordinates QSlaveTransitLeg2PV = slaveTopoToInertTransitLeg2.transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg2, PVCoordinates.ZERO));
    // Uplink time of flight from slave station to transit state leg2
    final double tSu = signalTimeOfFlight(QSlaveTransitLeg2PV, transitStateLeg2.getPVCoordinates().getPosition(), transitDateLeg2);
    // Total time of flight for leg 2
    final double t2 = tMd + tSu;
    // Compute propagation time for the 1st leg of the signal path
    // --
    // Absolute date of arrival of the signal to slave station
    final AbsoluteDate slaveStationArrivalDate = measurementDate.shiftedBy(-t2);
    // Slave station position in inertial frame at date slaveStationArrivalDate
    final Transform slaveTopoToInertArrivalDate = slaveGroundStation.getOffsetToInertial(state.getFrame(), slaveStationArrivalDate);
    final TimeStampedPVCoordinates slaveRebound = slaveTopoToInertArrivalDate.transformPVCoordinates(new TimeStampedPVCoordinates(slaveStationArrivalDate, PVCoordinates.ZERO));
    // Dowlink time of flight from transitStateLeg1 to slave station at slaveStationArrivalDate
    final double tSd = signalTimeOfFlight(transitStateLeg2.getPVCoordinates(), slaveRebound.getPosition(), slaveStationArrivalDate);
    // Transit state from which the satellite reflected the signal from master to slave station
    final SpacecraftState transitStateLeg1 = state.shiftedBy(delta - tMd - tSu - tSd);
    final AbsoluteDate transitDateLeg1 = transitStateLeg1.getDate();
    // Master station PV at transit state date of leg1
    final Transform masterTopoToInertTransitLeg1 = masterGroundStation.getOffsetToInertial(state.getFrame(), transitDateLeg1);
    final TimeStampedPVCoordinates QMasterTransitLeg1PV = masterTopoToInertTransitLeg1.transformPVCoordinates(new TimeStampedPVCoordinates(transitDateLeg1, PVCoordinates.ZERO));
    // Uplink time of flight from master station to transit state leg1
    final double tMu = signalTimeOfFlight(QMasterTransitLeg1PV, transitStateLeg1.getPVCoordinates().getPosition(), transitDateLeg1);
    final AbsoluteDate emissionDate = transitDateLeg1.shiftedBy(-tMu);
    final TimeStampedPVCoordinates masterDeparture = masterTopoToInertTransitLeg1.shiftedBy(emissionDate.durationFrom(masterTopoToInertTransitLeg1.getDate())).transformPVCoordinates(new TimeStampedPVCoordinates(emissionDate, PVCoordinates.ZERO));
    // Total time of flight for leg 1
    final double t1 = tSd + tMu;
    // Prepare the evaluation & evaluate
    // --
    // 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 arrival of the signal to 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(-tSu) }, new TimeStampedPVCoordinates[] { masterDeparture, transitStateLeg1.getPVCoordinates(), slaveRebound, transitStateLeg2.getPVCoordinates(), masterArrival });
    // Turn-around range value = Total time of flight for the 2 legs divided by 2
    final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
    final double tau = t1 + t2;
    estimated.setEstimatedValue(tau * cOver2);
    // TAR derivatives w/r state
    // -------------------------
    // tMd derivatives / state
    // -----------------------
    // QMt = Master station position at tmeas = t = signal arrival at master station
    final Vector3D vel = state.getPVCoordinates().getVelocity();
    final Transform FMt = masterGroundStation.getOffsetToInertial(state.getFrame(), getDate());
    final PVCoordinates QMt = FMt.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QMt_V = QMt.getVelocity();
    final Vector3D pos2 = transitStateLeg2.getPVCoordinates().getPosition();
    final Vector3D P2_QMt = QMt.getPosition().subtract(pos2);
    final double dMDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMd - Vector3D.dotProduct(P2_QMt, vel);
    // Derivatives w/r state
    final double dtMddPx = -P2_QMt.getX() / dMDown;
    final double dtMddPy = -P2_QMt.getY() / dMDown;
    final double dtMddPz = -P2_QMt.getZ() / dMDown;
    final double dt = delta - tMd;
    final double dtMddVx = dtMddPx * dt;
    final double dtMddVy = dtMddPy * dt;
    final double dtMddVz = dtMddPz * dt;
    // tSu derivatives / state
    // -----------------------
    // QSt = slave station position at tmeas = t = signal arrival at master station
    final Transform FSt = slaveGroundStation.getOffsetToInertial(state.getFrame(), getDate());
    final PVCoordinates QSt = FSt.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QSt_V = QSt.getVelocity();
    // QSt2 = slave station position at t-t2 = signal arrival at slave station
    final Transform FSt2 = slaveGroundStation.getOffsetToInertial(state.getFrame(), getDate().shiftedBy(-t2));
    final PVCoordinates QSt2 = FSt2.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QSt2_P2 = pos2.subtract(QSt2.getPosition());
    final double dSUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSu - Vector3D.dotProduct(QSt2_P2, QSt_V);
    // Derivatives w/r state
    final double alphaSu = 1. / dSUp * QSt2_P2.dotProduct(QSt_V.subtract(vel));
    final double dtSudPx = 1. / dSUp * QSt2_P2.getX() + alphaSu * dtMddPx;
    final double dtSudPy = 1. / dSUp * QSt2_P2.getY() + alphaSu * dtMddPy;
    final double dtSudPz = 1. / dSUp * QSt2_P2.getZ() + alphaSu * dtMddPz;
    final double dtSudVx = dtSudPx * dt;
    final double dtSudVy = dtSudPy * dt;
    final double dtSudVz = dtSudPz * dt;
    // t2 derivatives / state
    // -----------------------
    double dt2dPx = dtSudPx + dtMddPx;
    double dt2dPy = dtSudPy + dtMddPy;
    double dt2dPz = dtSudPz + dtMddPz;
    double dt2dVx = dtSudVx + dtMddVx;
    double dt2dVy = dtSudVy + dtMddVy;
    double dt2dVz = dtSudVz + dtMddVz;
    // tSd derivatives / state
    // -----------------------
    final Vector3D pos1 = transitStateLeg1.getPVCoordinates().getPosition();
    final Vector3D P1_QSt2 = QSt2.getPosition().subtract(pos1);
    final double dSDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tSd - Vector3D.dotProduct(P1_QSt2, vel);
    // derivatives w/r to state
    final double alphaSd = 1. / dSDown * P1_QSt2.dotProduct(vel.subtract(QSt_V));
    final double dtSddPx = -1. / dSDown * P1_QSt2.getX() + alphaSd * dt2dPx;
    final double dtSddPy = -1. / dSDown * P1_QSt2.getY() + alphaSd * dt2dPy;
    final double dtSddPz = -1. / dSDown * P1_QSt2.getZ() + alphaSd * dt2dPz;
    final double dt2 = delta - t2 - tSd;
    final double dtSddVx = -dt2 / dSDown * P1_QSt2.getX() + alphaSd * dt2dVx;
    final double dtSddVy = -dt2 / dSDown * P1_QSt2.getY() + alphaSd * dt2dVy;
    final double dtSddVz = -dt2 / dSDown * P1_QSt2.getZ() + alphaSd * dt2dVz;
    // tMu derivatives / state
    // -----------------------
    // QMt1 = Master station position at t1 = t - tau = signal departure from master station
    final Transform FMt1 = masterGroundStation.getOffsetToInertial(state.getFrame(), getDate().shiftedBy(-t1 - t2));
    final PVCoordinates QMt1 = FMt1.transformPVCoordinates(PVCoordinates.ZERO);
    final Vector3D QMt1_P1 = pos1.subtract(QMt1.getPosition());
    final double dMUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tMu - Vector3D.dotProduct(QMt1_P1, QMt_V);
    // derivatives w/r to state
    final double alphaMu = 1. / dMUp * QMt1_P1.dotProduct(QMt_V.subtract(vel));
    final double dtMudPx = 1. / dMUp * QMt1_P1.getX() + alphaMu * (dt2dPx + dtSddPx);
    final double dtMudPy = 1. / dMUp * QMt1_P1.getY() + alphaMu * (dt2dPy + dtSddPy);
    final double dtMudPz = 1. / dMUp * QMt1_P1.getZ() + alphaMu * (dt2dPz + dtSddPz);
    final double dtMudVx = dt2 / dMUp * QMt1_P1.getX() + alphaMu * (dt2dVx + dtSddVx);
    final double dtMudVy = dt2 / dMUp * QMt1_P1.getY() + alphaMu * (dt2dVy + dtSddVy);
    final double dtMudVz = dt2 / dMUp * QMt1_P1.getZ() + alphaMu * (dt2dVz + dtSddVz);
    // t1 derivatives / state
    // t1 = tauLeg1
    // -----------------------
    // t1 = Time leg 1
    double dt1dPx = dtSddPx + dtMudPx;
    double dt1dPy = dtSddPy + dtMudPy;
    double dt1dPz = dtSddPz + dtMudPz;
    double dt1dVx = dtSddVx + dtMudVx;
    double dt1dVy = dtSddVy + dtMudVy;
    double dt1dVz = dtSddVz + dtMudVz;
    // TAR derivatives / state
    // -----------------------
    // R = TAR
    double dRdPx = (dt1dPx + dt2dPx) * cOver2;
    double dRdPy = (dt1dPy + dt2dPy) * cOver2;
    double dRdPz = (dt1dPz + dt2dPz) * cOver2;
    double dRdVx = (dt1dVx + dt2dVx) * cOver2;
    double dRdVy = (dt1dVy + dt2dVy) * cOver2;
    double dRdVz = (dt1dVz + dt2dVz) * cOver2;
    estimated.setStateDerivatives(0, new double[] { // dROndP
    dRdPx, // dROndP
    dRdPy, // dROndP
    dRdPz, // dROndV
    dRdVx, // dROndV
    dRdVy, // dROndV
    dRdVz });
    if (masterGroundStation.getEastOffsetDriver().isSelected() || masterGroundStation.getNorthOffsetDriver().isSelected() || masterGroundStation.getZenithOffsetDriver().isSelected() || slaveGroundStation.getEastOffsetDriver().isSelected() || slaveGroundStation.getNorthOffsetDriver().isSelected() || slaveGroundStation.getZenithOffsetDriver().isSelected()) {
        // tMd derivatives / stations
        // --------------------------
        // Master station rotation and angular speed at tmeas
        final AngularCoordinates acM = FMt.getAngular().revert();
        final Rotation rotationMasterTopoToInert = acM.getRotation();
        final Vector3D OmegaM = acM.getRotationRate();
        // Slave station rotation and angular speed at tmeas
        final AngularCoordinates acS = FSt.getAngular().revert();
        final Rotation rotationSlaveTopoToInert = acS.getRotation();
        final Vector3D OmegaS = acS.getRotationRate();
        // Master station - Inertial frame
        final double dtMddQMx_I = P2_QMt.getX() / dMDown;
        final double dtMddQMy_I = P2_QMt.getY() / dMDown;
        final double dtMddQMz_I = P2_QMt.getZ() / dMDown;
        // Slave station - Inertial frame
        final double dtMddQSx_I = 0.;
        final double dtMddQSy_I = 0.;
        final double dtMddQSz_I = 0.;
        // Topo frames
        final Vector3D dtMddQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtMddQMx_I, dtMddQMy_I, dtMddQMz_I));
        final Vector3D dtMddQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtMddQSx_I, dtMddQSy_I, dtMddQSz_I));
        // tSu derivatives / stations
        // --------------------------
        // Master station - Inertial frame
        final double dtSudQMx_I = dtMddQMx_I * alphaSu;
        final double dtSudQMy_I = dtMddQMy_I * alphaSu;
        final double dtSudQMz_I = dtMddQMz_I * alphaSu;
        // Slave station - Inertial frame
        final double dtSudQSx_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_I.add(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
        final double dtSudQSy_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_J.add(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
        final double dtSudQSz_I = 1. / dSUp * QSt2_P2.dotProduct(Vector3D.MINUS_K.add(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
        // Topo frames
        final Vector3D dtSudQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtSudQMx_I, dtSudQMy_I, dtSudQMz_I));
        final Vector3D dtSudQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtSudQSx_I, dtSudQSy_I, dtSudQSz_I));
        // t2 = tauLeg2 derivatives / stations
        // --------------------------
        final double dt2dQSx_I = dtMddQSx_I + dtSudQSx_I;
        final double dt2dQSy_I = dtMddQSy_I + dtSudQSy_I;
        final double dt2dQSz_I = dtMddQSz_I + dtSudQSz_I;
        final double dt2dQMx_I = dtMddQMx_I + dtSudQMx_I;
        final double dt2dQMy_I = dtMddQMy_I + dtSudQMy_I;
        final double dt2dQMz_I = dtMddQMz_I + dtSudQMz_I;
        final Vector3D dt2dQM = dtSudQM.add(dtMddQM);
        final Vector3D dt2dQS = dtSudQS.add(dtMddQS);
        // tSd derivatives / stations
        // --------------------------
        // Master station - Inertial frame
        final double dtSddQMx_I = dt2dQMx_I * alphaSd;
        final double dtSddQMy_I = dt2dQMy_I * alphaSd;
        final double dtSddQMz_I = dt2dQMz_I * alphaSd;
        // Slave station - Inertial frame
        final double dtSddQSx_I = dt2dQSx_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_I.subtract(OmegaS.crossProduct(Vector3D.PLUS_I).scalarMultiply(t2)));
        final double dtSddQSy_I = dt2dQSy_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_J.subtract(OmegaS.crossProduct(Vector3D.PLUS_J).scalarMultiply(t2)));
        final double dtSddQSz_I = dt2dQSz_I * alphaSd + 1. / dSDown * P1_QSt2.dotProduct(Vector3D.PLUS_K.subtract(OmegaS.crossProduct(Vector3D.PLUS_K).scalarMultiply(t2)));
        // Topo frames
        final Vector3D dtSddQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtSddQMx_I, dtSddQMy_I, dtSddQMz_I));
        final Vector3D dtSddQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtSddQSx_I, dtSddQSy_I, dtSddQSz_I));
        // tMu derivatives / stations
        // --------------------------
        // Master station - Inertial frame
        final double dtMudQMx_I = alphaMu * (dt2dQMx_I + dtSddQMx_I) + 1 / dMUp * QMt1_P1.dotProduct(Vector3D.MINUS_I.add(OmegaM.crossProduct(Vector3D.PLUS_I).scalarMultiply(tau)));
        final double dtMudQMy_I = alphaMu * (dt2dQMy_I + dtSddQMy_I) + 1 / dMUp * QMt1_P1.dotProduct(Vector3D.MINUS_J.add(OmegaM.crossProduct(Vector3D.PLUS_J).scalarMultiply(tau)));
        final double dtMudQMz_I = alphaMu * (dt2dQMz_I + dtSddQMz_I) + 1 / dMUp * QMt1_P1.dotProduct(Vector3D.MINUS_K.add(OmegaM.crossProduct(Vector3D.PLUS_K).scalarMultiply(tau)));
        // Slave station - Inertial frame
        final double dtMudQSx_I = alphaMu * (dt2dQSx_I + dtSddQSx_I);
        final double dtMudQSy_I = alphaMu * (dt2dQSy_I + dtSddQSy_I);
        final double dtMudQSz_I = alphaMu * (dt2dQSz_I + dtSddQSz_I);
        // Topo frames
        final Vector3D dtMudQM = rotationMasterTopoToInert.applyTo(new Vector3D(dtMudQMx_I, dtMudQMy_I, dtMudQMz_I));
        final Vector3D dtMudQS = rotationSlaveTopoToInert.applyTo(new Vector3D(dtMudQSx_I, dtMudQSy_I, dtMudQSz_I));
        // t1 derivatives / stations
        // --------------------------
        final Vector3D dt1dQM = dtMudQM.add(dtSddQM);
        final Vector3D dt1dQS = dtMudQS.add(dtSddQS);
        // TAR derivatives / stations
        // --------------------------
        final Vector3D dRdQM = (dt1dQM.add(dt2dQM)).scalarMultiply(cOver2);
        final Vector3D dRdQS = (dt1dQS.add(dt2dQS)).scalarMultiply(cOver2);
        // Master station drivers
        if (masterGroundStation.getEastOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(masterGroundStation.getEastOffsetDriver(), dRdQM.getX());
        }
        if (masterGroundStation.getNorthOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(masterGroundStation.getNorthOffsetDriver(), dRdQM.getY());
        }
        if (masterGroundStation.getZenithOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(masterGroundStation.getZenithOffsetDriver(), dRdQM.getZ());
        }
        // Slave station drivers
        if (slaveGroundStation.getEastOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(slaveGroundStation.getEastOffsetDriver(), dRdQS.getX());
        }
        if (slaveGroundStation.getNorthOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(slaveGroundStation.getNorthOffsetDriver(), dRdQS.getY());
        }
        if (slaveGroundStation.getZenithOffsetDriver().isSelected()) {
            estimated.setParameterDerivatives(slaveGroundStation.getZenithOffsetDriver(), dRdQS.getZ());
        }
    }
    return estimated;
}

Example 4 with TimeStampedPVCoordinates

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

the class TurnAroundRangeMeasurementCreator method handleStep.

/**
 * Function handling the steps of the propagator
 * A turn-around measurement needs 2 stations, a master and a slave
 * The measurement is a signal:
 * - Emitted from the master ground station
 * - Reflected on the spacecraft
 * - Reflected on the slave ground station
 * - Reflected on the spacecraft again
 * - Received on the master ground station
 * Its value is the elapsed time between emission and reception
 * divided by 2c were c is the speed of light.
 *
 * The path of the signal is divided into 2 legs:
 *  - The 1st leg goes from emission by the master station to reception by the slave station
 *  - The 2nd leg goes from emission by the slave station to reception by the master station
 *
 * The spacecraft state date should, after a few iterations of the estimation process, be
 * set to the date of arrival/departure of the signal to/from the slave station.
 * It is guaranteed by implementation of the estimated measurement.
 * This is done to avoid big shifts in time to compute the transit states.
 * See TurnAroundRange.java for more
 * Thus the spacecraft date is the date when the 1st leg of the path ends and the 2nd leg begins
 */
public void handleStep(final SpacecraftState currentState, final boolean isLast) throws OrekitException {
    try {
        for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
            final GroundStation masterStation = entry.getKey();
            final GroundStation slaveStation = entry.getValue();
            final AbsoluteDate date = currentState.getDate();
            final Frame inertial = currentState.getFrame();
            final Vector3D position = currentState.toTransform().getInverse().transformPosition(antennaPhaseCenter);
            // Create a TAR measurement only if elevation for both stations is higher than elevationMin°
            if ((masterStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0)) && (slaveStation.getBaseFrame().getElevation(position, inertial, date) > FastMath.toRadians(30.0))) {
                // The solver used
                final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
                // Spacecraft date t = date of arrival/departure of the signal to/from from the slave station
                // Slave station position in inertial frame at t
                final Vector3D slaveStationPosition = slaveStation.getOffsetToInertial(inertial, date).transformPosition(Vector3D.ZERO);
                // Downlink time of flight to slave station
                // The date of arrival/departure of the signal to/from the slave station is known and
                // equal to spacecraft date t.
                // Therefore we can use the function "downlinkTimeOfFlight" from GroundStation class
                // final double slaveTauD = slaveStation.downlinkTimeOfFlight(currentState, date);
                final double slaveTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(-x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Uplink time of flight from slave station
                // A solver is used to know where the satellite is when it receives the signal
                // back from the slave station
                final double slaveTauU = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        final SpacecraftState transitState = currentState.shiftedBy(+x);
                        final double d = Vector3D.distance(transitState.toTransform().getInverse().transformPosition(antennaPhaseCenter), slaveStationPosition);
                        return d - x * Constants.SPEED_OF_LIGHT;
                    }
                }, -1.0, 1.0);
                // Find the position of the master station at signal departure and arrival
                // ----
                // Transit state position & date for the 1st leg of the signal path
                final SpacecraftState S1 = currentState.shiftedBy(-slaveTauD);
                final Vector3D P1 = S1.toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T1 = date.shiftedBy(-slaveTauD);
                // Transit state position & date for the 2nd leg of the signal path
                final Vector3D P2 = currentState.shiftedBy(+slaveTauU).toTransform().getInverse().transformPosition(antennaPhaseCenter);
                final AbsoluteDate T2 = date.shiftedBy(+slaveTauU);
                // Master station downlink delay - from P2 to master station
                // We use a solver to know where the master station is when it receives
                // the signal back from the satellite on the 2nd leg of the path
                final double masterTauD = solver.solve(1000, new UnivariateFunction() {

                    public double value(final double x) throws OrekitExceptionWrapper {
                        try {
                            final Transform t = masterStation.getOffsetToInertial(inertial, T2.shiftedBy(+x));
                            final double d = Vector3D.distance(P2, t.transformPosition(Vector3D.ZERO));
                            return d - x * Constants.SPEED_OF_LIGHT;
                        } catch (OrekitException oe) {
                            throw new OrekitExceptionWrapper(oe);
                        }
                    }
                }, -1.0, 1.0);
                final AbsoluteDate masterReceptionDate = T2.shiftedBy(+masterTauD);
                final TimeStampedPVCoordinates masterStationAtReception = masterStation.getOffsetToInertial(inertial, masterReceptionDate).transformPVCoordinates(new TimeStampedPVCoordinates(masterReceptionDate, PVCoordinates.ZERO));
                // Master station uplink delay - from master station to P1
                // Here the state date is known. Thus we can use the function "signalTimeOfFlight"
                // of the AbstractMeasurement class
                final double masterTauU = AbstractMeasurement.signalTimeOfFlight(masterStationAtReception, P1, T1);
                final AbsoluteDate masterEmissionDate = T1.shiftedBy(-masterTauU);
                final Vector3D masterStationAtEmission = masterStation.getOffsetToInertial(inertial, masterEmissionDate).transformPosition(Vector3D.ZERO);
                // Uplink/downlink distance from/to slave station
                final double slaveDownLinkDistance = Vector3D.distance(P1, slaveStationPosition);
                final double slaveUpLinkDistance = Vector3D.distance(P2, slaveStationPosition);
                // Uplink/downlink distance from/to master station
                final double masterUpLinkDistance = Vector3D.distance(P1, masterStationAtEmission);
                final double masterDownLinkDistance = Vector3D.distance(P2, masterStationAtReception.getPosition());
                addMeasurement(new TurnAroundRange(masterStation, slaveStation, masterReceptionDate, 0.5 * (masterUpLinkDistance + slaveDownLinkDistance + slaveUpLinkDistance + masterDownLinkDistance), 1.0, 10));
            }
        }
    } catch (OrekitExceptionWrapper oew) {
        throw new OrekitException(oew.getException());
    } catch (OrekitException oe) {
        throw new OrekitException(oe);
    }
}

Example 5 with TimeStampedPVCoordinates

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

the class TurnAroundRangeTest method genericTestValues.

/**
 * Generic test function for values of the TAR
 * @param printResults Print the results ?
 * @throws OrekitException
 */
void genericTestValues(final boolean printResults) throws OrekitException {
    Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
    // Context context = EstimationTestUtils.geoStationnaryContext();
    final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.0e-6, 60.0, 0.001);
    // create perfect range measurements
    final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
    final List<ObservedMeasurement<?>> measurements = EstimationTestUtils.createMeasurements(propagator, new TurnAroundRangeMeasurementCreator(context), 1.0, 3.0, 300.0);
    propagator.setSlaveMode();
    double[] absoluteErrors = new double[measurements.size()];
    double[] relativeErrors = new double[measurements.size()];
    int index = 0;
    // Print the results ? Header
    if (printResults) {
        System.out.format(Locale.US, "%-15s  %-15s  %-23s  %-23s  %17s  %17s  %13s %13s%n", "Master Station", "Slave Station", "Measurement Date", "State Date", "TAR observed [m]", "TAR estimated [m]", "|ΔTAR| [m]", "rel |ΔTAR|");
    }
    // Loop on the measurements
    for (final ObservedMeasurement<?> measurement : measurements) {
        final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
        final AbsoluteDate date = measurement.getDate().shiftedBy(meanDelay);
        final SpacecraftState state = propagator.propagate(date);
        // Values of the TAR & errors
        final double TARobserved = measurement.getObservedValue()[0];
        final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
        final double TARestimated = estimated.getEstimatedValue()[0];
        final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
        Assert.assertEquals(5, participants.length);
        Assert.assertEquals(0.5 * Constants.SPEED_OF_LIGHT * participants[4].getDate().durationFrom(participants[0].getDate()), estimated.getEstimatedValue()[0], 2.0e-8);
        absoluteErrors[index] = TARestimated - TARobserved;
        relativeErrors[index] = FastMath.abs(absoluteErrors[index]) / FastMath.abs(TARobserved);
        index++;
        // Print results ? Values
        if (printResults) {
            final AbsoluteDate measurementDate = measurement.getDate();
            String masterStationName = ((TurnAroundRange) measurement).getMasterStation().getBaseFrame().getName();
            String slaveStationName = ((TurnAroundRange) measurement).getSlaveStation().getBaseFrame().getName();
            System.out.format(Locale.US, "%-15s  %-15s  %-23s  %-23s  %17.6f  %17.6f  %13.6e %13.6e%n", masterStationName, slaveStationName, measurementDate, date, TARobserved, TARestimated, FastMath.abs(TARestimated - TARobserved), FastMath.abs((TARestimated - TARobserved) / TARobserved));
        }
    }
    // Compute some statistics
    final double absErrorsMedian = new Median().evaluate(absoluteErrors);
    final double absErrorsMin = new Min().evaluate(absoluteErrors);
    final double absErrorsMax = new Max().evaluate(absoluteErrors);
    final double relErrorsMedian = new Median().evaluate(relativeErrors);
    final double relErrorsMax = new Max().evaluate(relativeErrors);
    // Print the results on console ? Final results
    if (printResults) {
        System.out.println();
        System.out.println("Absolute errors median: " + absErrorsMedian);
        System.out.println("Absolute errors min   : " + absErrorsMin);
        System.out.println("Absolute errors max   : " + absErrorsMax);
        System.out.println("Relative errors median: " + relErrorsMedian);
        System.out.println("Relative errors max   : " + relErrorsMax);
    }
    // Assert statistical errors
    Assert.assertEquals(0.0, absErrorsMedian, 1.4e-7);
    Assert.assertEquals(0.0, absErrorsMin, 5.0e-7);
    Assert.assertEquals(0.0, absErrorsMax, 4.9e-7);
    Assert.assertEquals(0.0, relErrorsMedian, 8.9e-15);
    Assert.assertEquals(0.0, relErrorsMax, 2.9e-14);
}