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