1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.modifiers;
18
19 import java.util.Arrays;
20 import java.util.List;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.analysis.differentiation.Gradient;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.orekit.attitudes.FrameAlignedProvider;
28 import org.orekit.estimation.measurements.EstimatedMeasurement;
29 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
30 import org.orekit.estimation.measurements.EstimationModifier;
31 import org.orekit.estimation.measurements.GroundStation;
32 import org.orekit.estimation.measurements.TurnAroundRange;
33 import org.orekit.models.earth.troposphere.TroposphericModel;
34 import org.orekit.propagation.FieldSpacecraftState;
35 import org.orekit.propagation.SpacecraftState;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.utils.Differentiation;
38 import org.orekit.utils.FieldTrackingCoordinates;
39 import org.orekit.utils.ParameterDriver;
40 import org.orekit.utils.ParameterFunction;
41 import org.orekit.utils.TimeSpanMap.Span;
42 import org.orekit.utils.TrackingCoordinates;
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57 public class TurnAroundRangeTroposphericDelayModifier implements EstimationModifier<TurnAroundRange> {
58
59
60 private final TroposphericModel tropoModel;
61
62
63
64
65
66
67 @Deprecated
68 public TurnAroundRangeTroposphericDelayModifier(final org.orekit.models.earth.troposphere.DiscreteTroposphericModel model) {
69 this(new org.orekit.models.earth.troposphere.TroposphericModelAdapter(model));
70 }
71
72
73
74
75
76
77 public TurnAroundRangeTroposphericDelayModifier(final TroposphericModel model) {
78 tropoModel = model;
79 }
80
81
82
83
84
85
86 private double rangeErrorTroposphericModel(final GroundStation station, final SpacecraftState state) {
87
88 final Vector3D position = state.getPosition();
89
90
91 final TrackingCoordinates trackingCoordinates =
92 station.getBaseFrame().getTrackingCoordinates(position, state.getFrame(), state.getDate());
93
94
95 if (trackingCoordinates.getElevation() > 0) {
96
97 return tropoModel.pathDelay(trackingCoordinates,
98 station.getOffsetGeodeticPoint(state.getDate()),
99 station.getPressureTemperatureHumidity(state.getDate()),
100 tropoModel.getParameters(state.getDate()), state.getDate()).
101 getDelay();
102 }
103
104 return 0;
105 }
106
107
108
109
110
111
112
113
114 private <T extends CalculusFieldElement<T>> T rangeErrorTroposphericModel(final GroundStation station,
115 final FieldSpacecraftState<T> state,
116 final T[] parameters) {
117
118 final Field<T> field = state.getDate().getField();
119 final T zero = field.getZero();
120
121
122 final FieldVector3D<T> position = state.getPosition();
123 final FieldTrackingCoordinates<T> trackingCoordinates =
124 station.getBaseFrame().getTrackingCoordinates(position, state.getFrame(), state.getDate());
125
126
127 if (trackingCoordinates.getElevation().getReal() > 0) {
128
129 return tropoModel.pathDelay(trackingCoordinates,
130 station.getOffsetGeodeticPoint(state.getDate()),
131 station.getPressureTemperatureHumidity(state.getDate()),
132 parameters, state.getDate()).
133 getDelay();
134 }
135
136 return zero;
137 }
138
139
140
141
142
143
144
145
146 private double[][] rangeErrorJacobianState(final double[] derivatives) {
147 final double[][] finiteDifferencesJacobian = new double[1][6];
148 System.arraycopy(derivatives, 0, finiteDifferencesJacobian[0], 0, 6);
149 return finiteDifferencesJacobian;
150 }
151
152
153
154
155
156
157
158
159
160 private double rangeErrorParameterDerivative(final GroundStation station,
161 final ParameterDriver driver,
162 final SpacecraftState state) {
163
164 final ParameterFunction rangeError = new ParameterFunction() {
165
166 @Override
167 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
168 return rangeErrorTroposphericModel(station, state);
169 }
170 };
171
172 final ParameterFunction rangeErrorDerivative = Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());
173
174 return rangeErrorDerivative.value(driver, state.getDate());
175
176 }
177
178
179
180
181
182
183
184
185 private double[] rangeErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
186
187
188 return Arrays.copyOfRange(derivatives, freeStateParameters, derivatives.length);
189 }
190
191
192 @Override
193 public List<ParameterDriver> getParametersDrivers() {
194 return tropoModel.getParametersDrivers();
195 }
196
197
198 @Override
199 public void modifyWithoutDerivatives(final EstimatedMeasurementBase<TurnAroundRange> estimated) {
200
201 final TurnAroundRange measurement = estimated.getObservedMeasurement();
202 final GroundStation primaryStation = measurement.getPrimaryStation();
203 final GroundStation secondaryStation = measurement.getSecondaryStation();
204 final SpacecraftState state = estimated.getStates()[0];
205
206
207
208 final double[] newValue = estimated.getEstimatedValue();
209 final double primaryDelay = rangeErrorTroposphericModel(primaryStation, state);
210 final double secondaryDelay = rangeErrorTroposphericModel(secondaryStation, state);
211 newValue[0] = newValue[0] + primaryDelay + secondaryDelay;
212 estimated.modifyEstimatedValue(this, newValue);
213
214 }
215
216 @Override
217 public void modify(final EstimatedMeasurement<TurnAroundRange> estimated) {
218 final TurnAroundRange measurement = estimated.getObservedMeasurement();
219 final GroundStation primaryStation = measurement.getPrimaryStation();
220 final GroundStation secondaryStation = measurement.getSecondaryStation();
221 final SpacecraftState state = estimated.getStates()[0];
222
223 final double[] oldValue = estimated.getEstimatedValue();
224
225
226 final ModifierGradientConverter converter =
227 new ModifierGradientConverter(state, 6, new FrameAlignedProvider(state.getFrame()));
228 final FieldSpacecraftState<Gradient> gState = converter.getState(tropoModel);
229 final Gradient[] gParameters = converter.getParametersAtStateDate(gState, tropoModel);
230 final Gradient primaryGDelay = rangeErrorTroposphericModel(primaryStation, gState, gParameters);
231 final Gradient secondaryGDelay = rangeErrorTroposphericModel(secondaryStation, gState, gParameters);
232 final double[] primaryDerivatives = primaryGDelay.getGradient();
233 final double[] secondaryDerivatives = secondaryGDelay.getGradient();
234
235 final double[][] primaryDjac = rangeErrorJacobianState(primaryDerivatives);
236 final double[][] secondaryDjac = rangeErrorJacobianState(secondaryDerivatives);
237 final double[][] stateDerivatives = estimated.getStateDerivatives(0);
238 for (int irow = 0; irow < stateDerivatives.length; ++irow) {
239 for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
240 stateDerivatives[irow][jcol] += primaryDjac[irow][jcol] + secondaryDjac[irow][jcol];
241 }
242 }
243 estimated.setStateDerivatives(0, stateDerivatives);
244
245 int indexPrimary = 0;
246 for (final ParameterDriver driver : getParametersDrivers()) {
247 if (driver.isSelected()) {
248
249 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
250 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
251 final double[] derivatives = rangeErrorParameterDerivative(primaryDerivatives, converter.getFreeStateParameters());
252 parameterDerivative += derivatives[indexPrimary];
253 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
254 indexPrimary += 1;
255 }
256 }
257
258 }
259
260 int indexSecondary = 0;
261 for (final ParameterDriver driver : getParametersDrivers()) {
262 if (driver.isSelected()) {
263
264 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
265 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
266 final double[] derivatives = rangeErrorParameterDerivative(secondaryDerivatives, converter.getFreeStateParameters());
267 parameterDerivative += derivatives[indexSecondary];
268 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
269 indexSecondary += 1;
270 }
271 }
272
273 }
274
275
276 for (final ParameterDriver driver : Arrays.asList(primaryStation.getClockOffsetDriver(),
277 primaryStation.getEastOffsetDriver(),
278 primaryStation.getNorthOffsetDriver(),
279 primaryStation.getZenithOffsetDriver())) {
280 if (driver.isSelected()) {
281 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
282
283 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
284 parameterDerivative += rangeErrorParameterDerivative(primaryStation, driver, state);
285 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
286 }
287 }
288 }
289
290
291 for (final ParameterDriver driver : Arrays.asList(secondaryStation.getEastOffsetDriver(),
292 secondaryStation.getNorthOffsetDriver(),
293 secondaryStation.getZenithOffsetDriver())) {
294 if (driver.isSelected()) {
295 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
296
297 double parameterDerivative = estimated.getParameterDerivatives(driver, span.getStart())[0];
298 parameterDerivative += rangeErrorParameterDerivative(secondaryStation, driver, state);
299 estimated.setParameterDerivatives(driver, span.getStart(), parameterDerivative);
300 }
301 }
302 }
303
304
305
306 final double[] newValue = oldValue.clone();
307 newValue[0] = newValue[0] + primaryGDelay.getReal() + secondaryGDelay.getReal();
308 estimated.modifyEstimatedValue(this, newValue);
309
310 }
311
312 }