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.Field;
23 import org.hipparchus.RealFieldElement;
24 import org.hipparchus.analysis.differentiation.DerivativeStructure;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.orekit.estimation.measurements.EstimatedMeasurement;
28 import org.orekit.estimation.measurements.EstimationModifier;
29 import org.orekit.estimation.measurements.GroundStation;
30 import org.orekit.estimation.measurements.RangeRate;
31 import org.orekit.models.earth.DiscreteTroposphericModel;
32 import org.orekit.models.earth.TroposphericModel;
33 import org.orekit.orbits.OrbitType;
34 import org.orekit.orbits.PositionAngle;
35 import org.orekit.propagation.FieldSpacecraftState;
36 import org.orekit.propagation.Propagator;
37 import org.orekit.propagation.SpacecraftState;
38 import org.orekit.utils.Differentiation;
39 import org.orekit.utils.ParameterDriver;
40 import org.orekit.utils.ParameterFunction;
41 import org.orekit.utils.StateFunction;
42
43
44
45
46
47
48
49
50
51
52
53
54 public class RangeRateTroposphericDelayModifier implements EstimationModifier<RangeRate> {
55
56
57 private final DiscreteTroposphericModel tropoModel;
58
59
60 private final double fTwoWay;
61
62
63
64
65
66
67 public RangeRateTroposphericDelayModifier(final DiscreteTroposphericModel model, final boolean tw) {
68 tropoModel = model;
69 if (tw) {
70 fTwoWay = 2.;
71 } else {
72 fTwoWay = 1.;
73 }
74 }
75
76
77
78
79
80
81 private double getStationHeightAMSL(final GroundStation station) {
82
83 final double height = station.getBaseFrame().getPoint().getAltitude();
84 return height;
85 }
86
87
88
89
90
91
92
93 private <T extends RealFieldElement<T>> T getStationHeightAMSL(final Field<T> field, final GroundStation station) {
94
95 final T height = station.getBaseFrame().getPoint(field).getAltitude();
96 return height;
97 }
98
99
100
101
102
103
104 public double rangeRateErrorTroposphericModel(final GroundStation station,
105 final SpacecraftState state) {
106
107
108
109 final double dt = 10;
110
111
112 final double height = getStationHeightAMSL(station);
113
114
115 final Vector3D position = state.getPVCoordinates().getPosition();
116
117
118 final double elevation1 = station.getBaseFrame().getElevation(position,
119 state.getFrame(),
120 state.getDate());
121
122
123 if (elevation1 > 0) {
124
125 final double d1 = tropoModel.pathDelay(elevation1, height, tropoModel.getParameters(), state.getDate());
126
127
128 final SpacecraftState state2 = state.shiftedBy(dt);
129
130
131 final Vector3D position2 = state2.getPVCoordinates().getPosition();
132
133
134 final double elevation2 = station.getBaseFrame().getElevation(position2,
135 state2.getFrame(),
136 state2.getDate());
137
138
139 final double d2 = tropoModel.pathDelay(elevation2, height, tropoModel.getParameters(), state2.getDate());
140
141 return fTwoWay * (d2 - d1) / dt;
142 }
143
144 return 0;
145 }
146
147
148
149
150
151
152
153
154
155 public <T extends RealFieldElement<T>> T rangeRateErrorTroposphericModel(final GroundStation station,
156 final FieldSpacecraftState<T> state,
157 final T[] parameters) {
158
159 final Field<T> field = state.getDate().getField();
160 final T zero = field.getZero();
161
162
163
164
165 final double dt = 10;
166
167
168 final T height = getStationHeightAMSL(field, station);
169
170
171 final FieldVector3D<T> position = state.getPVCoordinates().getPosition();
172 final T elevation1 = station.getBaseFrame().getElevation(position,
173 state.getFrame(),
174 state.getDate());
175
176
177 if (elevation1.getReal() > 0) {
178
179 final T d1 = tropoModel.pathDelay(elevation1, height, parameters, state.getDate());
180
181
182 final FieldSpacecraftState<T> state2 = state.shiftedBy(dt);
183
184
185 final FieldVector3D<T> position2 = state2.getPVCoordinates().getPosition();
186
187
188 final T elevation2 = station.getBaseFrame().getElevation(position2,
189 state2.getFrame(),
190 state2.getDate());
191
192
193
194 final T d2 = tropoModel.pathDelay(elevation2, height, parameters, state2.getDate());
195
196 return (d2.subtract(d1)).divide(dt).multiply(fTwoWay);
197 }
198
199 return zero;
200 }
201
202
203
204
205
206
207
208 private double[][] rangeRateErrorJacobianState(final GroundStation station,
209 final SpacecraftState refstate) {
210 final double[][] finiteDifferencesJacobian =
211 Differentiation.differentiate(new StateFunction() {
212 public double[] value(final SpacecraftState state) {
213
214 final double value = rangeRateErrorTroposphericModel(station, state);
215
216 return new double[] {value };
217 }
218 }, 1, Propagator.DEFAULT_LAW, OrbitType.CARTESIAN,
219 PositionAngle.TRUE, 15.0, 3).value(refstate);
220
221 return finiteDifferencesJacobian;
222 }
223
224
225
226
227
228
229
230
231
232 private double[][] rangeRateErrorJacobianState(final double[] derivatives, final int freeStateParameters) {
233 final double[][] finiteDifferencesJacobian = new double[1][6];
234 for (int i = 0; i < freeStateParameters; i++) {
235
236 finiteDifferencesJacobian[0][i] = derivatives[i + 1];
237 }
238 return finiteDifferencesJacobian;
239 }
240
241
242
243
244
245
246
247
248 private double rangeRateErrorParameterDerivative(final GroundStation station,
249 final ParameterDriver driver,
250 final SpacecraftState state) {
251
252 final ParameterFunctionParameterFunction">ParameterFunction rangeError = new ParameterFunction() {
253
254 @Override
255 public double value(final ParameterDriver parameterDriver) {
256 return rangeRateErrorTroposphericModel(station, state);
257 }
258 };
259
260 final ParameterFunction rangeErrorDerivative =
261 Differentiation.differentiate(rangeError, 3, 10.0 * driver.getScale());
262
263 return rangeErrorDerivative.value(driver);
264
265 }
266
267
268
269
270
271
272
273
274 private double[] rangeRateErrorParameterDerivative(final double[] derivatives, final int freeStateParameters) {
275
276
277
278 final int dim = derivatives.length - 1 - freeStateParameters;
279 final double[] rangeError = new double[dim];
280
281 for (int i = 0; i < dim; i++) {
282 rangeError[i] = derivatives[1 + freeStateParameters + i];
283 }
284
285 return rangeError;
286 }
287
288
289 @Override
290 public List<ParameterDriver> getParametersDrivers() {
291 return tropoModel.getParametersDrivers();
292 }
293
294
295 @Override
296 public void modify(final EstimatedMeasurement<RangeRate> estimated) {
297 final RangeRate measurement = estimated.getObservedMeasurement();
298 final GroundStation station = measurement.getStation();
299 final SpacecraftState state = estimated.getStates()[0];
300
301 final double[] oldValue = estimated.getEstimatedValue();
302
303
304 final TroposphericDSConverterTroposphericDSConverter.html#TroposphericDSConverter">TroposphericDSConverter converter = new TroposphericDSConverter(state, 6, Propagator.DEFAULT_LAW);
305 final FieldSpacecraftState<DerivativeStructure> dsState = converter.getState(tropoModel);
306 final DerivativeStructure[] dsParameters = converter.getParameters(dsState, tropoModel);
307 final DerivativeStructure dsDelay = rangeRateErrorTroposphericModel(station, dsState, dsParameters);
308 final double[] derivatives = dsDelay.getAllDerivatives();
309
310 double[][] djac = new double[1][6];
311
312
313 if (tropoModel instanceof TroposphericModel) {
314 djac = rangeRateErrorJacobianState(station, state);
315 } else {
316 djac = rangeRateErrorJacobianState(derivatives, converter.getFreeStateParameters());
317 }
318 final double[][] stateDerivatives = estimated.getStateDerivatives(0);
319 for (int irow = 0; irow < stateDerivatives.length; ++irow) {
320 for (int jcol = 0; jcol < stateDerivatives[0].length; ++jcol) {
321 stateDerivatives[irow][jcol] += djac[irow][jcol];
322 }
323 }
324 estimated.setStateDerivatives(0, stateDerivatives);
325
326 int index = 0;
327 for (final ParameterDriver driver : getParametersDrivers()) {
328 if (driver.isSelected()) {
329
330 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
331 final double[] dDelaydP = rangeRateErrorParameterDerivative(derivatives, converter.getFreeStateParameters());
332 parameterDerivative += dDelaydP[index];
333 estimated.setParameterDerivatives(driver, parameterDerivative);
334 index += 1;
335 }
336
337 }
338
339 for (final ParameterDriver driver : Arrays.asList(station.getClockOffsetDriver(),
340 station.getEastOffsetDriver(),
341 station.getNorthOffsetDriver(),
342 station.getZenithOffsetDriver())) {
343 if (driver.isSelected()) {
344
345 double parameterDerivative = estimated.getParameterDerivatives(driver)[0];
346 parameterDerivative += rangeRateErrorParameterDerivative(station, driver, state);
347 estimated.setParameterDerivatives(driver, parameterDerivative);
348 }
349 }
350
351
352
353 final double[] newValue = oldValue.clone();
354 newValue[0] = newValue[0] + dsDelay.getReal();
355 estimated.setEstimatedValue(newValue);
356
357 }
358
359 }