Example 1 with AngularCoordinates
use of org.orekit.utils.AngularCoordinates 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;
}
Also used :
HashMap(java.util.HashMap)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
PVCoordinates(org.orekit.utils.PVCoordinates)
TimeStampedFieldPVCoordinates(org.orekit.utils.TimeStampedFieldPVCoordinates)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
AbsoluteDate(org.orekit.time.AbsoluteDate)
SpacecraftState(org.orekit.propagation.SpacecraftState)
Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D)
FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D)
AngularCoordinates(org.orekit.utils.AngularCoordinates)
DerivativeStructure(org.hipparchus.analysis.differentiation.DerivativeStructure)
DSFactory(org.hipparchus.analysis.differentiation.DSFactory)
ParameterDriver(org.orekit.utils.ParameterDriver)
Rotation(org.hipparchus.geometry.euclidean.threed.Rotation)
Transform(org.orekit.frames.Transform)
FieldTransform(org.orekit.frames.FieldTransform)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
Example 2 with AngularCoordinates
use of org.orekit.utils.AngularCoordinates 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;
}
Also used :
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
PVCoordinates(org.orekit.utils.PVCoordinates)
TimeStampedFieldPVCoordinates(org.orekit.utils.TimeStampedFieldPVCoordinates)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
Rotation(org.hipparchus.geometry.euclidean.threed.Rotation)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
AbsoluteDate(org.orekit.time.AbsoluteDate)
SpacecraftState(org.orekit.propagation.SpacecraftState)
Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D)
FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D)
AngularCoordinates(org.orekit.utils.AngularCoordinates)
Transform(org.orekit.frames.Transform)
FieldTransform(org.orekit.frames.FieldTransform)
Example 3 with AngularCoordinates
use of org.orekit.utils.AngularCoordinates in project Orekit by CS-SI.
the class RangeAnalytic method theoreticalEvaluationAnalytic.
/**
* Analytical version of the theoreticalEvaluation function in Range class
* The derivative structures are not used, an analytical computation is used instead.
* @param iteration current LS estimator iteration
* @param evaluation current LS estimator evaluation
* @param state spacecraft state. At measurement date on first iteration then close to emission date on further iterations
* @param interpolator Orekit step interpolator
* @return
* @throws OrekitException
*/
protected EstimatedMeasurement<Range> theoreticalEvaluationAnalytic(final int iteration, final int evaluation, final SpacecraftState state) throws OrekitException {
// Station attribute from parent Range class
final GroundStation groundStation = this.getStation();
// Station position at signal arrival
final AbsoluteDate downlinkDate = getDate();
final Transform topoToInertDownlink = groundStation.getOffsetToInertial(state.getFrame(), downlinkDate);
final TimeStampedPVCoordinates stationDownlink = topoToInertDownlink.transformPVCoordinates(new TimeStampedPVCoordinates(downlinkDate, PVCoordinates.ZERO));
// Take propagation time into account
// (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 time of flight
final double tauD = signalTimeOfFlight(state.getPVCoordinates(), stationDownlink.getPosition(), downlinkDate);
final double delta = downlinkDate.durationFrom(state.getDate());
final double dt = delta - tauD;
// Transit state position
final SpacecraftState transitState = state.shiftedBy(dt);
final AbsoluteDate transitDate = transitState.getDate();
final Vector3D transitP = transitState.getPVCoordinates().getPosition();
// Station position at transit state date
final Transform topoToInertAtTransitDate = groundStation.getOffsetToInertial(state.getFrame(), transitDate);
final TimeStampedPVCoordinates stationAtTransitDate = topoToInertAtTransitDate.transformPVCoordinates(new TimeStampedPVCoordinates(transitDate, PVCoordinates.ZERO));
// Uplink time of flight
final double tauU = signalTimeOfFlight(stationAtTransitDate, transitP, transitDate);
final double tau = tauD + tauU;
// Real date and position of station at signal departure
final AbsoluteDate uplinkDate = downlinkDate.shiftedBy(-tau);
final TimeStampedPVCoordinates stationUplink = topoToInertDownlink.shiftedBy(-tau).transformPVCoordinates(new TimeStampedPVCoordinates(uplinkDate, PVCoordinates.ZERO));
// Prepare the evaluation
final EstimatedMeasurement<Range> estimated = new EstimatedMeasurement<Range>(this, iteration, evaluation, new SpacecraftState[] { transitState }, new TimeStampedPVCoordinates[] { stationUplink, transitState.getPVCoordinates(), stationDownlink });
// Set range value
final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT;
estimated.setEstimatedValue(tau * cOver2);
// Partial derivatives with respect to state
// The formulas below take into account the fact the measurement is at fixed reception date.
// When spacecraft position is changed, the downlink delay is changed, and in order
// to still have the measurement arrive at exactly the same date on ground, we must
// take the spacecraft-station relative velocity into account.
final Vector3D v = state.getPVCoordinates().getVelocity();
final Vector3D qv = stationDownlink.getVelocity();
final Vector3D downInert = stationDownlink.getPosition().subtract(transitP);
final double dDown = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauD - Vector3D.dotProduct(downInert, v);
final Vector3D upInert = transitP.subtract(stationUplink.getPosition());
// test
// final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU -
// Vector3D.dotProduct(upInert, qv);
// test
final double dUp = Constants.SPEED_OF_LIGHT * Constants.SPEED_OF_LIGHT * tauU - Vector3D.dotProduct(upInert, stationUplink.getVelocity());
// derivatives of the downlink time of flight
final double dTauDdPx = -downInert.getX() / dDown;
final double dTauDdPy = -downInert.getY() / dDown;
final double dTauDdPz = -downInert.getZ() / dDown;
// Derivatives of the uplink time of flight
final double dTauUdPx = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdPx)));
final double dTauUdPy = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdPy)));
final double dTauUdPz = 1. / dUp * upInert.dotProduct(Vector3D.PLUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdPz)));
// derivatives of the range measurement
final double dRdPx = (dTauDdPx + dTauUdPx) * cOver2;
final double dRdPy = (dTauDdPy + dTauUdPy) * cOver2;
final double dRdPz = (dTauDdPz + dTauUdPz) * cOver2;
estimated.setStateDerivatives(0, new double[] { dRdPx, dRdPy, dRdPz, dRdPx * dt, dRdPy * dt, dRdPz * dt });
if (groundStation.getEastOffsetDriver().isSelected() || groundStation.getNorthOffsetDriver().isSelected() || groundStation.getZenithOffsetDriver().isSelected()) {
// Downlink tme of flight derivatives / station position in topocentric frame
final AngularCoordinates ac = topoToInertDownlink.getAngular().revert();
// final Rotation rotTopoToInert = ac.getRotation();
final Vector3D omega = ac.getRotationRate();
// Inertial frame
final double dTauDdQIx = downInert.getX() / dDown;
final double dTauDdQIy = downInert.getY() / dDown;
final double dTauDdQIz = downInert.getZ() / dDown;
// Uplink tme of flight derivatives / station position in topocentric frame
// Inertial frame
final double dTauUdQIx = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_I.add((qv.subtract(v)).scalarMultiply(dTauDdQIx)).subtract(Vector3D.PLUS_I.crossProduct(omega).scalarMultiply(tau)));
final double dTauUdQIy = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_J.add((qv.subtract(v)).scalarMultiply(dTauDdQIy)).subtract(Vector3D.PLUS_J.crossProduct(omega).scalarMultiply(tau)));
final double dTauUdQIz = 1 / dUp * upInert.dotProduct(Vector3D.MINUS_K.add((qv.subtract(v)).scalarMultiply(dTauDdQIz)).subtract(Vector3D.PLUS_K.crossProduct(omega).scalarMultiply(tau)));
// Range partial derivatives
// with respect to station position in inertial frame
final Vector3D dRdQI = new Vector3D((dTauDdQIx + dTauUdQIx) * cOver2, (dTauDdQIy + dTauUdQIy) * cOver2, (dTauDdQIz + dTauUdQIz) * cOver2);
// convert to topocentric frame, as the station position
// offset parameter is expressed in this frame
final Vector3D dRdQT = ac.getRotation().applyTo(dRdQI);
if (groundStation.getEastOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getEastOffsetDriver(), dRdQT.getX());
}
if (groundStation.getNorthOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getNorthOffsetDriver(), dRdQT.getY());
}
if (groundStation.getZenithOffsetDriver().isSelected()) {
estimated.setParameterDerivatives(groundStation.getZenithOffsetDriver(), dRdQT.getZ());
}
}
return estimated;
}
Also used :
SpacecraftState(org.orekit.propagation.SpacecraftState)
Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D)
FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D)
AngularCoordinates(org.orekit.utils.AngularCoordinates)
Transform(org.orekit.frames.Transform)
FieldTransform(org.orekit.frames.FieldTransform)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
AbsoluteDate(org.orekit.time.AbsoluteDate)
Example 4 with AngularCoordinates
use of org.orekit.utils.AngularCoordinates in project Orekit by CS-SI.
the class GroundPointing method getAttitude.
/**
* {@inheritDoc}
*/
public Attitude getAttitude(final PVCoordinatesProvider pvProv, final AbsoluteDate date, final Frame frame) throws OrekitException {
// satellite-target relative vector
final PVCoordinates pva = pvProv.getPVCoordinates(date, inertialFrame);
final TimeStampedPVCoordinates delta = new TimeStampedPVCoordinates(date, pva, getTargetPV(pvProv, date, inertialFrame));
// spacecraft and target should be away from each other to define a pointing direction
if (delta.getPosition().getNorm() == 0.0) {
throw new OrekitException(OrekitMessages.SATELLITE_COLLIDED_WITH_TARGET);
}
// attitude definition:
// line of sight -> +z satellite axis,
// orbital velocity -> (z, +x) half plane
final Vector3D p = pva.getPosition();
final Vector3D v = pva.getVelocity();
final Vector3D a = pva.getAcceleration();
final double r2 = p.getNormSq();
final double r = FastMath.sqrt(r2);
final Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
final PVCoordinates velocity = new PVCoordinates(v, a, keplerianJerk);
final PVCoordinates los = delta.normalize();
final PVCoordinates normal = PVCoordinates.crossProduct(delta, velocity).normalize();
AngularCoordinates ac = new AngularCoordinates(los, normal, PLUS_K, PLUS_J, 1.0e-9);
if (frame != inertialFrame) {
// prepend transform from specified frame to inertial frame
ac = ac.addOffset(frame.getTransformTo(inertialFrame, date).getAngular());
}
// build the attitude
return new Attitude(date, frame, ac);
}
Also used :
Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D)
FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D)
AngularCoordinates(org.orekit.utils.AngularCoordinates)
FieldAngularCoordinates(org.orekit.utils.FieldAngularCoordinates)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
FieldPVCoordinates(org.orekit.utils.FieldPVCoordinates)
PVCoordinates(org.orekit.utils.PVCoordinates)
TimeStampedFieldPVCoordinates(org.orekit.utils.TimeStampedFieldPVCoordinates)
OrekitException(org.orekit.errors.OrekitException)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
Example 5 with AngularCoordinates
use of org.orekit.utils.AngularCoordinates 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;
}
Also used :
HashMap(java.util.HashMap)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
PVCoordinates(org.orekit.utils.PVCoordinates)
TimeStampedFieldPVCoordinates(org.orekit.utils.TimeStampedFieldPVCoordinates)
TimeStampedPVCoordinates(org.orekit.utils.TimeStampedPVCoordinates)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
AbsoluteDate(org.orekit.time.AbsoluteDate)
SpacecraftState(org.orekit.propagation.SpacecraftState)
Vector3D(org.hipparchus.geometry.euclidean.threed.Vector3D)
FieldVector3D(org.hipparchus.geometry.euclidean.threed.FieldVector3D)
AngularCoordinates(org.orekit.utils.AngularCoordinates)
DerivativeStructure(org.hipparchus.analysis.differentiation.DerivativeStructure)
DSFactory(org.hipparchus.analysis.differentiation.DSFactory)
ParameterDriver(org.orekit.utils.ParameterDriver)
Rotation(org.hipparchus.geometry.euclidean.threed.Rotation)
Transform(org.orekit.frames.Transform)
FieldTransform(org.orekit.frames.FieldTransform)
FieldAbsoluteDate(org.orekit.time.FieldAbsoluteDate)
Aggregations
AngularCoordinates (org.orekit.utils.AngularCoordinates)7
FieldVector3D (org.hipparchus.geometry.euclidean.threed.FieldVector3D)5
Rotation (org.hipparchus.geometry.euclidean.threed.Rotation)5
Vector3D (org.hipparchus.geometry.euclidean.threed.Vector3D)5
FieldAbsoluteDate (org.orekit.time.FieldAbsoluteDate)5
PVCoordinates (org.orekit.utils.PVCoordinates)5
TimeStampedPVCoordinates (org.orekit.utils.TimeStampedPVCoordinates)5
FieldTransform (org.orekit.frames.FieldTransform)4
Transform (org.orekit.frames.Transform)4
SpacecraftState (org.orekit.propagation.SpacecraftState)4
AbsoluteDate (org.orekit.time.AbsoluteDate)4
TimeStampedFieldPVCoordinates (org.orekit.utils.TimeStampedFieldPVCoordinates)4
DSFactory (org.hipparchus.analysis.differentiation.DSFactory)3
DerivativeStructure (org.hipparchus.analysis.differentiation.DerivativeStructure)3
HashMap (java.util.HashMap)2
OrekitException (org.orekit.errors.OrekitException)2
ParameterDriver (org.orekit.utils.ParameterDriver)2
UnivariateVectorFunction (org.hipparchus.analysis.UnivariateVectorFunction)1
FiniteDifferencesDifferentiator (org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator)1
UnivariateDifferentiableVectorFunction (org.hipparchus.analysis.differentiation.UnivariateDifferentiableVectorFunction)1
