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

Example 86 with TimeStampedPVCoordinates

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

the class RangeRate method theoreticalEvaluation.

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

Example 87 with TimeStampedPVCoordinates

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

the class TurnAroundRange method theoreticalEvaluation.

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

Example 88 with TimeStampedPVCoordinates

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

the class OnBoardAntennaInterSatellitesRangeModifier method modifyOneWay.

/**
 * Apply a modifier to an estimated measurement in the one-way case.
 * @param estimated estimated measurement to modify
 */
private void modifyOneWay(final EstimatedMeasurement<InterSatellitesRange> estimated) {
    // the participants are satellite 2 at emission, satellite 1 at reception
    final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
    final AbsoluteDate emissionDate = participants[0].getDate();
    final AbsoluteDate receptionDate = participants[1].getDate();
    // transforms from spacecraft to inertial frame at emission/reception dates
    final SpacecraftState refState1 = estimated.getStates()[0];
    final SpacecraftState receptionState = refState1.shiftedBy(receptionDate.durationFrom(refState1.getDate()));
    final Transform receptionSpacecraftToInert = receptionState.toTransform().getInverse();
    final SpacecraftState refState2 = estimated.getStates()[1];
    final SpacecraftState emissionState = refState2.shiftedBy(emissionDate.durationFrom(refState2.getDate()));
    final Transform emissionSpacecraftToInert = emissionState.toTransform().getInverse();
    // compute the geometrical value of the inter-satellites range directly from participants positions.
    // Note that this may be different from the value returned by estimated.getEstimatedValue(),
    // because other modifiers may already have been taken into account
    final Vector3D pSpacecraftReception = receptionSpacecraftToInert.transformPosition(Vector3D.ZERO);
    final Vector3D pSpacecraftEmission = emissionSpacecraftToInert.transformPosition(Vector3D.ZERO);
    final double interSatellitesRangeUsingSpacecraftCenter = Vector3D.distance(pSpacecraftEmission, pSpacecraftReception);
    // compute the geometrical value of the range replacing
    // the spacecraft positions with antenna phase center positions
    final Vector3D pAPCReception = receptionSpacecraftToInert.transformPosition(antennaPhaseCenter1);
    final Vector3D pAPCEmission = emissionSpacecraftToInert.transformPosition(antennaPhaseCenter2);
    final double interSatellitesRangeUsingAntennaPhaseCenter = Vector3D.distance(pAPCEmission, pAPCReception);
    // get the estimated value before this modifier is applied
    final double[] value = estimated.getEstimatedValue();
    // modify the value
    value[0] += interSatellitesRangeUsingAntennaPhaseCenter - interSatellitesRangeUsingSpacecraftCenter;
    estimated.setEstimatedValue(value);
}

Example 89 with TimeStampedPVCoordinates

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

the class OnBoardAntennaTurnAroundRangeModifier method modify.

/**
 * {@inheritDoc}
 */
@Override
public void modify(final EstimatedMeasurement<TurnAroundRange> estimated) {
    // the participants are master station at emission, spacecraft during leg 1,
    // slave station at rebound, spacecraft during leg 2, master station at reception
    final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
    final Vector3D pMasterEmission = participants[0].getPosition();
    final AbsoluteDate transitDateLeg1 = participants[1].getDate();
    final Vector3D pSlaveRebound = participants[2].getPosition();
    final AbsoluteDate transitDateLeg2 = participants[3].getDate();
    final Vector3D pMasterReception = participants[4].getPosition();
    // transforms from spacecraft to inertial frame at transit dates
    final SpacecraftState refState = estimated.getStates()[0];
    final SpacecraftState transitStateLeg1 = refState.shiftedBy(transitDateLeg1.durationFrom(refState.getDate()));
    final Transform spacecraftToInertLeg1 = transitStateLeg1.toTransform().getInverse();
    final SpacecraftState transitStateLeg2 = refState.shiftedBy(transitDateLeg2.durationFrom(refState.getDate()));
    final Transform spacecraftToInertLeg2 = transitStateLeg2.toTransform().getInverse();
    // compute the geometrical value of the turn-around range directly from participants positions.
    // Note that this may be different from the value returned by estimated.getEstimatedValue(),
    // because other modifiers may already have been taken into account
    final Vector3D pSpacecraftLeg1 = spacecraftToInertLeg1.transformPosition(Vector3D.ZERO);
    final Vector3D pSpacecraftLeg2 = spacecraftToInertLeg2.transformPosition(Vector3D.ZERO);
    final double turnAroundRangeUsingSpacecraftCenter = 0.5 * (Vector3D.distance(pMasterEmission, pSpacecraftLeg1) + Vector3D.distance(pSpacecraftLeg1, pSlaveRebound) + Vector3D.distance(pSlaveRebound, pSpacecraftLeg2) + Vector3D.distance(pSpacecraftLeg2, pMasterReception));
    // compute the geometrical value of the range replacing
    // the spacecraft positions with antenna phase center positions
    final Vector3D pAPCLeg1 = spacecraftToInertLeg1.transformPosition(antennaPhaseCenter);
    final Vector3D pAPCLeg2 = spacecraftToInertLeg2.transformPosition(antennaPhaseCenter);
    final double turnAroundRangeUsingAntennaPhaseCenter = 0.5 * (Vector3D.distance(pMasterEmission, pAPCLeg1) + Vector3D.distance(pAPCLeg1, pSlaveRebound) + Vector3D.distance(pSlaveRebound, pAPCLeg2) + Vector3D.distance(pAPCLeg2, pMasterReception));
    // get the estimated value before this modifier is applied
    final double[] value = estimated.getEstimatedValue();
    // modify the value
    value[0] += turnAroundRangeUsingAntennaPhaseCenter - turnAroundRangeUsingSpacecraftCenter;
    estimated.setEstimatedValue(value);
}

Example 90 with TimeStampedPVCoordinates

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

the class EcksteinHechlerPropagator method toCartesian.

/**
 * Convert circular parameters <em>with derivatives</em> to Cartesian coordinates.
 * @param date date of the orbital parameters
 * @param parameters circular parameters (a, ex, ey, i, raan, alphaM)
 * @return Cartesian coordinates consistent with values and derivatives
 */
private TimeStampedPVCoordinates toCartesian(final AbsoluteDate date, final DerivativeStructure[] parameters) {
    // evaluate coordinates in the orbit canonical reference frame
    final DerivativeStructure cosOmega = parameters[4].cos();
    final DerivativeStructure sinOmega = parameters[4].sin();
    final DerivativeStructure cosI = parameters[3].cos();
    final DerivativeStructure sinI = parameters[3].sin();
    final DerivativeStructure alphaE = meanToEccentric(parameters[5], parameters[1], parameters[2]);
    final DerivativeStructure cosAE = alphaE.cos();
    final DerivativeStructure sinAE = alphaE.sin();
    final DerivativeStructure ex2 = parameters[1].multiply(parameters[1]);
    final DerivativeStructure ey2 = parameters[2].multiply(parameters[2]);
    final DerivativeStructure exy = parameters[1].multiply(parameters[2]);
    final DerivativeStructure q = ex2.add(ey2).subtract(1).negate().sqrt();
    final DerivativeStructure beta = q.add(1).reciprocal();
    final DerivativeStructure bx2 = beta.multiply(ex2);
    final DerivativeStructure by2 = beta.multiply(ey2);
    final DerivativeStructure bxy = beta.multiply(exy);
    final DerivativeStructure u = bxy.multiply(sinAE).subtract(parameters[1].add(by2.subtract(1).multiply(cosAE)));
    final DerivativeStructure v = bxy.multiply(cosAE).subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE)));
    final DerivativeStructure x = parameters[0].multiply(u);
    final DerivativeStructure y = parameters[0].multiply(v);
    // canonical orbit reference frame
    final FieldVector3D<DerivativeStructure> p = new FieldVector3D<>(x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))), x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))), y.multiply(sinI));
    // dispatch derivatives
    final Vector3D p0 = new Vector3D(p.getX().getValue(), p.getY().getValue(), p.getZ().getValue());
    final Vector3D p1 = new Vector3D(p.getX().getPartialDerivative(1), p.getY().getPartialDerivative(1), p.getZ().getPartialDerivative(1));
    final Vector3D p2 = new Vector3D(p.getX().getPartialDerivative(2), p.getY().getPartialDerivative(2), p.getZ().getPartialDerivative(2));
    return new TimeStampedPVCoordinates(date, p0, p1, p2);
}