1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import org.hipparchus.analysis.UnivariateFunction;
20 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
21 import org.hipparchus.analysis.solvers.UnivariateSolver;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.util.FastMath;
24 import org.orekit.estimation.StationDataProvider;
25 import org.orekit.frames.Frame;
26 import org.orekit.frames.Transform;
27 import org.orekit.gnss.antenna.PhaseCenterVariationFunction;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.time.AbsoluteDate;
30 import org.orekit.utils.Constants;
31 import org.orekit.utils.ParameterDriver;
32
33 public class TwoWayRangeMeasurementCreator extends MeasurementCreator {
34
35 private final StationDataProvider provider;
36 private final Vector3D stationMeanPosition;
37 private final PhaseCenterVariationFunction stationPhaseCenterVariation;
38 private final Vector3D satelliteMeanPosition;
39 private final PhaseCenterVariationFunction satellitePhaseCenterVariation;
40 private final ObservableSatellite satellite;
41 private final double bias;
42
43 public TwoWayRangeMeasurementCreator(final StationDataProvider context) {
44 this(context, Vector3D.ZERO, null, Vector3D.ZERO, null, 0.0);
45 }
46
47 public TwoWayRangeMeasurementCreator(final StationDataProvider provider,
48 final Vector3D stationMeanPosition, final PhaseCenterVariationFunction stationPhaseCenterVariation,
49 final Vector3D satelliteMeanPosition, final PhaseCenterVariationFunction satellitePhaseCenterVariation,
50 final double bias) {
51 this.provider = provider;
52 this.stationMeanPosition = stationMeanPosition;
53 this.stationPhaseCenterVariation = stationPhaseCenterVariation;
54 this.satelliteMeanPosition = satelliteMeanPosition;
55 this.satellitePhaseCenterVariation = satellitePhaseCenterVariation;
56 this.satellite = new ObservableSatellite(0);
57 this.bias = bias;
58 }
59
60 public StationDataProvider getStationDataProvider() {
61 return provider;
62 }
63
64 public void init(SpacecraftState s0, AbsoluteDate t, double step) {
65 for (final GroundStation station : provider.getStations()) {
66 for (ParameterDriver driver : station.getParametersDrivers()) {
67 if (driver.getReferenceDate() == null) {
68 driver.setReferenceDate(s0.getDate());
69 }
70 }
71 }
72 }
73
74 public void handleStep(final SpacecraftState currentState) {
75 for (final GroundStation station : provider.getStations()) {
76 final AbsoluteDate date = currentState.getDate();
77 final Frame inertial = currentState.getFrame();
78 final Vector3D position = currentState.toStaticTransform().getInverse().transformPosition(satelliteMeanPosition);
79
80 if (station.getBaseFrame().getTrackingCoordinates(position, inertial, date).getElevation() > FastMath.toRadians(30.0)) {
81 final UnivariateSolver solver = new BracketingNthOrderBrentSolver(1.0e-12, 5);
82
83 final double downLinkDelay = solveDownlinkDelay(station, currentState, stationMeanPosition);
84 final AbsoluteDate receptionDate = currentState.getDate().shiftedBy(downLinkDelay);
85 final Transform stationToInertReception = station.getOffsetToInertial(inertial, receptionDate, true);
86 final Vector3D stationAtReception = stationToInertReception.transformPosition(stationMeanPosition);
87 final double downLinkDistance = Vector3D.distance(position, stationAtReception);
88
89 final Vector3D satLosDown = currentState.toTransform().
90 transformVector(stationAtReception.subtract(position));
91 final double satPCVDown = satellitePhaseCenterVariation == null ?
92 0.0 :
93 satellitePhaseCenterVariation.value(0.5 * FastMath.PI - satLosDown.getDelta(),
94 satLosDown.getAlpha());
95 final Vector3D staLosDown = stationToInertReception.getInverse().
96 transformVector(position.subtract(stationAtReception));
97 final double staPCVDown = stationPhaseCenterVariation == null ?
98 0.0 :
99 stationPhaseCenterVariation.value(0.5 * FastMath.PI - staLosDown.getDelta(),
100 staLosDown.getAlpha());
101
102 final double correctedDownLinkDistance = downLinkDistance + satPCVDown + staPCVDown;
103
104 final double upLinkDelay = solveUplinkDelay(station, currentState, stationMeanPosition);
105 final AbsoluteDate emissionDate = currentState.getDate().shiftedBy(-upLinkDelay);
106 final Transform stationToInertEmission = station.getOffsetToInertial(inertial, emissionDate, true);
107 final Vector3D stationAtEmission = stationToInertEmission.transformPosition(stationMeanPosition);
108 final double upLinkDistance = Vector3D.distance(position, stationAtEmission);
109
110 final Vector3D staLosUp = stationToInertEmission.getInverse().
111 transformVector(position.subtract(stationAtEmission));
112 final double staPCVUp = stationPhaseCenterVariation == null ?
113 0.0 :
114 stationPhaseCenterVariation.value(0.5 * FastMath.PI - staLosUp.getDelta(),
115 staLosUp.getAlpha());
116 final Vector3D satLosUp = currentState.toTransform().
117 transformVector(stationAtEmission.subtract(position));
118 final double satPCVUp = satellitePhaseCenterVariation == null ?
119 0.0 :
120 satellitePhaseCenterVariation.value(0.5 * FastMath.PI - satLosUp.getDelta(),
121 satLosUp.getAlpha());
122
123 final double correctedUpLinkDistance = upLinkDistance + satPCVUp + staPCVUp;
124
125 final double clockOffset = station.getOffsetValue(receptionDate);
126 addMeasurement(new Range(station, true, receptionDate.shiftedBy(clockOffset),
127 0.5 * (correctedDownLinkDistance + correctedUpLinkDistance) + bias,
128 1.0, 10, satellite));
129
130 }
131
132 }
133 }
134
135 }