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.stat.descriptive.moment.Mean;
20 import org.hipparchus.stat.descriptive.rank.Max;
21 import org.hipparchus.stat.descriptive.rank.Median;
22 import org.hipparchus.stat.descriptive.rank.Min;
23 import org.hipparchus.util.FastMath;
24 import org.junit.jupiter.api.Assertions;
25 import org.junit.jupiter.api.BeforeEach;
26 import org.junit.jupiter.api.Test;
27 import org.orekit.Utils;
28 import org.orekit.estimation.Context;
29 import org.orekit.estimation.EstimationTestUtils;
30 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
31 import org.orekit.models.earth.troposphere.ModifiedSaastamoinenModel;
32 import org.orekit.models.earth.troposphere.TroposphericModelUtils;
33 import org.orekit.orbits.OrbitType;
34 import org.orekit.orbits.PositionAngleType;
35 import org.orekit.propagation.Propagator;
36 import org.orekit.propagation.SpacecraftState;
37 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.utils.Constants;
40 import org.orekit.utils.Differentiation;
41 import org.orekit.utils.ParameterDriver;
42 import org.orekit.utils.ParameterFunction;
43 import org.orekit.utils.TimeStampedPVCoordinates;
44
45 import java.util.ArrayList;
46 import java.util.Comparator;
47 import java.util.List;
48 import java.util.Locale;
49
50 public class Range2Test {
51
52 @BeforeEach
53 public void setUp() {
54
55 Utils.setDataRoot("gnss:rinex");
56 }
57
58
59
60
61
62 @Test
63 public void testValues() {
64 boolean printResults = false;
65 if (printResults) {
66 System.out.println("\nTest Range Values\n");
67 }
68
69 this.genericTestValues(printResults);
70 }
71
72
73
74
75
76 @Test
77 public void testStateDerivatives() {
78
79 boolean printResults = false;
80 if (printResults) {
81 System.out.println("\nTest Range State Derivatives - Finite Differences Comparison\n");
82 }
83
84 boolean isModifier = false;
85 double refErrorsPMedian = 6.3e-10;
86 double refErrorsPMean = 4.2e-09;
87 double refErrorsPMax = 2.8e-07;
88 double refErrorsVMedian = 1.4e-04;
89 double refErrorsVMean = 9.6e-04;
90 double refErrorsVMax = 5.2e-02;
91 this.genericTestStateDerivatives(isModifier, printResults,
92 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
93 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
94 }
95
96
97
98
99
100 @Test
101 public void testStateDerivativesWithModifier() {
102
103 boolean printResults = false;
104 if (printResults) {
105 System.out.println("\nTest Range State Derivatives with Modifier - Finite Differences Comparison\n");
106 }
107
108 boolean isModifier = true;
109 double refErrorsPMedian = 5.5e-10;
110 double refErrorsPMean = 4.6e-09;
111 double refErrorsPMax = 2.1e-07;
112 double refErrorsVMedian = 1.3e-04;
113 double refErrorsVMean = 9.4e-04;
114 double refErrorsVMax = 4.8e-02;
115 this.genericTestStateDerivatives(isModifier, printResults,
116 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
117 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
118 }
119
120
121
122
123
124 @Test
125 public void testParameterDerivatives() {
126
127
128 boolean printResults = false;
129
130 if (printResults) {
131 System.out.println("\nTest Range Parameter Derivatives - Finite Differences Comparison\n");
132 }
133
134 boolean isModifier = false;
135 double refErrorsMedian = 5.8e-9;
136 double refErrorsMean = 6.4e-8;
137 double refErrorsMax = 5.1e-6;
138 this.genericTestParameterDerivatives(isModifier, printResults,
139 refErrorsMedian, refErrorsMean, refErrorsMax);
140
141 }
142
143
144
145
146
147 @Test
148 public void testParameterDerivativesWithModifier() {
149
150
151 boolean printResults = false;
152
153 if (printResults) {
154 System.out.println("\nTest Range Parameter Derivatives with Modifier - Finite Differences Comparison\n");
155 }
156
157 boolean isModifier = true;
158 double refErrorsMedian = 2.9e-8;
159 double refErrorsMean = 4.9e-7;
160 double refErrorsMax = 1.5e-5;
161 this.genericTestParameterDerivatives(isModifier, printResults,
162 refErrorsMedian, refErrorsMean, refErrorsMax);
163
164 }
165
166
167
168
169
170 void genericTestValues(final boolean printResults)
171 {
172
173 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
174
175 final NumericalPropagatorBuilder propagatorBuilder =
176 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
177 1.0e-6, 60.0, 0.001);
178
179
180 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
181 propagatorBuilder);
182 final List<ObservedMeasurement<?>> measurements =
183 EstimationTestUtils.createMeasurements(propagator,
184 new OneWayRangeMeasurementCreator(context),
185 1.0, 3.0, 300.0);
186
187
188
189 final List<Double> absoluteErrors = new ArrayList<>();
190 final List<Double> relativeErrors = new ArrayList<>();
191
192
193
194 propagator.setStepHandler(interpolator -> {
195
196 for (final ObservedMeasurement<?> measurement : measurements) {
197
198
199 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
200 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
201 ) {
202
203
204
205
206
207
208
209 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
210 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
211 final SpacecraftState state = interpolator.getInterpolatedState(date);
212
213
214 final double RangeObserved = measurement.getObservedValue()[0];
215 final EstimatedMeasurementBase<?> estimated = measurement.estimateWithoutDerivatives(new SpacecraftState[] { state });
216
217 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
218 Assertions.assertEquals(2, participants.length);
219 Assertions.assertEquals(Constants.SPEED_OF_LIGHT * participants[1].getDate().durationFrom(participants[0].getDate()),
220 estimated.getEstimatedValue()[0],
221 2.0e-8);
222
223
224 double adjustment = state.getDate().durationFrom(estimated.getStates()[0].getDate());
225 Assertions.assertTrue(adjustment > 0.006);
226 Assertions.assertTrue(adjustment < 0.010);
227
228 final double RangeEstimated = estimated.getEstimatedValue()[0];
229 final double absoluteError = RangeEstimated-RangeObserved;
230 absoluteErrors.add(absoluteError);
231 relativeErrors.add(FastMath.abs(absoluteError)/FastMath.abs(RangeObserved));
232
233
234 if (printResults) {
235 final AbsoluteDate measurementDate = measurement.getDate();
236 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
237
238 System.out.format(Locale.US, "%-15s %-23s %-23s %19.6f %19.6f %13.6e %13.6e%n",
239 stationName, measurementDate, date,
240 RangeObserved, RangeEstimated,
241 FastMath.abs(RangeEstimated-RangeObserved),
242 FastMath.abs((RangeEstimated-RangeObserved)/RangeObserved));
243 }
244
245 }
246 }
247 });
248
249
250
251
252 if (printResults) {
253 System.out.format(Locale.US, "%-15s %-23s %-23s %19s %19s %13s %13s%n",
254 "Station", "Measurement Date", "State Date",
255 "Range observed [m]", "Range estimated [m]",
256 "ΔRange [m]", "rel ΔRange");
257 }
258
259 propagator.propagate(context.initialOrbit.getDate());
260
261
262 measurements.sort(Comparator.naturalOrder());
263
264
265 propagator.propagate(measurements.get(measurements.size()-1).getDate());
266
267
268 final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
269 final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
270
271
272 final double absErrorsMedian = new Median().evaluate(absErrors);
273 final double absErrorsMin = new Min().evaluate(absErrors);
274 final double absErrorsMax = new Max().evaluate(absErrors);
275 final double relErrorsMedian = new Median().evaluate(relErrors);
276 final double relErrorsMax = new Max().evaluate(relErrors);
277
278
279 if (printResults) {
280 System.out.println();
281 System.out.println("Absolute errors median: " + absErrorsMedian);
282 System.out.println("Absolute errors min : " + absErrorsMin);
283 System.out.println("Absolute errors max : " + absErrorsMax);
284 System.out.println("Relative errors median: " + relErrorsMedian);
285 System.out.println("Relative errors max : " + relErrorsMax);
286 }
287
288 Assertions.assertEquals(0.0, absErrorsMedian, 4.9e-8);
289 Assertions.assertEquals(0.0, absErrorsMin, 2.7e-7);
290 Assertions.assertEquals(0.0, absErrorsMax, 3.0e-7);
291 Assertions.assertEquals(0.0, relErrorsMedian, 1.0e-14);
292 Assertions.assertEquals(0.0, relErrorsMax, 3.1e-14);
293
294 }
295
296 void genericTestStateDerivatives(final boolean isModifier, final boolean printResults,
297 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
298 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax)
299 {
300
301 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
302
303 final NumericalPropagatorBuilder propagatorBuilder =
304 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
305 1.0e-6, 60.0, 0.001);
306
307
308 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
309 propagatorBuilder);
310 final List<ObservedMeasurement<?>> measurements =
311 EstimationTestUtils.createMeasurements(propagator,
312 new OneWayRangeMeasurementCreator(context),
313 1.0, 3.0, 300.0);
314
315
316
317 final List<Double> errorsP = new ArrayList<>();
318 final List<Double> errorsV = new ArrayList<>();
319
320
321
322 propagator.setStepHandler(interpolator -> {
323
324 for (final ObservedMeasurement<?> measurement : measurements) {
325
326
327 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
328 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
329 ) {
330
331
332 final RangeTroposphericDelayModifier modifier =
333 new RangeTroposphericDelayModifier(new ModifiedSaastamoinenModel(TroposphericModelUtils.STANDARD_ATMOSPHERE_PROVIDER));
334 if (isModifier) {
335 ((Range) measurement).addModifier(modifier);
336 }
337
338
339
340
341
342
343
344
345 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
346 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
347 final SpacecraftState state = interpolator.getInterpolatedState(date);
348 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
349
350
351 final double[][] jacobianRef;
352
353
354 jacobianRef = Differentiation.differentiate(state1 -> measurement.
355 estimateWithoutDerivatives(new SpacecraftState[] { state1 }).
356 getEstimatedValue(), measurement.getDimension(), propagator.getAttitudeProvider(),
357 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(state);
358
359 Assertions.assertEquals(jacobianRef.length, jacobian.length);
360 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
361
362
363 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
364 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
365 for (int i = 0; i < jacobian.length; ++i) {
366 for (int j = 0; j < jacobian[i].length; ++j) {
367 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
368 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
369
370 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
371 } else { errorsV.add(dJacobianRelative[i][j]); }
372 }
373 }
374
375 if (printResults) {
376 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
377 System.out.format(Locale.US, "%-15s %-23s %-23s " +
378 "%10.3e %10.3e %10.3e " +
379 "%10.3e %10.3e %10.3e " +
380 "%10.3e %10.3e %10.3e " +
381 "%10.3e %10.3e %10.3e%n",
382 stationName, measurement.getDate(), date,
383 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
384 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
385 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
386 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
387 }
388 }
389 }
390 });
391
392
393 if (printResults) {
394 System.out.format(Locale.US, "%-15s %-23s %-23s " +
395 "%10s %10s %10s " +
396 "%10s %10s %10s " +
397 "%10s %10s %10s " +
398 "%10s %10s %10s%n",
399 "Station", "Measurement Date", "State Date",
400 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
401 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
402 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
403 }
404
405
406 propagator.propagate(context.initialOrbit.getDate());
407
408
409 measurements.sort(Comparator.naturalOrder());
410
411
412 propagator.propagate(measurements.get(measurements.size()-1).getDate());
413
414
415 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
416 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
417
418 final double errorsPMedian = new Median().evaluate(relErrorsP);
419 final double errorsPMean = new Mean().evaluate(relErrorsP);
420 final double errorsPMax = new Max().evaluate(relErrorsP);
421 final double errorsVMedian = new Median().evaluate(relErrorsV);
422 final double errorsVMean = new Mean().evaluate(relErrorsV);
423 final double errorsVMax = new Max().evaluate(relErrorsV);
424
425
426 if (printResults) {
427 System.out.println();
428 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
429 errorsPMedian, errorsPMean, errorsPMax);
430 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
431 errorsVMedian, errorsVMean, errorsVMax);
432 }
433
434 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
435 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
436 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
437 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
438 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
439 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
440 }
441
442 void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults,
443 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
444
445 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
446
447 final NumericalPropagatorBuilder propagatorBuilder =
448 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
449 1.0e-6, 60.0, 0.001);
450
451
452 for (final GroundStation station : context.stations) {
453 station.getClockOffsetDriver().setSelected(true);
454 station.getEastOffsetDriver().setSelected(true);
455 station.getNorthOffsetDriver().setSelected(true);
456 station.getZenithOffsetDriver().setSelected(true);
457 }
458 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
459 propagatorBuilder);
460 final List<ObservedMeasurement<?>> measurements =
461 EstimationTestUtils.createMeasurements(propagator,
462 new OneWayRangeMeasurementCreator(context),
463 1.0, 3.0, 300.0);
464
465
466 final List<Double> relErrorList = new ArrayList<>();
467
468
469
470 propagator.setStepHandler(interpolator -> {
471
472 for (final ObservedMeasurement<?> measurement : measurements) {
473
474
475 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
476 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
477 ) {
478
479
480 final RangeTroposphericDelayModifier modifier =
481 new RangeTroposphericDelayModifier(ModifiedSaastamoinenModel.getStandardModel());
482 if (isModifier) {
483 ((Range) measurement).addModifier(modifier);
484 }
485
486
487 final GroundStation stationParameter = ((Range) measurement).getStation();
488
489
490
491
492
493
494
495
496 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
497 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
498 final SpacecraftState state = interpolator.getInterpolatedState(date);
499 final ParameterDriver[] drivers = new ParameterDriver[] {
500 stationParameter.getClockOffsetDriver(),
501 stationParameter.getEastOffsetDriver(),
502 stationParameter.getNorthOffsetDriver(),
503 stationParameter.getZenithOffsetDriver()
504 };
505
506 if (printResults) {
507 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
508 System.out.format(Locale.US, "%-15s %-23s %-23s ",
509 stationName, measurement.getDate(), date);
510 }
511
512 for (int i = 0; i < drivers.length; ++i) {
513 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
514 Assertions.assertEquals(1, measurement.getDimension());
515 Assertions.assertEquals(1, gradient.length);
516
517
518 final ParameterFunction dMkdP =
519 Differentiation.differentiate(new ParameterFunction() {
520
521 @Override
522 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
523 return measurement.
524 estimateWithoutDerivatives(new SpacecraftState[] { state }).
525 getEstimatedValue()[0];
526 }
527 }, 3, 20.0 * drivers[i].getScale());
528 final double ref = dMkdP.value(drivers[i], date);
529
530 if (printResults) {
531 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
532 }
533
534 final double relError = FastMath.abs((ref-gradient[0])/ref);
535 relErrorList.add(relError);
536
537 }
538 if (printResults) {
539 System.out.format(Locale.US, "%n");
540 }
541
542 }
543 }
544 });
545
546
547 propagator.propagate(context.initialOrbit.getDate());
548
549
550 measurements.sort(Comparator.naturalOrder());
551
552
553 if (printResults) {
554 System.out.format(Locale.US, "%-15s %-23s %-23s " +
555 "%10s %10s %10s %10s %10s %10s %10s %10s%n",
556 "Station", "Measurement Date", "State Date",
557 "Δt", "rel Δt",
558 "ΔdQx", "rel ΔdQx",
559 "ΔdQy", "rel ΔdQy",
560 "ΔdQz", "rel ΔdQz");
561 }
562
563
564 propagator.propagate(measurements.get(measurements.size()-1).getDate());
565
566
567 final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
568
569
570 final double relErrorsMedian = new Median().evaluate(relErrors);
571 final double relErrorsMean = new Mean().evaluate(relErrors);
572 final double relErrorsMax = new Max().evaluate(relErrors);
573
574
575 if (printResults) {
576 System.out.println();
577 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
578 relErrorsMedian, relErrorsMean, relErrorsMax);
579 }
580
581 Assertions.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
582 Assertions.assertEquals(0.0, relErrorsMean, refErrorsMean);
583 Assertions.assertEquals(0.0, relErrorsMax, refErrorsMax);
584
585 }
586
587 }