diff --git a/src/main/java/org/orekit/estimation/measurements/AbstractMeasurement.java b/src/main/java/org/orekit/estimation/measurements/AbstractMeasurement.java index b7ed484de13ee8631be5a1c625885927caf86a94..d5ee8d812a6d6480b66c9d8559fd76768d1fde8a 100644 --- a/src/main/java/org/orekit/estimation/measurements/AbstractMeasurement.java +++ b/src/main/java/org/orekit/estimation/measurements/AbstractMeasurement.java @@ -248,6 +248,37 @@ public abstract class AbstractMeasurement> final Vector3D receiverPosition, final AbsoluteDate signalArrivalDate) { + return signalTimeOfFlightFixedReception(adjustableEmitterPV, receiverPosition, signalArrivalDate); + + } + + /** Compute propagation delay on a link leg (typically downlink or uplink). + * @param adjustableEmitterPV position/velocity of emitter that may be adjusted + * @param receiverPosition fixed position of receiver at {@code signalArrivalDate}, + * in the same frame as {@code adjustableEmitterPV} + * @param signalArrivalDate date at which the signal arrives to receiver + * @return positive delay between signal emission and signal reception dates + * @param the type of the components + */ + public static > T signalTimeOfFlight(final TimeStampedFieldPVCoordinates adjustableEmitterPV, + final FieldVector3D receiverPosition, + final FieldAbsoluteDate signalArrivalDate) { + + return signalTimeOfFlightFixedReception(adjustableEmitterPV, receiverPosition, signalArrivalDate); + + } + + /** Compute propagation delay on a link leg (typically downlink or uplink). + * @param adjustableEmitterPV position/velocity of emitter that may be adjusted + * @param receiverPosition fixed position of receiver at {@code signalArrivalDate}, + * in the same frame as {@code adjustableEmitterPV} + * @param signalArrivalDate date at which the signal arrives to receiver + * @return positive delay between signal emission and signal reception dates + */ + public static double signalTimeOfFlightFixedReception(final TimeStampedPVCoordinates adjustableEmitterPV, + final Vector3D receiverPosition, + final AbsoluteDate signalArrivalDate) { + // initialize emission date search loop assuming the state is already correct // this will be true for all but the first orbit determination iteration, // and even for the first iteration the loop will converge very fast @@ -277,9 +308,9 @@ public abstract class AbstractMeasurement> * @return positive delay between signal emission and signal reception dates * @param the type of the components */ - public static > T signalTimeOfFlight(final TimeStampedFieldPVCoordinates adjustableEmitterPV, - final FieldVector3D receiverPosition, - final FieldAbsoluteDate signalArrivalDate) { + public static > T signalTimeOfFlightFixedReception(final TimeStampedFieldPVCoordinates adjustableEmitterPV, + final FieldVector3D receiverPosition, + final FieldAbsoluteDate signalArrivalDate) { // Initialize emission date search loop assuming the emitter PV is almost correct // this will be true for all but the first orbit determination iteration, @@ -348,4 +379,38 @@ public abstract class AbstractMeasurement> } + + /** Compute propagation delay on a link leg (typically downlink or uplink). + * @param adjustableReceiver position/velocity of receiver that may be adjusted + * @param emitterPosition fixed position of emitter at {@code signalArrivalDate}, + * in the same frame as {@code adjustableReceiver} + * @param signalEmissionDate date at which the signal is emitted to receiver + * @return positive delay between signal emission and signal reception dates + * @param the type of the components + */ + public static > T signalTimeOfFlightFixedEmission(final TimeStampedFieldPVCoordinates adjustableReceiver, + final FieldVector3D emitterPosition, + final FieldAbsoluteDate signalEmissionDate) { + + // initialize reception date date search loop assuming the state is already correct + // this will be true for all but the first orbit determination iteration, + // and even for the first iteration the loop will converge very fast + final T offset = signalEmissionDate.getDate().durationFrom(adjustableReceiver.getDate()); + T delay = offset.negate(); + + // search signal transit date, computing the signal travel in inertial frame + final double cReciprocal = 1.0 / Constants.SPEED_OF_LIGHT; + double delta; + int count = 0; + do { + final double previous = delay.getReal(); + final FieldVector3D transitP = adjustableReceiver.shiftedBy(offset.add(delay)).getPosition(); + delay = emitterPosition.distance(transitP).multiply(cReciprocal); + delta = FastMath.abs(delay.getReal() - previous); + } while (count++ < 10 && delta >= 2 * FastMath.ulp(delay.getReal())); + + return delay; + + } + } diff --git a/src/main/java/org/orekit/estimation/measurements/AngularAzEl.java b/src/main/java/org/orekit/estimation/measurements/AngularAzEl.java index cffe4c555f512744d9d7cf1cb1aaf99f8aec013c..8b6873d9cf27b333883c186d8cec070e80851e38 100644 --- a/src/main/java/org/orekit/estimation/measurements/AngularAzEl.java +++ b/src/main/java/org/orekit/estimation/measurements/AngularAzEl.java @@ -42,11 +42,15 @@ import org.orekit.utils.TimeStampedPVCoordinates; * @author Thierry Ceolin * @since 8.0 */ -public class AngularAzEl extends AbstractMeasurement { +public class AngularAzEl extends AbstractMeasurement +{ /** Ground station from which measurement is performed. */ private final GroundStation station; + /** Enum indicating the time tag specification of a range observation. */ + private final TimeTagSpecificationType timeTagSpecificationType; + /** Simple constructor. * @param station ground station from which measurement is performed * @param date date of the measurement @@ -54,11 +58,12 @@ public class AngularAzEl extends AbstractMeasurement { * @param sigma theoretical standard deviation * @param baseWeight base weight * @param satellite satellite related to this measurement - * @since 9.3 + * @param timeTagSpecificationType specify the timetag configuration of the provided angular AzEl observation + * @since xx.xx */ public AngularAzEl(final GroundStation station, final AbsoluteDate date, final double[] angular, final double[] sigma, final double[] baseWeight, - final ObservableSatellite satellite) { + final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { super(date, angular, sigma, baseWeight, Collections.singletonList(satellite)); addParameterDriver(station.getClockOffsetDriver()); addParameterDriver(station.getEastOffsetDriver()); @@ -71,6 +76,23 @@ public class AngularAzEl extends AbstractMeasurement { addParameterDriver(station.getPolarOffsetYDriver()); addParameterDriver(station.getPolarDriftYDriver()); this.station = station; + this.timeTagSpecificationType = timeTagSpecificationType; + } + + + /** Simple constructor. + * @param station ground station from which measurement is performed + * @param date date of the measurement + * @param angular observed value + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param satellite satellite related to this measurement + * @since 9.3 + */ + public AngularAzEl(final GroundStation station, final AbsoluteDate date, + final double[] angular, final double[] sigma, final double[] baseWeight, + final ObservableSatellite satellite) { + this(station, date, angular, sigma, baseWeight, satellite, TimeTagSpecificationType.RX); } /** Get the ground station from which measurement is performed. @@ -113,54 +135,109 @@ public class AngularAzEl extends AbstractMeasurement { // Transform between station and inertial frame, expressed as a gradient // The components of station's position in offset frame are the 3 last derivative parameters - final FieldTransform offsetToInertialDownlink = - station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); - final FieldAbsoluteDate downlinkDateDS = - offsetToInertialDownlink.getFieldDate(); + final FieldTransform offsetToInertialObsEpoch = + station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); + final FieldAbsoluteDate obsEpochFieldDate = + offsetToInertialObsEpoch.getFieldDate(); // Station position/velocity in inertial frame at end of the downlink leg - final TimeStampedFieldPVCoordinates stationDownlink = - offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, - zero, zero, zero)); - // Station topocentric frame (east-north-zenith) in inertial frame expressed as Gradient - final FieldVector3D east = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusI(field)); - final FieldVector3D north = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusJ(field)); - final FieldVector3D zenith = offsetToInertialDownlink.transformVector(FieldVector3D.getPlusK(field)); + final TimeStampedFieldPVCoordinates stationObsEpoch = + offsetToInertialObsEpoch.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(obsEpochFieldDate, + 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) + final Gradient delta = obsEpochFieldDate.durationFrom(state.getDate()); - // Downlink delay - final Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS); + final TimeStampedFieldPVCoordinates transitStateDS; + final TimeStampedFieldPVCoordinates stationDownlink; - // Transit state - final Gradient delta = downlinkDateDS.durationFrom(state.getDate()); - final Gradient deltaMTauD = tauD.negate().add(delta); - final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + /* The station position for relative position vector calculation - set to downlink for transmit and transmit + * receive apparent (TXRX). For transit/bounce time tag specification we use the station at bounce time. For transmit + * apparent the station at time of transmission is used. + */ + final TimeStampedFieldPVCoordinates stationPositionEstimated; - // Transit state (re)computed with derivative structures - final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauD); + final SpacecraftState transitState; + final Gradient tauD; + + if (timeTagSpecificationType == TimeTagSpecificationType.TX || timeTagSpecificationType == TimeTagSpecificationType.TXRX) { + //Date = epoch of transmission. + //Vary position of receiver -> in case of uplink leg, receiver is satellite + final Gradient tauU = signalTimeOfFlightFixedEmission(pvaDS, stationObsEpoch.getPosition(), stationObsEpoch.getDate()); + final Gradient deltaMTauU = tauU.add(delta); + //Get state at transit + transitStateDS = pvaDS.shiftedBy(deltaMTauU); + transitState = state.shiftedBy(deltaMTauU.getValue()); + + //Get station at transit - although this is effectively an initial seed for fitting the downlink delay + final TimeStampedFieldPVCoordinates stationTransit = stationObsEpoch.shiftedBy(tauU); + + //project time of flight forwards with 0 offset. + tauD = signalTimeOfFlightFixedEmission(stationTransit, transitStateDS.getPosition(), transitStateDS.getDate()); + + stationDownlink = stationObsEpoch.shiftedBy(tauU.add(tauD)); + + //Decide whether observation is transmit or receive apparent. + if (timeTagSpecificationType == TimeTagSpecificationType.TXRX) { + stationPositionEstimated = stationDownlink; + } else { + stationPositionEstimated = stationObsEpoch; + } + } + + else if (timeTagSpecificationType == TimeTagSpecificationType.TRANSIT) { + transitStateDS = pvaDS.shiftedBy(delta); + transitState = state.shiftedBy(delta.getValue()); + + tauD = signalTimeOfFlightFixedEmission(stationObsEpoch, transitStateDS.getPosition(), transitStateDS.getDate()); + + stationDownlink = stationObsEpoch.shiftedBy(tauD); + stationPositionEstimated = stationObsEpoch; + } + + else { + // 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 + tauD = signalTimeOfFlightFixedReception(pvaDS, stationObsEpoch.getPosition(), obsEpochFieldDate); + + // Transit state + final Gradient deltaMTauD = tauD.negate().add(delta); + transitState = state.shiftedBy(deltaMTauD.getValue()); + + // Transit state (re)computed with gradients + transitStateDS = pvaDS.shiftedBy(deltaMTauD); + stationDownlink = stationObsEpoch; + stationPositionEstimated = stationDownlink; + } + + final FieldTransform offsetToInertialEstimationTime = station.getOffsetToInertial(state.getFrame(), stationPositionEstimated.getDate(), nbParams, indices); + + // Station topocentric frame (east-north-zenith) in inertial frame expressed as Gradient + final FieldVector3D east = offsetToInertialEstimationTime.transformVector(FieldVector3D.getPlusI(field)); + final FieldVector3D north = offsetToInertialEstimationTime.transformVector(FieldVector3D.getPlusJ(field)); + final FieldVector3D zenith = offsetToInertialEstimationTime.transformVector(FieldVector3D.getPlusK(field)); // Station-satellite vector expressed in inertial frame - final FieldVector3D staSat = transitStateDS.getPosition().subtract(stationDownlink.getPosition()); + final FieldVector3D staSat = transitStateDS.getPosition().subtract(stationPositionEstimated.getPosition()); // Compute azimuth/elevation final Gradient baseAzimuth = staSat.dotProduct(east).atan2(staSat.dotProduct(north)); final double twoPiWrap = MathUtils.normalizeAngle(baseAzimuth.getReal(), getObservedValue()[0]) - - baseAzimuth.getReal(); + baseAzimuth.getReal(); final Gradient azimuth = baseAzimuth.add(twoPiWrap); final Gradient elevation = staSat.dotProduct(zenith).divide(staSat.getNorm()).asin(); // Prepare the estimation final EstimatedMeasurement estimated = - new EstimatedMeasurement<>(this, iteration, evaluation, - new SpacecraftState[] { - transitState - }, new TimeStampedPVCoordinates[] { - transitStateDS.toTimeStampedPVCoordinates(), - stationDownlink.toTimeStampedPVCoordinates() - }); + new EstimatedMeasurement<>(this, iteration, evaluation, + new SpacecraftState[] { transitState }, + new TimeStampedPVCoordinates[] { + transitStateDS.toTimeStampedPVCoordinates(), + stationDownlink.toTimeStampedPVCoordinates() + }); // azimuth - elevation values estimated.setEstimatedValue(azimuth.getValue(), elevation.getValue()); @@ -170,7 +247,7 @@ public class AngularAzEl extends AbstractMeasurement { final double[] azDerivatives = azimuth.getGradient(); final double[] elDerivatives = elevation.getGradient(); estimated.setStateDerivatives(0, - Arrays.copyOfRange(azDerivatives, 0, 6), Arrays.copyOfRange(elDerivatives, 0, 6)); + Arrays.copyOfRange(azDerivatives, 0, 6), Arrays.copyOfRange(elDerivatives, 0, 6)); // Set partial derivatives with respect to parameters // (beware element at index 0 is the value, not a derivative) diff --git a/src/main/java/org/orekit/estimation/measurements/AngularRaDec.java b/src/main/java/org/orekit/estimation/measurements/AngularRaDec.java index b261fa334e9e02c6149436ccd308a1b42d609e91..3cfee102f693954e49473c15e4c20d66cc8dc73d 100644 --- a/src/main/java/org/orekit/estimation/measurements/AngularRaDec.java +++ b/src/main/java/org/orekit/estimation/measurements/AngularRaDec.java @@ -44,7 +44,8 @@ import org.orekit.utils.TimeStampedPVCoordinates; * @author Maxime Journot * @since 9.0 */ -public class AngularRaDec extends AbstractMeasurement { +public class AngularRaDec extends AbstractMeasurement +{ /** Ground station from which measurement is performed. */ private final GroundStation station; @@ -52,6 +53,9 @@ public class AngularRaDec extends AbstractMeasurement { /** Reference frame in which the right ascension - declination angles are given. */ private final Frame referenceFrame; + /** Enum indicating the time tag specification of a range observation. */ + private final TimeTagSpecificationType timeTagSpecificationType; + /** Simple constructor. * @param station ground station from which measurement is performed * @param referenceFrame Reference frame in which the right ascension - declination angles are given @@ -60,11 +64,12 @@ public class AngularRaDec extends AbstractMeasurement { * @param sigma theoretical standard deviation * @param baseWeight base weight * @param satellite satellite related to this measurement - * @since 9.3 + * @param timeTagSpecificationType specify the timetag configuration of the provided angular RaDec observation + * @since xx.xx */ public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date, final double[] angular, final double[] sigma, final double[] baseWeight, - final ObservableSatellite satellite) { + final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { super(date, angular, sigma, baseWeight, Collections.singletonList(satellite)); addParameterDriver(station.getClockOffsetDriver()); addParameterDriver(station.getEastOffsetDriver()); @@ -78,6 +83,24 @@ public class AngularRaDec extends AbstractMeasurement { addParameterDriver(station.getPolarDriftYDriver()); this.station = station; this.referenceFrame = referenceFrame; + this.timeTagSpecificationType = timeTagSpecificationType; + } + + + /** Simple constructor. + * @param station ground station from which measurement is performed + * @param referenceFrame Reference frame in which the right ascension - declination angles are given + * @param date date of the measurement + * @param angular observed value + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param satellite satellite related to this measurement + * @since 9.3 + */ + public AngularRaDec(final GroundStation station, final Frame referenceFrame, final AbsoluteDate date, + final double[] angular, final double[] sigma, final double[] baseWeight, + final ObservableSatellite satellite) { + this(station, referenceFrame, date, angular, sigma, baseWeight, satellite, TimeTagSpecificationType.RX); } /** Get the ground station from which measurement is performed. @@ -126,58 +149,107 @@ public class AngularRaDec extends AbstractMeasurement { // Transform between station and inertial frame, expressed as a gradient // The components of station's position in offset frame are the 3 last derivative parameters - final FieldTransform offsetToInertialDownlink = - station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); - final FieldAbsoluteDate downlinkDateDS = - offsetToInertialDownlink.getFieldDate(); + final FieldTransform offsetToInertialObsEpoch = + station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); + final FieldAbsoluteDate obsEpochFieldDate = + offsetToInertialObsEpoch.getFieldDate(); // Station position/velocity in inertial frame at end of the downlink leg - final TimeStampedFieldPVCoordinates stationDownlink = - offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, - zero, zero, zero)); + final TimeStampedFieldPVCoordinates stationObsEpoch = + offsetToInertialObsEpoch.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(obsEpochFieldDate, + zero, zero, zero)); + + final Gradient delta = obsEpochFieldDate.durationFrom(state.getDate()); + + final TimeStampedFieldPVCoordinates transitStateDS; + final TimeStampedFieldPVCoordinates stationDownlink; + + /* The station position for relative position vector calculation - set to downlink for transmit and transmit + * receive apparent (TXRX). For transit/bounce time tag specification we use the station at bounce time. For transmit + * apparent the station at time of transmission is used. + */ + final TimeStampedFieldPVCoordinates stationPositionEstimated; + + final SpacecraftState transitState; + final Gradient tauD; - // 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) + if (timeTagSpecificationType == TimeTagSpecificationType.TX || timeTagSpecificationType == TimeTagSpecificationType.TXRX) { + //Date = epoch of transmission. + //Vary position of receiver -> in case of uplink leg, receiver is satellite + final Gradient tauU = signalTimeOfFlightFixedEmission(pvaDS, stationObsEpoch.getPosition(), stationObsEpoch.getDate()); + final Gradient deltaMTauU = tauU.add(delta); + //Get state at transit + transitStateDS = pvaDS.shiftedBy(deltaMTauU); + transitState = state.shiftedBy(deltaMTauU.getValue()); - // Downlink delay - final Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS); + //Get station at transit - although this is effectively an initial seed for fitting the downlink delay + final TimeStampedFieldPVCoordinates stationTransit = stationObsEpoch.shiftedBy(tauU); + + //project time of flight forwards with 0 offset. + tauD = signalTimeOfFlightFixedEmission(stationTransit, transitStateDS.getPosition(), transitStateDS.getDate()); + + stationDownlink = stationObsEpoch.shiftedBy(tauU.add(tauD)); + //Decide whether observation is transmit or receive apparent. + if (timeTagSpecificationType == TimeTagSpecificationType.TXRX) { + stationPositionEstimated = stationDownlink; + } else { + stationPositionEstimated = stationObsEpoch; + } + } - // Transit state - final Gradient delta = downlinkDateDS.durationFrom(state.getDate()); - final Gradient deltaMTauD = tauD.negate().add(delta); - final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + else if (timeTagSpecificationType == TimeTagSpecificationType.TRANSIT) { - // Transit state (re)computed with gradients - final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauD); + transitStateDS = pvaDS.shiftedBy(delta); + transitState = state.shiftedBy(delta.getValue()); + tauD = signalTimeOfFlightFixedEmission(stationObsEpoch, transitStateDS.getPosition(), transitStateDS.getDate()); + + stationDownlink = stationObsEpoch.shiftedBy(tauD); + stationPositionEstimated = stationObsEpoch; + } + + else { + // 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 + tauD = signalTimeOfFlightFixedReception(pvaDS, stationObsEpoch.getPosition(), obsEpochFieldDate); + + // Transit state + final Gradient deltaMTauD = tauD.negate().add(delta); + transitState = state.shiftedBy(deltaMTauD.getValue()); + + // Transit state (re)computed with gradients + transitStateDS = pvaDS.shiftedBy(deltaMTauD); + stationDownlink = stationObsEpoch; + stationPositionEstimated = stationObsEpoch; + } // Station-satellite vector expressed in inertial frame - final FieldVector3D staSatInertial = transitStateDS.getPosition().subtract(stationDownlink.getPosition()); + final FieldVector3D staSatInertial = transitStateDS.getPosition().subtract(stationPositionEstimated.getPosition()); // Field transform from inertial to reference frame at station's reception date - final FieldTransform inertialToReferenceDownlink = - state.getFrame().getTransformTo(referenceFrame, downlinkDateDS); + final FieldTransform inertialToReferenceEstimationTime = + state.getFrame().getTransformTo(referenceFrame, stationPositionEstimated.getDate()); // Station-satellite vector in reference frame - final FieldVector3D staSatReference = inertialToReferenceDownlink.transformPosition(staSatInertial); + final FieldVector3D staSatReference = inertialToReferenceEstimationTime.transformPosition(staSatInertial); // Compute right ascension and declination final Gradient baseRightAscension = staSatReference.getAlpha(); final double twoPiWrap = MathUtils.normalizeAngle(baseRightAscension.getReal(), - getObservedValue()[0]) - baseRightAscension.getReal(); + getObservedValue()[0]) - baseRightAscension.getReal(); final Gradient rightAscension = baseRightAscension.add(twoPiWrap); final Gradient declination = staSatReference.getDelta(); // Prepare the estimation final EstimatedMeasurement estimated = - new EstimatedMeasurement<>(this, iteration, evaluation, - new SpacecraftState[] { - transitState - }, new TimeStampedPVCoordinates[] { - transitStateDS.toTimeStampedPVCoordinates(), - stationDownlink.toTimeStampedPVCoordinates() - }); - + new EstimatedMeasurement<>(this, iteration, evaluation, + new SpacecraftState[] { transitState }, + new TimeStampedPVCoordinates[] { + transitStateDS.toTimeStampedPVCoordinates(), + stationDownlink.toTimeStampedPVCoordinates() + }); // azimuth - elevation values estimated.setEstimatedValue(rightAscension.getValue(), declination.getValue()); @@ -186,7 +258,7 @@ public class AngularRaDec extends AbstractMeasurement { final double[] raDerivatives = rightAscension.getGradient(); final double[] decDerivatives = declination.getGradient(); estimated.setStateDerivatives(0, - Arrays.copyOfRange(raDerivatives, 0, 6), Arrays.copyOfRange(decDerivatives, 0, 6)); + Arrays.copyOfRange(raDerivatives, 0, 6), Arrays.copyOfRange(decDerivatives, 0, 6)); // Partial derivatives with respect to parameters // (beware element at index 0 is the value, not a derivative) diff --git a/src/main/java/org/orekit/estimation/measurements/Range.java b/src/main/java/org/orekit/estimation/measurements/Range.java index d5d20915ac734155cd45c472a96b7edee704bb36..2e68cfe863d5ee70ccf6cd5d0c008c051b836780 100644 --- a/src/main/java/org/orekit/estimation/measurements/Range.java +++ b/src/main/java/org/orekit/estimation/measurements/Range.java @@ -48,8 +48,11 @@ import org.orekit.utils.TimeStampedPVCoordinates; *

*

* The motion of both the station and the spacecraft during the signal - * flight time are taken into account. The date of the measurement - * corresponds to the reception on ground of the emitted or reflected signal. + * flight time are taken into account. For two way measurements the date of the measurement + * corresponds to either the time of signal reception at ground station, + * the time of signal bounce arrival or the date of transmission dependent on the time tag specification + * enumerated value. If not specified, the time tag specification defaults to receive time. + * For one way measurements the date of the measurement is assumed to be a time of signal reception. *

*

* The clock offsets of both the ground station and the satellite are taken @@ -58,7 +61,7 @@ import org.orekit.utils.TimeStampedPVCoordinates; * date. These offsets have two effects: *

*
    - *
  • as measurement date is evaluated at reception time, the real physical date + *
  • If measurement date is evaluated at reception time, the real physical date * of the measurement is the observed date to which the receiving ground station * clock offset is subtracted
    • *
  • as range is evaluated using the total signal time of flight, for one-way @@ -75,7 +78,8 @@ import org.orekit.utils.TimeStampedPVCoordinates; * @author Maxime Journot * @since 8.0 */ -public class Range extends AbstractMeasurement { +public class Range extends AbstractMeasurement +{ /** Ground station from which measurement is performed. */ private final GroundStation station; @@ -83,6 +87,9 @@ public class Range extends AbstractMeasurement { /** Flag indicating whether it is a two-way measurement. */ private final boolean twoway; + /** Enum indicating the time tag specification of a range observation. */ + private final TimeTagSpecificationType timeTagSpecificationType; + /** Simple constructor. * @param station ground station from which measurement is performed * @param twoWay flag indicating whether it is a two-way measurement @@ -91,11 +98,12 @@ public class Range extends AbstractMeasurement { * @param sigma theoretical standard deviation * @param baseWeight base weight * @param satellite satellite related to this measurement - * @since 9.3 + * @param timeTagSpecificationType specify the timetag configuration of the provided range observation + * @since xx.xx */ - public Range(final GroundStation station, final boolean twoWay, final AbsoluteDate date, - final double range, final double sigma, final double baseWeight, - final ObservableSatellite satellite) { + private Range(final GroundStation station, final boolean twoWay, final AbsoluteDate date, + final double range, final double sigma, final double baseWeight, + final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { super(date, range, sigma, baseWeight, Collections.singletonList(satellite)); addParameterDriver(station.getClockOffsetDriver()); addParameterDriver(station.getEastOffsetDriver()); @@ -113,8 +121,44 @@ public class Range extends AbstractMeasurement { } this.station = station; this.twoway = twoWay; + this.timeTagSpecificationType = timeTagSpecificationType; + } + + /** Range measurement constructor for one or two way measurements with timetag of observed value + * set to reception time. + * @param station ground station from which measurement is performed + * @param twoWay flag indicating whether it is a two-way measurement + * @param date date of the measurement + * @param range observed value + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param satellite satellite related to this measurement + * @since 9.3 + */ + public Range(final GroundStation station, final boolean twoWay, final AbsoluteDate date, + final double range, final double sigma, final double baseWeight, + final ObservableSatellite satellite) { + this(station, twoWay, date, range, sigma, baseWeight, satellite, TimeTagSpecificationType.RX); + } + + /** Range constructor for two-way measurements with a user specified observed value timetag specification. + * @param station ground station from which measurement is performed + * @param date date of the measurement + * @param range observed value + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param satellite satellite related to this measurement + * @param timeTagSpecificationType specify the timetag configuration of the provided range observation + * @since xx.xx + */ + public Range(final GroundStation station, final AbsoluteDate date, + final double range, final double sigma, final double baseWeight, + final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { + this(station, true, date, range, sigma, baseWeight, satellite, timeTagSpecificationType); } + + /** Get the ground station from which measurement is performed. * @return ground station from which measurement is performed */ @@ -132,8 +176,8 @@ public class Range extends AbstractMeasurement { /** {@inheritDoc} */ @Override protected EstimatedMeasurement theoreticalEvaluation(final int iteration, - final int evaluation, - final SpacecraftState[] states) { + final int evaluation, + final SpacecraftState[] states) { final SpacecraftState state = states[0]; @@ -156,70 +200,119 @@ public class Range extends AbstractMeasurement { // Coordinates of the spacecraft expressed as a gradient final TimeStampedFieldPVCoordinates pvaDS = getCoordinates(state, 0, nbParams); - // transform between station and inertial frame, expressed as a gradient // The components of station's position in offset frame are the 3 last derivative parameters - final FieldTransform offsetToInertialDownlink = - station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); - final FieldAbsoluteDate downlinkDateDS = offsetToInertialDownlink.getFieldDate(); - - // Station position in inertial frame at end of the downlink leg - final TimeStampedFieldPVCoordinates 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 Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS); + final FieldTransform offsetToInertialObsEpoch = + station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); + final FieldAbsoluteDate obsEpochDateDS = offsetToInertialObsEpoch.getFieldDate(); - // Transit state & Transit state (re)computed with gradients - final Gradient delta = downlinkDateDS.durationFrom(state.getDate()); - final Gradient deltaMTauD = tauD.negate().add(delta); - final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); - final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauD); + // Station position in inertial frame at epoch of observation + final TimeStampedFieldPVCoordinates stationObsEpoch = + offsetToInertialObsEpoch.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(obsEpochDateDS, + zero, zero, zero)); + final Gradient delta = obsEpochDateDS.durationFrom(state.getDate()); // prepare the evaluation final EstimatedMeasurement estimated; final Gradient range; + final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT; + final Gradient tauD; if (twoway) { - // Station at transit state date (derivatives of tauD taken into account) - final TimeStampedFieldPVCoordinates stationAtTransitDate = - stationDownlink.shiftedBy(tauD.negate()); - // Uplink delay - final Gradient tauU = - signalTimeOfFlight(stationAtTransitDate, transitStateDS.getPosition(), transitStateDS.getDate()); - final TimeStampedFieldPVCoordinates stationUplink = - stationDownlink.shiftedBy(-tauD.getValue() - tauU.getValue()); + final Gradient tauU; - // Prepare the evaluation - estimated = new EstimatedMeasurement(this, iteration, evaluation, - new SpacecraftState[] { - transitState - }, new TimeStampedPVCoordinates[] { - stationUplink.toTimeStampedPVCoordinates(), - transitStateDS.toTimeStampedPVCoordinates(), - stationDownlink.toTimeStampedPVCoordinates() - }); + if (timeTagSpecificationType == TimeTagSpecificationType.TX) { + //Date = epoch of transmission. + //Vary position of receiver -> in case of uplink leg, receiver is satellite + tauU = signalTimeOfFlightFixedEmission(pvaDS, stationObsEpoch.getPosition(), stationObsEpoch.getDate()); - // Range value - final double cOver2 = 0.5 * Constants.SPEED_OF_LIGHT; - final Gradient tau = tauD.add(tauU); - range = tau.multiply(cOver2); + final Gradient deltaMTauU = tauU.add(delta); + //Get state at transit + final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauU); + final SpacecraftState transitState = state.shiftedBy(deltaMTauU.getValue()); + + //Get station at transit - although this is effectively an initial seed for fitting the downlink delay + final TimeStampedFieldPVCoordinates stationTransit = stationObsEpoch.shiftedBy(tauU); + + //project time of flight forwards with 0 offset. + tauD = signalTimeOfFlightFixedEmission(stationTransit, transitStateDS.getPosition(), transitStateDS.getDate()); + + estimated = new EstimatedMeasurement(this, iteration, evaluation, new SpacecraftState[] {transitState}, + new TimeStampedPVCoordinates[] { + stationObsEpoch.toTimeStampedPVCoordinates(), + transitStateDS.toTimeStampedPVCoordinates(), + stationTransit.shiftedBy(tauD).toTimeStampedPVCoordinates()}); + + // Range value + final Gradient tau = tauD.add(tauU); + range = tau.multiply(cOver2); + + } + else if (timeTagSpecificationType == TimeTagSpecificationType.TRANSIT) { + final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(delta); + final SpacecraftState transitState = state.shiftedBy(delta.getValue()); + + //Calculate time of flight for return measurement participants + tauD = signalTimeOfFlightFixedEmission(stationObsEpoch, transitStateDS.getPosition(), transitStateDS.getDate()); + tauU = signalTimeOfFlightFixedReception(stationObsEpoch, transitStateDS.getPosition(), transitStateDS.getDate()); + estimated = new EstimatedMeasurement(this, iteration, evaluation, new SpacecraftState[] {transitState}, + new TimeStampedPVCoordinates[] { + stationObsEpoch.shiftedBy(tauU.negate()).toTimeStampedPVCoordinates(), + transitStateDS.toTimeStampedPVCoordinates(), + stationObsEpoch.shiftedBy(tauD).toTimeStampedPVCoordinates() + }); + //use transit state as corrected state + range = stationObsEpoch.getPosition().distance(transitStateDS.getPosition()); + } + else { + // Downlink delay + tauD = signalTimeOfFlightFixedReception(pvaDS, stationObsEpoch.getPosition(), obsEpochDateDS); + + // Transit state & Transit state (re)computed with gradients + final Gradient deltaMTauD = tauD.negate().add(delta); + final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauD); + + // Station at transit state date (derivatives of tauD taken into account) + final TimeStampedFieldPVCoordinates stationAtTransitDate = stationObsEpoch.shiftedBy(tauD.negate()); + // Uplink delay + tauU = signalTimeOfFlightFixedReception(stationAtTransitDate, transitStateDS.getPosition(), + transitStateDS.getDate()); + final TimeStampedFieldPVCoordinates stationUplink = + stationObsEpoch.shiftedBy(-tauD.getValue() - tauU.getValue()); + + // Prepare the evaluation + estimated = new EstimatedMeasurement(this, iteration, evaluation, new SpacecraftState[] {transitState}, + new TimeStampedPVCoordinates[] { + stationUplink.toTimeStampedPVCoordinates(), + transitStateDS.toTimeStampedPVCoordinates(), + stationObsEpoch.toTimeStampedPVCoordinates()}); + + // Range value + final Gradient tau = tauD.add(tauU); + range = tau.multiply(cOver2); + } } else { + // (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 + tauD = signalTimeOfFlightFixedReception(pvaDS, stationObsEpoch.getPosition(), obsEpochDateDS); + + // Transit state & Transit state (re)computed with gradients + final Gradient deltaMTauD = tauD.negate().add(delta); + final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + final TimeStampedFieldPVCoordinates transitStateDS = pvaDS.shiftedBy(deltaMTauD); + estimated = new EstimatedMeasurement(this, iteration, evaluation, - new SpacecraftState[] { - transitState - }, new TimeStampedPVCoordinates[] { - transitStateDS.toTimeStampedPVCoordinates(), - stationDownlink.toTimeStampedPVCoordinates() - }); + new SpacecraftState[] {transitState}, + new TimeStampedPVCoordinates[] { + transitStateDS.toTimeStampedPVCoordinates(), + stationObsEpoch.toTimeStampedPVCoordinates() + }); // Clock offsets final ObservableSatellite satellite = getSatellites().get(0); diff --git a/src/main/java/org/orekit/estimation/measurements/RangeRate.java b/src/main/java/org/orekit/estimation/measurements/RangeRate.java index 0eca1f81e7caad0c292000107905524cff4fe9ab..0403faf9d1b65b729a8cf3b4c2d8deeba1fc7090 100644 --- a/src/main/java/org/orekit/estimation/measurements/RangeRate.java +++ b/src/main/java/org/orekit/estimation/measurements/RangeRate.java @@ -47,7 +47,8 @@ import org.orekit.utils.TimeStampedPVCoordinates; * @author Joris Olympio * @since 8.0 */ -public class RangeRate extends AbstractMeasurement { +public class RangeRate extends AbstractMeasurement +{ /** Ground station from which measurement is performed. */ private final GroundStation station; @@ -55,6 +56,9 @@ public class RangeRate extends AbstractMeasurement { /** Flag indicating whether it is a two-way measurement. */ private final boolean twoway; + /** Enum indicating the time tag specification of a range observation. */ + private final TimeTagSpecificationType timeTagSpecificationType; + /** Simple constructor. * @param station ground station from which measurement is performed * @param date date of the measurement @@ -63,11 +67,12 @@ public class RangeRate extends AbstractMeasurement { * @param baseWeight base weight * @param twoway if true, this is a two-way measurement * @param satellite satellite related to this measurement + * @param timeTagSpecificationType specify the timetag configuration of the provided range rate observation * @since 9.3 */ - public RangeRate(final GroundStation station, final AbsoluteDate date, + private RangeRate(final GroundStation station, final AbsoluteDate date, final double rangeRate, final double sigma, final double baseWeight, - final boolean twoway, final ObservableSatellite satellite) { + final boolean twoway, final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { super(date, rangeRate, sigma, baseWeight, Collections.singletonList(satellite)); addParameterDriver(station.getClockOffsetDriver()); addParameterDriver(station.getClockDriftDriver()); @@ -83,6 +88,42 @@ public class RangeRate extends AbstractMeasurement { addParameterDriver(station.getPolarDriftYDriver()); this.station = station; this.twoway = twoway; + this.timeTagSpecificationType = timeTagSpecificationType; + } + + /** + * Range rate constructor for one or two-way measurements with timetag of + * observed value set to reception time. + * @param station ground station from which measurement is performed + * @param date date of the measurement + * @param rangeRate observed value, m/s + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param twoway if true, this is a two-way measurement + * @param satellite satellite related to this measurement + * @since xx.xx + */ + public RangeRate(final GroundStation station, final AbsoluteDate date, + final double rangeRate, final double sigma, final double baseWeight, + final boolean twoway, final ObservableSatellite satellite) { + this(station, date, rangeRate, sigma, baseWeight, twoway, satellite, TimeTagSpecificationType.RX); + } + + /** + * Range rate constructor for two-way measurements with a user specified observed value timetag specification. + * @param station ground station from which measurement is performed + * @param date date of the measurement + * @param rangeRate observed value, m/s + * @param sigma theoretical standard deviation + * @param baseWeight base weight + * @param satellite satellite related to this measurement + * @param timeTagSpecificationType specify the timetag configuration of the provided range rate observation + * @since xx.xx + **/ + public RangeRate(final GroundStation station, final AbsoluteDate date, + final double rangeRate, final double sigma, final double baseWeight, + final ObservableSatellite satellite, final TimeTagSpecificationType timeTagSpecificationType) { + this(station, date, rangeRate, sigma, baseWeight, true, satellite, timeTagSpecificationType); } /** Check if the instance represents a two-way measurement. @@ -128,90 +169,159 @@ public class RangeRate extends AbstractMeasurement { // transform between station and inertial frame, expressed as a gradient // The components of station's position in offset frame are the 3 last derivative parameters - final FieldTransform offsetToInertialDownlink = - station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); - final FieldAbsoluteDate downlinkDateDS = - offsetToInertialDownlink.getFieldDate(); + final FieldTransform offsetToInertialObsEpoch = + station.getOffsetToInertial(state.getFrame(), getDate(), nbParams, indices); + final FieldAbsoluteDate obsEpochFieldDate = + offsetToInertialObsEpoch.getFieldDate(); // Station position in inertial frame at end of the downlink leg - final TimeStampedFieldPVCoordinates stationDownlink = - offsetToInertialDownlink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(downlinkDateDS, - zero, zero, zero)); + final TimeStampedFieldPVCoordinates stationObsEpoch = + offsetToInertialObsEpoch.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(obsEpochFieldDate, + 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) + // Delta to account for the case in which the state is provided at a different time to the obs epoch + final Gradient delta = obsEpochFieldDate.durationFrom(state.getDate()); - // Downlink delay - final Gradient tauD = signalTimeOfFlight(pvaDS, stationDownlink.getPosition(), downlinkDateDS); + final EstimatedMeasurement estimated; + final Gradient tauD; + final EstimatedMeasurement evalOneWay1; + + if (twoway) { + final Gradient tauU; + final EstimatedMeasurement evalOneWay2; + + if (timeTagSpecificationType == TimeTagSpecificationType.TX) { + //Date = epoch of transmission. + //Vary position of receiver -> in case of uplink leg, receiver is satellite + tauU = signalTimeOfFlightFixedEmission(pvaDS, stationObsEpoch.getPosition(), stationObsEpoch.getDate()); + //Get state at transit + final Gradient deltaMTauU = tauU.add(delta); + final TimeStampedFieldPVCoordinates transitPV = pvaDS.shiftedBy(deltaMTauU); + final SpacecraftState transitState = state.shiftedBy(deltaMTauU.getValue()); + evalOneWay2 = oneWayTheoreticalEvaluation(iteration, evaluation, false, stationObsEpoch, transitPV, transitState, indices, nbParams); + //Get station at transit - although this is effectively an initial seed for fitting the downlink delay + final TimeStampedFieldPVCoordinates stationTransit = stationObsEpoch.shiftedBy(tauU); + + //project time of flight forwards with 0 offset. + tauD = signalTimeOfFlightFixedEmission(stationTransit, transitPV.getPosition(), transitPV.getDate()); + evalOneWay1 = oneWayTheoreticalEvaluation(iteration, evaluation, true, stationTransit.shiftedBy(tauD), transitPV, transitState, indices, nbParams); + + } else if (timeTagSpecificationType == TimeTagSpecificationType.TRANSIT) { + final TimeStampedFieldPVCoordinates transitPV = pvaDS.shiftedBy(delta); + final SpacecraftState transitState = state.shiftedBy(delta.getValue()); + + //In transit obs case, do not correct the value for motion during time of flight. + final Gradient transitRangeRate = getRangeRateValue(stationObsEpoch, transitPV); + + //Calculate time of flight for return measurement participants + tauD = signalTimeOfFlightFixedEmission(stationObsEpoch, transitPV.getPosition(), transitPV.getDate()); + tauU = signalTimeOfFlightFixedReception(stationObsEpoch, transitPV.getPosition(), transitPV.getDate()); + + //shift station forwards to get downlink + evalOneWay1 = oneWayTheoreticalEvaluation(iteration, evaluation, true, stationObsEpoch.shiftedBy(tauD), transitPV, transitState, indices, nbParams); + evalOneWay1.setEstimatedValue(transitRangeRate.getValue()); + //shift station backwards to get uplink + evalOneWay2 = oneWayTheoreticalEvaluation(iteration, evaluation, false, stationObsEpoch.shiftedBy(tauU.negate()), transitPV, transitState, indices, nbParams); + evalOneWay2.setEstimatedValue(transitRangeRate.getValue()); + } + else { + // 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) - // Transit state - final Gradient delta = downlinkDateDS.durationFrom(state.getDate()); - final Gradient deltaMTauD = tauD.negate().add(delta); - final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + // Downlink delay + tauD = signalTimeOfFlight(pvaDS, stationObsEpoch.getPosition(), obsEpochFieldDate); - // Transit state (re)computed with gradients - final TimeStampedFieldPVCoordinates transitPV = pvaDS.shiftedBy(deltaMTauD); + // Transit state + final Gradient deltaMTauD = tauD.negate().add(delta); + final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); - // one-way (downlink) range-rate - final EstimatedMeasurement evalOneWay1 = + // Transit state (re)computed with gradients + final TimeStampedFieldPVCoordinates transitPV = pvaDS.shiftedBy(deltaMTauD); + + evalOneWay1 = oneWayTheoreticalEvaluation(iteration, evaluation, true, - stationDownlink, transitPV, transitState, indices, nbParams); - final EstimatedMeasurement estimated; - if (twoway) { - // one-way (uplink) light time correction - final FieldTransform offsetToInertialApproxUplink = - station.getOffsetToInertial(state.getFrame(), - downlinkDateDS.shiftedBy(tauD.multiply(-2)), nbParams, indices); - final FieldAbsoluteDate approxUplinkDateDS = - offsetToInertialApproxUplink.getFieldDate(); + stationObsEpoch, transitPV, transitState, indices, nbParams); + + // one-way (uplink) light time correction + final FieldTransform offsetToInertialApproxUplink = + station.getOffsetToInertial(state.getFrame(), obsEpochFieldDate.shiftedBy(tauD.multiply(-2)), + nbParams, indices); + final FieldAbsoluteDate approxUplinkDateDS = offsetToInertialApproxUplink.getFieldDate(); - final TimeStampedFieldPVCoordinates stationApproxUplink = - offsetToInertialApproxUplink.transformPVCoordinates(new TimeStampedFieldPVCoordinates<>(approxUplinkDateDS, - zero, zero, zero)); + final TimeStampedFieldPVCoordinates stationApproxUplink = + offsetToInertialApproxUplink.transformPVCoordinates( + new TimeStampedFieldPVCoordinates<>(approxUplinkDateDS, zero, zero, + zero)); - final Gradient tauU = signalTimeOfFlight(stationApproxUplink, transitPV.getPosition(), transitPV.getDate()); + tauU = + signalTimeOfFlightFixedReception(stationApproxUplink, transitPV.getPosition(), transitPV.getDate()); - final TimeStampedFieldPVCoordinates stationUplink = - stationApproxUplink.shiftedBy(transitPV.getDate().durationFrom(approxUplinkDateDS).subtract(tauU)); + final TimeStampedFieldPVCoordinates stationUplink = stationApproxUplink + .shiftedBy(transitPV.getDate().durationFrom(approxUplinkDateDS).subtract(tauU)); - final EstimatedMeasurement evalOneWay2 = - oneWayTheoreticalEvaluation(iteration, evaluation, false, - stationUplink, transitPV, transitState, indices, nbParams); + evalOneWay2 = + oneWayTheoreticalEvaluation(iteration, evaluation, false, stationUplink, + transitPV, transitState, indices, nbParams); + } // 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 = 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) { + 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); + 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) { + for (int i = 0; i < pd.length; ++i) + { pd[i] = 0.5 * (pd1[i] + pd2[i]); } - estimated.setParameterDerivatives(driver, pd); + estimated.setParameterDerivatives( + driver, pd); }); - } else { + + // 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 + tauD = signalTimeOfFlightFixedReception(pvaDS, stationObsEpoch.getPosition(), obsEpochFieldDate); + + // Transit state + final Gradient deltaMTauD = tauD.negate().add(delta); + final SpacecraftState transitState = state.shiftedBy(deltaMTauD.getValue()); + + // Transit state (re)computed with gradients + final TimeStampedFieldPVCoordinates transitPV = pvaDS.shiftedBy(deltaMTauD); + + // one-way (downlink) range-rate + evalOneWay1 = + oneWayTheoreticalEvaluation(iteration, evaluation, true, + stationObsEpoch, transitPV, transitState, indices, nbParams); + + estimated = evalOneWay1; } @@ -229,7 +339,7 @@ public class RangeRate extends AbstractMeasurement { * @param indices indices of the estimated parameters in derivatives computations * @param nbParams the number of estimated parameters in derivative computations * @return theoretical value - * @see #evaluate(SpacecraftStatet) + * //@see #evaluate(SpacecraftStatet) */ private EstimatedMeasurement oneWayTheoreticalEvaluation(final int iteration, final int evaluation, final boolean downlink, final TimeStampedFieldPVCoordinates stationPV, @@ -240,29 +350,15 @@ public class RangeRate extends AbstractMeasurement { // prepare the evaluation final EstimatedMeasurement estimated = - new EstimatedMeasurement(this, iteration, evaluation, - new SpacecraftState[] { - transitState - }, new TimeStampedPVCoordinates[] { - (downlink ? transitPV : stationPV).toTimeStampedPVCoordinates(), - (downlink ? stationPV : transitPV).toTimeStampedPVCoordinates() - }); - - // range rate value - final FieldVector3D stationPosition = stationPV.getPosition(); - final FieldVector3D relativePosition = stationPosition.subtract(transitPV.getPosition()); - - final FieldVector3D stationVelocity = stationPV.getVelocity(); - final FieldVector3D relativeVelocity = stationVelocity.subtract(transitPV.getVelocity()); - - // radial direction - final FieldVector3D lineOfSight = relativePosition.normalize(); - - // line of sight velocity - final Gradient lineOfSightVelocity = FieldVector3D.dotProduct(relativeVelocity, lineOfSight); + new EstimatedMeasurement(this, iteration, evaluation, + new SpacecraftState[] {transitState}, + new TimeStampedPVCoordinates[] { + (downlink ? transitPV : stationPV).toTimeStampedPVCoordinates(), + (downlink ? stationPV : transitPV).toTimeStampedPVCoordinates() + }); // range rate - Gradient rangeRate = lineOfSightVelocity; + Gradient rangeRate = getRangeRateValue(stationPV, transitPV); if (!twoway) { // clock drifts, taken in account only in case of one way @@ -294,4 +390,19 @@ public class RangeRate extends AbstractMeasurement { } + private static Gradient getRangeRateValue(final TimeStampedFieldPVCoordinates stationPV, final TimeStampedFieldPVCoordinates transitPV) { + // range rate value + final FieldVector3D stationPosition = stationPV.getPosition(); + final FieldVector3D relativePosition = stationPosition.subtract(transitPV.getPosition()); + + final FieldVector3D stationVelocity = stationPV.getVelocity(); + final FieldVector3D relativeVelocity = stationVelocity.subtract(transitPV.getVelocity()); + + // radial direction + final FieldVector3D lineOfSight = relativePosition.normalize(); + + // line of sight velocity + return FieldVector3D.dotProduct(relativeVelocity, lineOfSight); + } + } diff --git a/src/main/java/org/orekit/estimation/measurements/TimeTagSpecificationType.java b/src/main/java/org/orekit/estimation/measurements/TimeTagSpecificationType.java new file mode 100644 index 0000000000000000000000000000000000000000..49a3266a0df2a08f6cf451af88d18d7a82b3c9a9 --- /dev/null +++ b/src/main/java/org/orekit/estimation/measurements/TimeTagSpecificationType.java @@ -0,0 +1,61 @@ +/* Copyright 2002-2022 CS GROUP + * Licensed to CS GROUP (CS) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. + * CS licenses this file to You under the Apache License, Version 2.0 + * (the "License"); you may not use this file except in compliance with + * the License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +package org.orekit.estimation.measurements; + +/** + * Enumeration for different time tag specification types, specifying the point at which the + * measurement epoch is taken from. + * + * @author Tommy Fryer + * @since 11.1 + */ +public enum TimeTagSpecificationType { + + /** Time tag at transit/bounce time. */ + TRANSIT("Transit"), + + /** Time tag is at time of signal reception. */ + RX("Receive"), + + /** Time tag is at time of signal transmission. */ + TX("Transmit"), + + /** For Angles measurements where the time tag is at time of signal transmission but the observation is receive apparent. */ + TXRX("Transmit (Receive Apparent)"); + + /** Name of time tag specification. */ + private final String name; + + /** + * Constructor. + * @param name name of the time tag specification + */ + TimeTagSpecificationType(final String name) { + this.name = name; + } + + /** + * Get the name of the time tag specification. + * @return the name + */ + public String getName() { + return name; + } + +} + diff --git a/src/test/java/org/orekit/estimation/measurements/AngularAzElTest.java b/src/test/java/org/orekit/estimation/measurements/AngularAzElTest.java index b2ad595dafc0104a411126d20ddb6ac52ae27a4e..fb4da742d1eb92b4c81694d374c342f0bf817dd5 100644 --- a/src/test/java/org/orekit/estimation/measurements/AngularAzElTest.java +++ b/src/test/java/org/orekit/estimation/measurements/AngularAzElTest.java @@ -22,20 +22,19 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.stat.descriptive.StreamingStatistics; import org.hipparchus.stat.descriptive.rank.Median; import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathUtils; import org.junit.Assert; import org.junit.Test; import org.orekit.estimation.Context; import org.orekit.estimation.EstimationTestUtils; +import org.orekit.frames.Transform; import org.orekit.orbits.OrbitType; import org.orekit.orbits.PositionAngle; import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.conversion.NumericalPropagatorBuilder; import org.orekit.time.AbsoluteDate; -import org.orekit.utils.Differentiation; -import org.orekit.utils.ParameterDriver; -import org.orekit.utils.ParameterFunction; -import org.orekit.utils.StateFunction; +import org.orekit.utils.*; public class AngularAzElTest { @@ -265,5 +264,105 @@ public class AngularAzElTest { } } } + + /** + * Test the estimated values when the observed angular AzEl value is provided at TX (Transmit), + * RX (Receive (default)), transit (bounce) + */ + @Test + public void testTimeTagSpecifications(){ + Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); + SpacecraftState state = new SpacecraftState(context.initialOrbit); + ObservableSatellite obsSat = new ObservableSatellite(0); + + for (GroundStation gs : context.getStations()) { + + gs.getPrimeMeridianOffsetDriver().setReferenceDate(state.getDate()); + gs.getPrimeMeridianDriftDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetXDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftXDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetYDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftYDriver().setReferenceDate(state.getDate()); + + Transform gsToInertialTransform = gs.getOffsetToInertial(state.getFrame(), state.getDate()); + TimeStampedPVCoordinates stationPosition = gsToInertialTransform.transformPVCoordinates(new TimeStampedPVCoordinates(state.getDate(), Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO)); + + double staticDistance = stationPosition.getPosition().distance(state.getPVCoordinates().getPosition()); + double staticTimeOfFlight = staticDistance / Constants.SPEED_OF_LIGHT; + + //Transmit (TX) + AngularAzEl azElTx = new AngularAzEl(gs, state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat, TimeTagSpecificationType.TX); + //Transmit Receive Apparent (TXRX) + AngularAzEl azElTxRx = new AngularAzEl(gs, state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat, TimeTagSpecificationType.TXRX); + //Receive (RX) + AngularAzEl azElRx = new AngularAzEl(gs, state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat, TimeTagSpecificationType.RX); + //Transit (Bounce) + AngularAzEl azElT = new AngularAzEl(gs, state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat, TimeTagSpecificationType.TRANSIT); + + EstimatedMeasurement estAzElTx = azElTx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estAzElTxRx = azElTxRx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estAzElRx = azElRx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estAzElTransit = azElT.estimate(0, 0, new SpacecraftState[]{state}); + + //Calculate Range calculated at transit and receive by shifting the state forward/backwards assuming time of flight = r/c + SpacecraftState tx = state.shiftedBy(staticTimeOfFlight); + SpacecraftState rx = state.shiftedBy(-staticTimeOfFlight); + + //Calculate the expected values, varying the estimated state + the time the ground station position is evaluated at. + + //state.getDate() == transmit (TX) time, state.getDate().shiftedBy(2*staticTimeOfFlight) == reception time + double transitAzTxRx = MathUtils.normalizeAngle(gs.getBaseFrame().getAzimuth(tx.getPVCoordinates().getPosition(), tx.getFrame(), state.getDate().shiftedBy(2*staticTimeOfFlight)), 0.0); + double transitElTxRx = MathUtils.normalizeAngle(gs.getBaseFrame().getElevation(tx.getPVCoordinates().getPosition(), tx.getFrame(), state.getDate().shiftedBy(2*staticTimeOfFlight)), 0.0); + + //state.getDate() == transmit (TX) time + double transitAzTx = MathUtils.normalizeAngle(gs.getBaseFrame().getAzimuth(tx.getPVCoordinates().getPosition(), tx.getFrame(), state.getDate()), 0.0); + double transitElTx = MathUtils.normalizeAngle(gs.getBaseFrame().getElevation(tx.getPVCoordinates().getPosition(), tx.getFrame(), state.getDate()), 0.0); + + //state.getDate() == reception time + double transitAzRx = MathUtils.normalizeAngle(gs.getBaseFrame().getAzimuth(rx.getPVCoordinates().getPosition(), rx.getFrame(), state.getDate()), 0.0); + double transitElRx = MathUtils.normalizeAngle(gs.getBaseFrame().getElevation(rx.getPVCoordinates().getPosition(), rx.getFrame(), state.getDate()), 0.0); + + //state.getDate() == transit time + double transitAzT = MathUtils.normalizeAngle(gs.getBaseFrame().getAzimuth(state.getPVCoordinates().getPosition(), state.getFrame(), state.getDate()), 0.0); + double transitElT = MathUtils.normalizeAngle(gs.getBaseFrame().getElevation(state.getPVCoordinates().getPosition(), state.getFrame(), state.getDate()), 0.0); + + + //Static time of flight does not take into account motion during tof. Very small differences expected however + //delta expected vs actual <<< difference between TX, RX and transit predictions by a few orders of magnitude. + Assert.assertEquals("TX", transitAzTx, estAzElTx.getEstimatedValue()[0], 2e-8); + Assert.assertEquals("TX", transitElTx, estAzElTx.getEstimatedValue()[1], 2e-8); + + Assert.assertEquals("TXRX", transitAzTxRx, estAzElTxRx.getEstimatedValue()[0], 2e-8); + Assert.assertEquals("TXRX", transitElTxRx, estAzElTxRx.getEstimatedValue()[1], 2e-8); + + Assert.assertEquals("RX", transitAzRx, estAzElRx.getEstimatedValue()[0], 2e-8); + Assert.assertEquals("RX", transitElRx, estAzElRx.getEstimatedValue()[1], 2e-8); + + Assert.assertEquals("Transit", transitAzT, estAzElTransit.getEstimatedValue()[0], 2e-8); + Assert.assertEquals("Transit", transitElT, estAzElTransit.getEstimatedValue()[1], 2e-8); + + //Test providing pre corrected states + an arbitarily shifted case - since this should have no significant effect on the value. + EstimatedMeasurement estAzElTxShifted = azElTx.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(staticTimeOfFlight)}); + EstimatedMeasurement estAzElRxShifted = azElRx.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(-staticTimeOfFlight)}); + EstimatedMeasurement estAzElTransitShifted = azElT.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(0.1)}); + + //tolerances are required since shifting the state forwards and backwards produces slight estimated value changes + Assert.assertEquals("TX shifted", estAzElTxShifted.getEstimatedValue()[0], estAzElTx.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("TX shifted", estAzElTxShifted.getEstimatedValue()[1], estAzElTx.getEstimatedValue()[1], 1e-11); + + Assert.assertEquals("RX shifted", estAzElRxShifted.getEstimatedValue()[0], estAzElRx.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("RX shifted", estAzElRxShifted.getEstimatedValue()[1], estAzElRx.getEstimatedValue()[1], 1e-11); + + Assert.assertEquals("Transit shifted", estAzElTransitShifted.getEstimatedValue()[0], estAzElTransit.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("Transit shifted", estAzElTransitShifted.getEstimatedValue()[1], estAzElTransit.getEstimatedValue()[1], 1e-11); + + //Show the effect of the change in time tag specification is far greater than the test tolerance due to usage + //of a static time of flight correction. + Assert.assertTrue("Proof of difference - Azimuth", (Math.abs(transitAzRx - transitAzTx) > 1e-5)); + Assert.assertTrue("Proof of difference - Elevation", (Math.abs(transitAzRx - transitAzTx) > 1e-5)); + } + } } diff --git a/src/test/java/org/orekit/estimation/measurements/AngularRaDecTest.java b/src/test/java/org/orekit/estimation/measurements/AngularRaDecTest.java index a62b572ec3c13c97d5112426245c5c39459316c3..d4e34339731c75ce0f9a0eb31152c9e8ba212a0d 100644 --- a/src/test/java/org/orekit/estimation/measurements/AngularRaDecTest.java +++ b/src/test/java/org/orekit/estimation/measurements/AngularRaDecTest.java @@ -22,20 +22,21 @@ import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.stat.descriptive.StreamingStatistics; import org.hipparchus.stat.descriptive.rank.Median; import org.hipparchus.util.FastMath; +import org.hipparchus.util.MathUtils; import org.junit.Assert; import org.junit.Test; import org.orekit.estimation.Context; import org.orekit.estimation.EstimationTestUtils; +import org.orekit.frames.Frame; +import org.orekit.frames.FramesFactory; +import org.orekit.frames.Transform; import org.orekit.orbits.OrbitType; import org.orekit.orbits.PositionAngle; import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.conversion.NumericalPropagatorBuilder; import org.orekit.time.AbsoluteDate; -import org.orekit.utils.Differentiation; -import org.orekit.utils.ParameterDriver; -import org.orekit.utils.ParameterFunction; -import org.orekit.utils.StateFunction; +import org.orekit.utils.*; public class AngularRaDecTest { @@ -265,5 +266,107 @@ public class AngularRaDecTest { } } } -} + /** + * Test the estimated values when the observed angular ra dec value is provided at TX (Transmit), + * RX (Receive (default)), transit (bounce) + */ + @Test + public void testTimeTagSpecifications(){ + Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); + SpacecraftState state = new SpacecraftState(context.initialOrbit); + ObservableSatellite obsSat = new ObservableSatellite(0); + + for (GroundStation gs : context.getStations()) { + + gs.getPrimeMeridianOffsetDriver().setReferenceDate(state.getDate()); + gs.getPrimeMeridianDriftDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetXDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftXDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetYDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftYDriver().setReferenceDate(state.getDate()); + + Transform gsToInertialTransform = gs.getOffsetToInertial(state.getFrame(), state.getDate()); + TimeStampedPVCoordinates stationPosition = gsToInertialTransform.transformPVCoordinates(new TimeStampedPVCoordinates(state.getDate(), Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO)); + + double staticDistance = stationPosition.getPosition().distance(state.getPVCoordinates().getPosition()); + double staticTimeOfFlight = staticDistance / Constants.SPEED_OF_LIGHT; + + AngularRaDec raDecTx = new AngularRaDec(gs, state.getFrame(), state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat,TimeTagSpecificationType.TX); + AngularRaDec raDecTxRx = new AngularRaDec(gs, state.getFrame(), state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat,TimeTagSpecificationType.TXRX); + AngularRaDec raDecRx = new AngularRaDec(gs, state.getFrame(), state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat,TimeTagSpecificationType.RX); + AngularRaDec raDecT = new AngularRaDec(gs, state.getFrame(), state.getDate(), new double[]{0.0,0.0}, new double[]{0.0,0.0},new double[]{0.0,0.0},obsSat,TimeTagSpecificationType.TRANSIT); + + EstimatedMeasurement estRaDecTx = raDecTx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estRaDecTxRx = raDecTxRx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estRaDecRx = raDecRx.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estRaDecTransit = raDecT.estimate(0, 0, new SpacecraftState[]{state}); + + //Calculate Range calculated at transit and receive by shifting the state forward/backwards assuming time of flight = r/c + SpacecraftState tx = state.shiftedBy(staticTimeOfFlight); + SpacecraftState rx = state.shiftedBy(-staticTimeOfFlight); + + //evaluate station position at different times for different obs time tags + //state.getDate()==transmit (TX) + double[] transitRaDecTx = calcRaDec(tx, stationPosition.getPosition(), tx.getFrame(), state.getDate()); + //state.getDate()==transmit (TX). state.getDate().shiftedBy(2*staticTimeOfFlight) == receive. + double[] transitRaDecTxRx = calcRaDec(tx, stationPosition.shiftedBy(2*staticTimeOfFlight).getPosition(), + tx.getFrame(), state.getDate().shiftedBy(2*staticTimeOfFlight)); + //state.getDate()==receive + double[] transitRaDecRx = calcRaDec(rx, stationPosition.getPosition(), tx.getFrame(), state.getDate()); + //state.getDate()==transit + double[] transitRaDecT = calcRaDec(state, stationPosition.getPosition(), tx.getFrame(), state.getDate()); + + //Static time of flight does not take into account motion during tof. Very small differences expected however + //delta expected vs actual <<< difference between TX, RX and transit predictions by a few orders of magnitude. + + Assert.assertEquals("TX", transitRaDecTx[0], estRaDecTx.getEstimatedValue()[0], 1e-9); + Assert.assertEquals("TX", transitRaDecTx[1], estRaDecTx.getEstimatedValue()[1], 1e-9); + + Assert.assertEquals("TXRX", transitRaDecTxRx[0], estRaDecTxRx.getEstimatedValue()[0], 1e-9); + Assert.assertEquals("TXRX", transitRaDecTxRx[1], estRaDecTxRx.getEstimatedValue()[1], 1e-9); + + Assert.assertEquals("RX", transitRaDecRx[0], estRaDecRx.getEstimatedValue()[0], 1e-9); + Assert.assertEquals("RX", transitRaDecRx[1], estRaDecRx.getEstimatedValue()[1], 1e-9); + + Assert.assertEquals("Transit", transitRaDecT[0], estRaDecTransit.getEstimatedValue()[0], 1e-9); + Assert.assertEquals("Transit", transitRaDecT[1], estRaDecTransit.getEstimatedValue()[1], 1e-9); + + //Test providing pre corrected states + an arbitarily shifted case - since this should have no significant effect on the value. + EstimatedMeasurement estRaDecTxShifted = raDecTx.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(staticTimeOfFlight)}); + EstimatedMeasurement estRaDecRxShifted = raDecRx.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(-staticTimeOfFlight)}); + EstimatedMeasurement estRaDecTransitShifted = raDecT.estimate(0, 0, new SpacecraftState[]{state.shiftedBy(0.1)}); + + //tolerances are required since shifting the state forwards and backwards produces slight estimated value changes + Assert.assertEquals("TX shifted", estRaDecTxShifted.getEstimatedValue()[0], estRaDecTx.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("TX shifted", estRaDecTxShifted.getEstimatedValue()[1], estRaDecTx.getEstimatedValue()[1], 1e-11); + + Assert.assertEquals("RX shifted", estRaDecRxShifted.getEstimatedValue()[0], estRaDecRx.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("RX shifted", estRaDecRxShifted.getEstimatedValue()[1], estRaDecRx.getEstimatedValue()[1], 1e-11); + + Assert.assertEquals("Transit shifted", estRaDecTransitShifted.getEstimatedValue()[0], estRaDecTransit.getEstimatedValue()[0], 1e-11); + Assert.assertEquals("Transit shifted", estRaDecTransitShifted.getEstimatedValue()[1], estRaDecTransit.getEstimatedValue()[1], 1e-11); + + + //Show the effect of the change in time tag specification is far greater than the test tolerance due to usage + //of a static time of flight correction. + Assert.assertTrue("Proof of difference - RA", (Math.abs(transitRaDecRx[0] - transitRaDecTx[0]) > 1e-5)); + Assert.assertTrue("Proof of difference - DEC", (Math.abs(transitRaDecRx[1] - transitRaDecTx[1]) > 1e-5)); + } + } + + private static double[] calcRaDec(SpacecraftState state, Vector3D station, Frame referenceFrame, AbsoluteDate date){ + Vector3D statePos = state.getPVCoordinates().getPosition(); + Vector3D staSatInertial = statePos.subtract(station); + + // Field transform from inertial to reference frame at station's reception date + Transform inertialToReferenceDownlink = + state.getFrame().getTransformTo(referenceFrame, date); + + // Station-satellite vector in reference frame + Vector3D raDec = inertialToReferenceDownlink.transformPosition(staSatInertial); + return new double[]{raDec.getAlpha(), raDec.getDelta()}; + } +} \ No newline at end of file diff --git a/src/test/java/org/orekit/estimation/measurements/RangeRateTest.java b/src/test/java/org/orekit/estimation/measurements/RangeRateTest.java index 067ae79b6d6e4b8d3380cbb5bbc0cafba9f20107..de58841537454c8d51e8d9e5e28e43892f4d1d68 100644 --- a/src/test/java/org/orekit/estimation/measurements/RangeRateTest.java +++ b/src/test/java/org/orekit/estimation/measurements/RangeRateTest.java @@ -18,6 +18,7 @@ package org.orekit.estimation.measurements; import java.util.List; +import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.hipparchus.stat.descriptive.StreamingStatistics; import org.hipparchus.util.FastMath; import org.junit.Assert; @@ -25,6 +26,7 @@ import org.junit.Test; import org.orekit.estimation.Context; import org.orekit.estimation.EstimationTestUtils; import org.orekit.estimation.measurements.modifiers.RangeRateTroposphericDelayModifier; +import org.orekit.frames.Transform; import org.orekit.models.earth.troposphere.EstimatedTroposphericModel; import org.orekit.models.earth.troposphere.GlobalMappingFunctionModel; import org.orekit.models.earth.troposphere.SaastamoinenModel; @@ -34,11 +36,7 @@ import org.orekit.propagation.Propagator; import org.orekit.propagation.SpacecraftState; import org.orekit.propagation.conversion.NumericalPropagatorBuilder; import org.orekit.time.AbsoluteDate; -import org.orekit.utils.Constants; -import org.orekit.utils.Differentiation; -import org.orekit.utils.ParameterDriver; -import org.orekit.utils.ParameterFunction; -import org.orekit.utils.StateFunction; +import org.orekit.utils.*; public class RangeRateTest { @@ -722,6 +720,81 @@ public class RangeRateTest { } + /** + * Test the estimated values when the observed range rate value is provided at TX (Transmit), + * RX (Receive (default)), transit (bounce) + */ + @Test + public void testTimeTagSpecifications(){ + + Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); + SpacecraftState state = new SpacecraftState(context.initialOrbit); + ObservableSatellite obsSat = new ObservableSatellite(0); + + for (GroundStation gs : context.getStations()){ + + gs.getPrimeMeridianOffsetDriver().setReferenceDate(state.getDate()); + gs.getPrimeMeridianDriftDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetXDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftXDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetYDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftYDriver().setReferenceDate(state.getDate()); + + Transform gsToInertialTransform = gs.getOffsetToInertial(state.getFrame(), state.getDate()); + TimeStampedPVCoordinates stationPosition = gsToInertialTransform.transformPVCoordinates(new TimeStampedPVCoordinates(state.getDate(), Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO)); + + double staticDistance = stationPosition.getPosition().distance(state.getPVCoordinates().getPosition()); + double staticTimeOfFlight = staticDistance / Constants.SPEED_OF_LIGHT; + + RangeRate rangeRateTX = new RangeRate(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.TX); + RangeRate rangeRateRX = new RangeRate(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.RX); + RangeRate rangeRateTransit = new RangeRate(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.TRANSIT); + + EstimatedMeasurement estRangeRateTX = rangeRateTX.estimate(0,0,new SpacecraftState[]{state}); + EstimatedMeasurement estRangeRateRX = rangeRateRX.estimate(0,0,new SpacecraftState[]{state}); + EstimatedMeasurement estRangeRateTransit = rangeRateTransit.estimate(0,0,new SpacecraftState[]{state}); + + //Calculate Range Rate calculated at transit and receive by shifting the state forward/backwards assuming time of flight = r/c + SpacecraftState tx = state.shiftedBy(staticTimeOfFlight); + SpacecraftState rx = state.shiftedBy(-staticTimeOfFlight); + + double transitRangeRateTX = calcRangeRate(tx.getPVCoordinates(), stationPosition.shiftedBy(staticTimeOfFlight)); + double transitRangeRateRX = calcRangeRate(rx.getPVCoordinates(), stationPosition.shiftedBy(-staticTimeOfFlight)); + double transitRangeRateT = calcRangeRate(state.getPVCoordinates(), stationPosition); + + //Static time of flight does not take into account motion during tof. Very small differences expected however + //delta expected vs actual <<< difference between TX, RX and transit predictions by a few orders of magnitude. + Assert.assertEquals("TX", transitRangeRateTX, estRangeRateTX.getEstimatedValue()[0],1e-6); + Assert.assertEquals("RX", transitRangeRateRX, estRangeRateRX.getEstimatedValue()[0],1e-6); + Assert.assertEquals("Transit", transitRangeRateT, estRangeRateTransit.getEstimatedValue()[0], 1e-11); + + //Test for case in which the state has been precorrected for propagation delay / an arbitary shift + EstimatedMeasurement estRangeRateTXPrecorr = rangeRateTX.estimate(0,0,new SpacecraftState[]{state.shiftedBy(staticTimeOfFlight)}); + EstimatedMeasurement estRangeRateRXPrecorr = rangeRateRX.estimate(0,0,new SpacecraftState[]{state.shiftedBy(-staticTimeOfFlight)}); + EstimatedMeasurement estRangeRateTransitShift = rangeRateTransit.estimate(0,0,new SpacecraftState[]{state.shiftedBy(0.05)}); + + //tolerances are required since shifting the state forwards and backwards produces slight estimated value changes + Assert.assertEquals("TX shifted", estRangeRateTXPrecorr.getEstimatedValue()[0], estRangeRateTX.getEstimatedValue()[0],1e-6); + Assert.assertEquals("RX shifted", estRangeRateRXPrecorr.getEstimatedValue()[0], estRangeRateRX.getEstimatedValue()[0],1e-6); + Assert.assertEquals("Transit shifted", estRangeRateTransitShift.getEstimatedValue()[0], estRangeRateTransit.getEstimatedValue()[0], 1e-6); + + //Show the effect of the change in time tag specification is far greater than the test tolerance due to usage + //of a static time of flight correction. + Assert.assertTrue(Math.abs(transitRangeRateRX - transitRangeRateTX) > 5e-3); + } + + } + + // range rate value + private double calcRangeRate(TimeStampedPVCoordinates state, TimeStampedPVCoordinates station){ + Vector3D relativePosition = station.getPosition().subtract(state.getPosition()); + Vector3D relativeVelocity = station.getVelocity().subtract(state.getVelocity()); + Vector3D lineOfSight = relativePosition.normalize(); + return Vector3D.dotProduct(relativeVelocity, lineOfSight); + } + } diff --git a/src/test/java/org/orekit/estimation/measurements/RangeTest.java b/src/test/java/org/orekit/estimation/measurements/RangeTest.java index 6e82d09cc3e20ed20df51c442e9569633cf473b9..61274470eb33de254dd827b7a562c62fd6dcab3b 100644 --- a/src/test/java/org/orekit/estimation/measurements/RangeTest.java +++ b/src/test/java/org/orekit/estimation/measurements/RangeTest.java @@ -32,6 +32,7 @@ import org.junit.Test; import org.orekit.estimation.Context; import org.orekit.estimation.EstimationTestUtils; import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier; +import org.orekit.frames.Transform; import org.orekit.models.earth.troposphere.EstimatedTroposphericModel; import org.orekit.models.earth.troposphere.NiellMappingFunctionModel; import org.orekit.models.earth.troposphere.SaastamoinenModel; @@ -78,14 +79,14 @@ public class RangeTest { // Run test boolean isModifier = false; double refErrorsPMedian = 6.5e-10; - double refErrorsPMean = 4.1e-09; - double refErrorsPMax = 2.1e-07; + double refErrorsPMean = 4.1e-09; + double refErrorsPMax = 2.1e-07; double refErrorsVMedian = 2.2e-04; - double refErrorsVMean = 6.2e-04; - double refErrorsVMax = 1.3e-02; + double refErrorsVMean = 6.2e-04; + double refErrorsVMax = 1.3e-02; this.genericTestStateDerivatives(isModifier, printResults, - refErrorsPMedian, refErrorsPMean, refErrorsPMax, - refErrorsVMedian, refErrorsVMean, refErrorsVMax); + refErrorsPMedian, refErrorsPMean, refErrorsPMax, + refErrorsVMedian, refErrorsVMean, refErrorsVMax); } /** @@ -102,14 +103,14 @@ public class RangeTest { // Run test boolean isModifier = true; double refErrorsPMedian = 6.2e-10; - double refErrorsPMean = 3.5e-09; - double refErrorsPMax = 1.6e-07; + double refErrorsPMean = 3.5e-09; + double refErrorsPMax = 1.6e-07; double refErrorsVMedian = 2.2e-04; - double refErrorsVMean = 6.2e-04; - double refErrorsVMax = 1.3e-02; + double refErrorsVMean = 6.2e-04; + double refErrorsVMax = 1.3e-02; this.genericTestStateDerivatives(isModifier, printResults, - refErrorsPMedian, refErrorsPMean, refErrorsPMax, - refErrorsVMedian, refErrorsVMean, refErrorsVMax); + refErrorsPMedian, refErrorsPMean, refErrorsPMax, + refErrorsVMedian, refErrorsVMean, refErrorsVMax); } /** @@ -128,10 +129,10 @@ public class RangeTest { // Run test boolean isModifier = false; double refErrorsMedian = 5.7e-9; - double refErrorsMean = 6.4e-8; - double refErrorsMax = 5.1e-6; + double refErrorsMean = 6.4e-8; + double refErrorsMax = 5.1e-6; this.genericTestParameterDerivatives(isModifier, printResults, - refErrorsMedian, refErrorsMean, refErrorsMax); + refErrorsMedian, refErrorsMean, refErrorsMax); } @@ -151,10 +152,10 @@ public class RangeTest { // Run test boolean isModifier = true; double refErrorsMedian = 2.9e-8; - double refErrorsMean = 4.9e-7; - double refErrorsMax = 1.5e-5; + double refErrorsMean = 4.9e-7; + double refErrorsMax = 1.5e-5; this.genericTestParameterDerivatives(isModifier, printResults, - refErrorsMedian, refErrorsMean, refErrorsMax); + refErrorsMedian, refErrorsMean, refErrorsMax); } @@ -174,15 +175,16 @@ public class RangeTest { // Run test boolean isModifier = true; double refErrorsMedian = 1.2e-9; - double refErrorsMean = 1.9e-9; - double refErrorsMax = 6.6e-9; + double refErrorsMean = 1.9e-9; + double refErrorsMax = 6.6e-9; this.genericTestEstimatedParameterDerivatives(isModifier, printResults, - refErrorsMedian, refErrorsMean, refErrorsMax); + refErrorsMedian, refErrorsMean, refErrorsMax); } /** * Generic test function for values of the range + * * @param printResults Print the results ? */ void genericTestValues(final boolean printResults) { @@ -190,20 +192,20 @@ public class RangeTest { Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); final NumericalPropagatorBuilder propagatorBuilder = - context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, - 1.0e-6, 60.0, 0.001); + context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, + 1.0e-6, 60.0, 0.001); // Create perfect range measurements final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, - propagatorBuilder); + propagatorBuilder); final double groundClockOffset = 1.234e-3; for (final GroundStation station : context.stations) { station.getClockOffsetDriver().setValue(groundClockOffset); } final List> measurements = - EstimationTestUtils.createMeasurements(propagator, - new RangeMeasurementCreator(context, Vector3D.ZERO), - 1.0, 3.0, 300.0); + EstimationTestUtils.createMeasurements(propagator, + new RangeMeasurementCreator(context, Vector3D.ZERO), + 1.0, 3.0, 300.0); // Lists for results' storage - Used only for derivatives with respect to state // "final" value to be seen by "handleStep" function of the propagator @@ -218,8 +220,8 @@ public class RangeTest { // Play test if the measurement date is between interpolator previous and current date if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && - (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) - ) { + (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) + ) { // We intentionally propagate to a date which is close to the // real spacecraft state but is *not* the accurate date, by // compensating only part of the downlink delay. This is done @@ -227,19 +229,19 @@ public class RangeTest { // to velocity. If we had chosen the proper state date, the // range would have depended only on the current position but // not on the current velocity. - final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; - final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); - final SpacecraftState state = interpolator.getInterpolatedState(date); + final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; + final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); + final SpacecraftState state = interpolator.getInterpolatedState(date); // Values of the Range & errors - final double RangeObserved = measurement.getObservedValue()[0]; - final EstimatedMeasurement estimated = measurement.estimate(0, 0, new SpacecraftState[] { state }); + final double RangeObserved = measurement.getObservedValue()[0]; + final EstimatedMeasurement estimated = measurement.estimate(0, 0, new SpacecraftState[]{state}); final TimeStampedPVCoordinates[] participants = estimated.getParticipants(); Assert.assertEquals(3, participants.length); Assert.assertEquals(0.5 * Constants.SPEED_OF_LIGHT * participants[2].getDate().durationFrom(participants[0].getDate()), - estimated.getEstimatedValue()[0], - 2.0e-8); + estimated.getEstimatedValue()[0], + 2.0e-8); // the real state used for estimation is adjusted according to downlink delay double adjustment = state.getDate().durationFrom(estimated.getStates()[0].getDate()); @@ -247,9 +249,9 @@ public class RangeTest { Assert.assertTrue(adjustment < 0.011); final double RangeEstimated = estimated.getEstimatedValue()[0]; - final double absoluteError = RangeEstimated-RangeObserved; + final double absoluteError = RangeEstimated - RangeObserved; absoluteErrors.add(absoluteError); - relativeErrors.add(FastMath.abs(absoluteError)/FastMath.abs(RangeObserved)); + relativeErrors.add(FastMath.abs(absoluteError) / FastMath.abs(RangeObserved)); // Print results on console ? if (printResults) { @@ -257,10 +259,10 @@ public class RangeTest { String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); System.out.format(Locale.US, "%-15s %-23s %-23s %19.6f %19.6f %13.6e %13.6e%n", - stationName, measurementDate, date, - RangeObserved, RangeEstimated, - FastMath.abs(RangeEstimated-RangeObserved), - FastMath.abs((RangeEstimated-RangeObserved)/RangeObserved)); + stationName, measurementDate, date, + RangeObserved, RangeEstimated, + FastMath.abs(RangeEstimated - RangeObserved), + FastMath.abs((RangeEstimated - RangeObserved) / RangeObserved)); } } // End if measurement date between previous and current interpolator step @@ -270,9 +272,9 @@ public class RangeTest { // Print results on console ? Header if (printResults) { System.out.format(Locale.US, "%-15s %-23s %-23s %19s %19s %13s %13s%n", - "Station", "Measurement Date", "State Date", - "Range observed [m]", "Range estimated [m]", - "ΔRange [m]", "rel ΔRange"); + "Station", "Measurement Date", "State Date", + "Range observed [m]", "Range estimated [m]", + "ΔRange [m]", "rel ΔRange"); } // Rewind the propagator to initial date @@ -282,7 +284,7 @@ public class RangeTest { measurements.sort(Comparator.naturalOrder()); // Propagate to final measurement's date - propagator.propagate(measurements.get(measurements.size()-1).getDate()); + propagator.propagate(measurements.get(measurements.size() - 1).getDate()); // Convert lists to double array final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray(); @@ -290,26 +292,26 @@ public class RangeTest { // Statistics' assertion final double absErrorsMedian = new Median().evaluate(absErrors); - final double absErrorsMin = new Min().evaluate(absErrors); - final double absErrorsMax = new Max().evaluate(absErrors); + final double absErrorsMin = new Min().evaluate(absErrors); + final double absErrorsMax = new Max().evaluate(absErrors); final double relErrorsMedian = new Median().evaluate(relErrors); - final double relErrorsMax = new Max().evaluate(relErrors); + final double relErrorsMax = new Max().evaluate(relErrors); // Print the results on console ? Final results if (printResults) { System.out.println(); - System.out.println("Absolute errors median: " + absErrorsMedian); - System.out.println("Absolute errors min : " + absErrorsMin); - System.out.println("Absolute errors max : " + absErrorsMax); - System.out.println("Relative errors median: " + relErrorsMedian); - System.out.println("Relative errors max : " + relErrorsMax); + System.out.println("Absolute errors median: " + absErrorsMedian); + System.out.println("Absolute errors min : " + absErrorsMin); + System.out.println("Absolute errors max : " + absErrorsMax); + System.out.println("Relative errors median: " + relErrorsMedian); + System.out.println("Relative errors max : " + relErrorsMax); } Assert.assertEquals(0.0, absErrorsMedian, 4.9e-8); - Assert.assertEquals(0.0, absErrorsMin, 2.2e-7); - Assert.assertEquals(0.0, absErrorsMax, 2.1e-7); + Assert.assertEquals(0.0, absErrorsMin, 2.2e-7); + Assert.assertEquals(0.0, absErrorsMax, 2.1e-7); Assert.assertEquals(0.0, relErrorsMedian, 1.0e-14); - Assert.assertEquals(0.0, relErrorsMax, 2.6e-14); + Assert.assertEquals(0.0, relErrorsMax, 2.6e-14); } @@ -321,16 +323,16 @@ public class RangeTest { Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); final NumericalPropagatorBuilder propagatorBuilder = - context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, - 1.0e-6, 60.0, 0.001); + context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, + 1.0e-6, 60.0, 0.001); // Create perfect range measurements final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, - propagatorBuilder); + propagatorBuilder); final List> measurements = - EstimationTestUtils.createMeasurements(propagator, - new RangeMeasurementCreator(context), - 1.0, 3.0, 300.0); + EstimationTestUtils.createMeasurements(propagator, + new RangeMeasurementCreator(context), + 1.0, 3.0, 300.0); // Lists for results' storage - Used only for derivatives with respect to state // "final" value to be seen by "handleStep" function of the propagator @@ -345,8 +347,8 @@ public class RangeTest { // Play test if the measurement date is between interpolator previous and current date if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && - (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) - ) { + (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) + ) { // Add modifiers if test implies it final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel()); @@ -361,50 +363,53 @@ public class RangeTest { // to velocity. If we had chosen the proper state date, the // range would have depended only on the current position but // not on the current velocity. - final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; - final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); - final SpacecraftState state = interpolator.getInterpolatedState(date); - final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0); + final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; + final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); + final SpacecraftState state = interpolator.getInterpolatedState(date); + final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[]{state}).getStateDerivatives(0); // Jacobian reference value final double[][] jacobianRef; // Compute a reference value using finite differences jacobianRef = Differentiation.differentiate(new StateFunction() { - public double[] value(final SpacecraftState state) { - return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue(); - } - }, measurement.getDimension(), propagator.getAttitudeProvider(), - OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state); + public double[] value(final SpacecraftState state) { + return measurement.estimate(0, 0, new SpacecraftState[]{state}).getEstimatedValue(); + } + }, measurement.getDimension(), propagator.getAttitudeProvider(), + OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state); Assert.assertEquals(jacobianRef.length, jacobian.length); Assert.assertEquals(jacobianRef[0].length, jacobian[0].length); // Errors & relative errors on the Jacobian - double [][] dJacobian = new double[jacobian.length][jacobian[0].length]; - double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length]; + double[][] dJacobian = new double[jacobian.length][jacobian[0].length]; + double[][] dJacobianRelative = new double[jacobian.length][jacobian[0].length]; for (int i = 0; i < jacobian.length; ++i) { for (int j = 0; j < jacobian[i].length; ++j) { dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j]; - dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]); + dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j] / jacobianRef[i][j]); - if (j < 3) { errorsP.add(dJacobianRelative[i][j]); - } else { errorsV.add(dJacobianRelative[i][j]); } + if (j < 3) { + errorsP.add(dJacobianRelative[i][j]); + } else { + errorsV.add(dJacobianRelative[i][j]); + } } } // Print values in console ? if (printResults) { - String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); + String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); System.out.format(Locale.US, "%-15s %-23s %-23s " + "%10.3e %10.3e %10.3e " + "%10.3e %10.3e %10.3e " + "%10.3e %10.3e %10.3e " + "%10.3e %10.3e %10.3e%n", - stationName, measurement.getDate(), date, - dJacobian[0][0], dJacobian[0][1], dJacobian[0][2], - dJacobian[0][3], dJacobian[0][4], dJacobian[0][5], - dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2], - dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]); + stationName, measurement.getDate(), date, + dJacobian[0][0], dJacobian[0][1], dJacobian[0][2], + dJacobian[0][3], dJacobian[0][4], dJacobian[0][5], + dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2], + dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]); } } // End if measurement date between previous and current interpolator step } // End for loop on the measurements @@ -417,10 +422,10 @@ public class RangeTest { "%10s %10s %10s " + "%10s %10s %10s " + "%10s %10s %10s%n", - "Station", "Measurement Date", "State Date", - "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz", - "rel ΔdPx", "rel ΔdPy", "rel ΔdPz", - "rel ΔdVx", "rel ΔdVy", "rel ΔdVz"); + "Station", "Measurement Date", "State Date", + "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz", + "rel ΔdPx", "rel ΔdPy", "rel ΔdPz", + "rel ΔdVx", "rel ΔdVy", "rel ΔdVz"); } // Rewind the propagator to initial date @@ -430,26 +435,26 @@ public class RangeTest { measurements.sort(Comparator.naturalOrder()); // Propagate to final measurement's date - propagator.propagate(measurements.get(measurements.size()-1).getDate()); + propagator.propagate(measurements.get(measurements.size() - 1).getDate()); // Convert lists to double[] and evaluate some statistics final double relErrorsP[] = errorsP.stream().mapToDouble(Double::doubleValue).toArray(); final double relErrorsV[] = errorsV.stream().mapToDouble(Double::doubleValue).toArray(); final double errorsPMedian = new Median().evaluate(relErrorsP); - final double errorsPMean = new Mean().evaluate(relErrorsP); - final double errorsPMax = new Max().evaluate(relErrorsP); + final double errorsPMean = new Mean().evaluate(relErrorsP); + final double errorsPMax = new Max().evaluate(relErrorsP); final double errorsVMedian = new Median().evaluate(relErrorsV); - final double errorsVMean = new Mean().evaluate(relErrorsV); - final double errorsVMax = new Max().evaluate(relErrorsV); + final double errorsVMean = new Mean().evaluate(relErrorsV); + final double errorsVMax = new Max().evaluate(relErrorsV); // Print the results on console ? if (printResults) { System.out.println(); System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", - errorsPMedian, errorsPMean, errorsPMax); + errorsPMedian, errorsPMean, errorsPMax); System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", - errorsVMedian, errorsVMean, errorsVMax); + errorsVMedian, errorsVMean, errorsVMax); } Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian); @@ -466,8 +471,8 @@ public class RangeTest { Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); final NumericalPropagatorBuilder propagatorBuilder = - context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, - 1.0e-6, 60.0, 0.001); + context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, + 1.0e-6, 60.0, 0.001); // Create perfect range measurements for (final GroundStation station : context.stations) { @@ -477,11 +482,11 @@ public class RangeTest { station.getZenithOffsetDriver().setSelected(true); } final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, - propagatorBuilder); + propagatorBuilder); final List> measurements = - EstimationTestUtils.createMeasurements(propagator, - new RangeMeasurementCreator(context), - 1.0, 3.0, 300.0); + EstimationTestUtils.createMeasurements(propagator, + new RangeMeasurementCreator(context), + 1.0, 3.0, 300.0); // List to store the results final List relErrorList = new ArrayList(); @@ -494,8 +499,8 @@ public class RangeTest { // Play test if the measurement date is between interpolator previous and current date if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && - (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) - ) { + (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) + ) { // Add modifiers if test implies it final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel()); @@ -513,43 +518,43 @@ public class RangeTest { // to velocity. If we had chosen the proper state date, the // range would have depended only on the current position but // not on the current velocity. - final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; - final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); - final SpacecraftState state = interpolator.getInterpolatedState(date); - final ParameterDriver[] drivers = new ParameterDriver[] { - station.getClockOffsetDriver(), - station.getEastOffsetDriver(), - station.getNorthOffsetDriver(), - station.getZenithOffsetDriver() + final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; + final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); + final SpacecraftState state = interpolator.getInterpolatedState(date); + final ParameterDriver[] drivers = new ParameterDriver[]{ + station.getClockOffsetDriver(), + station.getEastOffsetDriver(), + station.getNorthOffsetDriver(), + station.getZenithOffsetDriver() }; if (printResults) { - String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); + String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); System.out.format(Locale.US, "%-15s %-23s %-23s ", - stationName, measurement.getDate(), date); + stationName, measurement.getDate(), date); } for (int i = 0; i < drivers.length; ++i) { - final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]); + final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[]{state}).getParameterDerivatives(drivers[i]); Assert.assertEquals(1, measurement.getDimension()); Assert.assertEquals(1, gradient.length); // Compute a reference value using finite differences final ParameterFunction dMkdP = - Differentiation.differentiate(new ParameterFunction() { - /** {@inheritDoc} */ - @Override - public double value(final ParameterDriver parameterDriver) { - return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0]; - } - }, 3, 20.0 * drivers[i].getScale()); + Differentiation.differentiate(new ParameterFunction() { + /** {@inheritDoc} */ + @Override + public double value(final ParameterDriver parameterDriver) { + return measurement.estimate(0, 0, new SpacecraftState[]{state}).getEstimatedValue()[0]; + } + }, 3, 20.0 * drivers[i].getScale()); final double ref = dMkdP.value(drivers[i]); if (printResults) { - System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref)); + System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0] - ref, FastMath.abs((gradient[0] - ref) / ref)); } - final double relError = FastMath.abs((ref-gradient[0])/ref); + final double relError = FastMath.abs((ref - gradient[0]) / ref); relErrorList.add(relError); Assert.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref)); } @@ -570,30 +575,30 @@ public class RangeTest { // Print results ? Header if (printResults) { System.out.format(Locale.US, "%-15s %-23s %-23s " + - "%10s %10s %10s %10s %10s %10s %10s %10s%n", - "Station", "Measurement Date", "State Date", - "Δt", "rel Δt", - "ΔdQx", "rel ΔdQx", - "ΔdQy", "rel ΔdQy", - "ΔdQz", "rel ΔdQz"); - } + "%10s %10s %10s %10s %10s %10s %10s %10s%n", + "Station", "Measurement Date", "State Date", + "Δt", "rel Δt", + "ΔdQx", "rel ΔdQx", + "ΔdQy", "rel ΔdQy", + "ΔdQz", "rel ΔdQz"); + } // Propagate to final measurement's date - propagator.propagate(measurements.get(measurements.size()-1).getDate()); + propagator.propagate(measurements.get(measurements.size() - 1).getDate()); // Convert error list to double[] final double relErrors[] = relErrorList.stream().mapToDouble(Double::doubleValue).toArray(); // Compute statistics final double relErrorsMedian = new Median().evaluate(relErrors); - final double relErrorsMean = new Mean().evaluate(relErrors); - final double relErrorsMax = new Max().evaluate(relErrors); + final double relErrorsMean = new Mean().evaluate(relErrors); + final double relErrorsMax = new Max().evaluate(relErrors); // Print the results on console ? if (printResults) { System.out.println(); System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", - relErrorsMedian, relErrorsMean, relErrorsMax); + relErrorsMedian, relErrorsMean, relErrorsMax); } Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian); @@ -603,21 +608,20 @@ public class RangeTest { } void genericTestEstimatedParameterDerivatives(final boolean isModifier, final boolean printResults, - final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) - { + final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) { Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); final NumericalPropagatorBuilder propagatorBuilder = - context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, - 1.0e-6, 60.0, 0.001); + context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, + 1.0e-6, 60.0, 0.001); final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit, - propagatorBuilder); + propagatorBuilder); final List> measurements = - EstimationTestUtils.createMeasurements(propagator, - new RangeMeasurementCreator(context), - 1.0, 3.0, 300.0); + EstimationTestUtils.createMeasurements(propagator, + new RangeMeasurementCreator(context), + 1.0, 3.0, 300.0); // List to store the results final List relErrorList = new ArrayList(); @@ -630,15 +634,15 @@ public class RangeTest { // Play test if the measurement date is between interpolator previous and current date if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) && - (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) - ) { + (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.) + ) { - String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); + String stationName = ((Range) measurement).getStation().getBaseFrame().getName(); // Add modifiers if test implies it final NiellMappingFunctionModel mappingFunction = new NiellMappingFunctionModel(); - final EstimatedTroposphericModel tropoModel = new EstimatedTroposphericModel(mappingFunction, 5.0); - + final EstimatedTroposphericModel tropoModel = new EstimatedTroposphericModel(mappingFunction, 5.0); + final List parameters = tropoModel.getParametersDrivers(); for (ParameterDriver driver : parameters) { driver.setSelected(true); @@ -657,39 +661,39 @@ public class RangeTest { // to velocity. If we had chosen the proper state date, the // range would have depended only on the current position but // not on the current velocity. - final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; - final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); - final SpacecraftState state = interpolator.getInterpolatedState(date); - final ParameterDriver[] drivers = new ParameterDriver[] { - parameters.get(0) + final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT; + final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay); + final SpacecraftState state = interpolator.getInterpolatedState(date); + final ParameterDriver[] drivers = new ParameterDriver[]{ + parameters.get(0) }; if (printResults) { System.out.format(Locale.US, "%-15s %-23s %-23s ", - stationName, measurement.getDate(), date); + stationName, measurement.getDate(), date); } for (int i = 0; i < 1; ++i) { - final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]); + final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[]{state}).getParameterDerivatives(drivers[i]); Assert.assertEquals(1, measurement.getDimension()); Assert.assertEquals(1, gradient.length); // Compute a reference value using finite differences final ParameterFunction dMkdP = - Differentiation.differentiate(new ParameterFunction() { - /** {@inheritDoc} */ - @Override - public double value(final ParameterDriver parameterDriver) { - return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0]; - } - }, 3, 0.1 * drivers[i].getScale()); + Differentiation.differentiate(new ParameterFunction() { + /** {@inheritDoc} */ + @Override + public double value(final ParameterDriver parameterDriver) { + return measurement.estimate(0, 0, new SpacecraftState[]{state}).getEstimatedValue()[0]; + } + }, 3, 0.1 * drivers[i].getScale()); final double ref = dMkdP.value(drivers[i]); if (printResults) { - System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref)); + System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0] - ref, FastMath.abs((gradient[0] - ref) / ref)); } - final double relError = FastMath.abs((ref-gradient[0])/ref); + final double relError = FastMath.abs((ref - gradient[0]) / ref); relErrorList.add(relError); // Assert.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref)); } @@ -712,28 +716,28 @@ public class RangeTest { System.out.format(Locale.US, "%-15s %-23s %-23s " + "%10s %10s %10s " + "%10s %10s %10s%n ", - "Station", "Measurement Date", "State Date", - "ΔdDelay", "rel ΔdDelay", - "ΔdNorthG", "rel ΔdNorthG", - "ΔdEastG", "rel ΔdEastG"); - } + "Station", "Measurement Date", "State Date", + "ΔdDelay", "rel ΔdDelay", + "ΔdNorthG", "rel ΔdNorthG", + "ΔdEastG", "rel ΔdEastG"); + } // Propagate to final measurement's date - propagator.propagate(measurements.get(measurements.size()-1).getDate()); + propagator.propagate(measurements.get(measurements.size() - 1).getDate()); // Convert error list to double[] final double relErrors[] = relErrorList.stream().mapToDouble(Double::doubleValue).toArray(); // Compute statistics final double relErrorsMedian = new Median().evaluate(relErrors); - final double relErrorsMean = new Mean().evaluate(relErrors); - final double relErrorsMax = new Max().evaluate(relErrors); + final double relErrorsMean = new Mean().evaluate(relErrors); + final double relErrorsMax = new Max().evaluate(relErrors); // Print the results on console ? if (printResults) { System.out.println(); System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n", - relErrorsMedian, relErrorsMean, relErrorsMax); + relErrorsMedian, relErrorsMean, relErrorsMax); } Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian); @@ -742,4 +746,71 @@ public class RangeTest { } + + /** + * Test the estimated values when the observed range value is provided at TX (Transmit), + * RX (Receive (default)), transit (bounce) + */ + @Test + public void testTimeTagSpecifications() { + + Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides"); + SpacecraftState state = new SpacecraftState(context.initialOrbit); + ObservableSatellite obsSat = new ObservableSatellite(0); + + for (GroundStation gs : context.getStations()) { + + gs.getPrimeMeridianOffsetDriver().setReferenceDate(state.getDate()); + gs.getPrimeMeridianDriftDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetXDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftXDriver().setReferenceDate(state.getDate()); + + gs.getPolarOffsetYDriver().setReferenceDate(state.getDate()); + gs.getPolarDriftYDriver().setReferenceDate(state.getDate()); + + Transform gsToInertialTransform = gs.getOffsetToInertial(state.getFrame(), state.getDate()); + TimeStampedPVCoordinates stationPosition = gsToInertialTransform.transformPVCoordinates(new TimeStampedPVCoordinates(state.getDate(), Vector3D.ZERO, Vector3D.ZERO, Vector3D.ZERO)); + + double staticDistance = stationPosition.getPosition().distance(state.getPVCoordinates().getPosition()); + double staticTimeOfFlight = staticDistance / Constants.SPEED_OF_LIGHT; + + Range rangeTX = new Range(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.TX); + Range rangeRX = new Range(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.RX); + Range rangeTransit = new Range(gs, state.getDate(), 1.0, 1.0, 1.0, obsSat, TimeTagSpecificationType.TRANSIT); + + EstimatedMeasurement estRangeTX = rangeTX.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estRangeRX = rangeRX.estimate(0, 0, new SpacecraftState[]{state}); + EstimatedMeasurement estRangeTransit = rangeTransit.estimate(0, 0, new SpacecraftState[]{state}); + + //Calculate Range calculated at transit and receive by shifting the state forward/backwards assuming time of flight = r/c + SpacecraftState tx = state.shiftedBy(staticTimeOfFlight); + SpacecraftState rx = state.shiftedBy(-staticTimeOfFlight); + + double transitRangeTX = tx.getPVCoordinates().getPosition().distance(stationPosition.shiftedBy(staticTimeOfFlight).getPosition()); + double transitRangeRX = rx.getPVCoordinates().getPosition().distance(stationPosition.shiftedBy(-staticTimeOfFlight).getPosition()); + double transitRangeT = state.getPVCoordinates().getPosition().distance(stationPosition.getPosition()); + + //Static time of flight does not take into account motion during tof. Very small differences expected however + //delta expected vs actual <<< difference between TX, RX and transit predictions by a few orders of magnitude. + Assert.assertEquals("TX", transitRangeTX, estRangeTX.getEstimatedValue()[0], 1e-3); + Assert.assertEquals("RX", transitRangeRX, estRangeRX.getEstimatedValue()[0], 1e-3); + Assert.assertEquals("Transit", transitRangeT, estRangeTransit.getEstimatedValue()[0], 1e-6); + + //Test providing pre corrected states + an arbitarily shifted case - since this should have no significant effect on the value. + EstimatedMeasurement estRangeTXPreCorr = rangeTX.estimate(0,0,new SpacecraftState[]{state.shiftedBy(staticTimeOfFlight)}); + EstimatedMeasurement estRangeRXPreCorr = rangeRX.estimate(0,0,new SpacecraftState[]{state.shiftedBy(-staticTimeOfFlight)}); + EstimatedMeasurement estRangeTransitPreCorr = rangeTransit.estimate(0,0,new SpacecraftState[]{state.shiftedBy(0.1)}); + + //tolerances are required since shifting the state forwards and backwards produces slight estimated value changes + Assert.assertEquals("TX shifted", estRangeTXPreCorr.getEstimatedValue()[0], estRangeTX.getEstimatedValue()[0],1e-7); + Assert.assertEquals("RX shifted", estRangeRXPreCorr.getEstimatedValue()[0], estRangeRX.getEstimatedValue()[0],1e-7); + Assert.assertEquals("Transit shifted", estRangeTransitPreCorr.getEstimatedValue()[0], estRangeTransit.getEstimatedValue()[0], 1e-7); + + //Show the effect of the change in time tag specification is far greater than the test tolerance due to usage + //of a static time of flight correction. + Assert.assertTrue(Math.abs(transitRangeTX - transitRangeRX)>100.0); + } + + } }