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