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.Before;
31 import org.junit.Test;
32 import org.orekit.Utils;
33 import org.orekit.estimation.Context;
34 import org.orekit.estimation.EstimationTestUtils;
35 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
36 import org.orekit.models.earth.troposphere.SaastamoinenModel;
37 import org.orekit.orbits.OrbitType;
38 import org.orekit.orbits.PositionAngle;
39 import org.orekit.propagation.Propagator;
40 import org.orekit.propagation.SpacecraftState;
41 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
42 import org.orekit.time.AbsoluteDate;
43 import org.orekit.utils.Constants;
44 import org.orekit.utils.Differentiation;
45 import org.orekit.utils.ParameterDriver;
46 import org.orekit.utils.ParameterFunction;
47 import org.orekit.utils.StateFunction;
48 import org.orekit.utils.TimeStampedPVCoordinates;
49
50 public class Range2Test {
51
52 @Before
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 = 5.0e-04;
90 double refErrorsVMax = 1.3e-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 = 6.3e-10;
110 double refErrorsPMean = 4.2e-09;
111 double refErrorsPMax = 2.4e-07;
112 double refErrorsVMedian = 1.4e-04;
113 double refErrorsVMean = 5.0e-04;
114 double refErrorsVMax = 1.3e-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, PositionAngle.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 RangeMeasurementCreator2(context),
185 1.0, 3.0, 300.0);
186
187
188
189 final List<Double> absoluteErrors = new ArrayList<Double>();
190 final List<Double> relativeErrors = new ArrayList<Double>();
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 EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
216
217 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
218 Assert.assertEquals(2, participants.length);
219 Assert.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 Assert.assertTrue(adjustment > 0.006);
226 Assert.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 Assert.assertEquals(0.0, absErrorsMedian, 4.9e-8);
289 Assert.assertEquals(0.0, absErrorsMin, 2.7e-7);
290 Assert.assertEquals(0.0, absErrorsMax, 3.0e-7);
291 Assert.assertEquals(0.0, relErrorsMedian, 1.0e-14);
292 Assert.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, PositionAngle.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 RangeMeasurementCreator2(context),
313 1.0, 3.0, 300.0);
314
315
316
317 final List<Double> errorsP = new ArrayList<Double>();
318 final List<Double> errorsV = new ArrayList<Double>();
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 = new RangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
333 if (isModifier) {
334 ((Range) measurement).addModifier(modifier);
335 }
336
337
338
339
340
341
342
343
344 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
345 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
346 final SpacecraftState state = interpolator.getInterpolatedState(date);
347 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
348
349
350 final double[][] jacobianRef;
351
352
353 jacobianRef = Differentiation.differentiate(new StateFunction() {
354 public double[] value(final SpacecraftState state) {
355 return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
356 }
357 }, measurement.getDimension(), propagator.getAttitudeProvider(),
358 OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state);
359
360 Assert.assertEquals(jacobianRef.length, jacobian.length);
361 Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
362
363
364 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
365 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
366 for (int i = 0; i < jacobian.length; ++i) {
367 for (int j = 0; j < jacobian[i].length; ++j) {
368 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
369 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
370
371 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
372 } else { errorsV.add(dJacobianRelative[i][j]); }
373 }
374 }
375
376 if (printResults) {
377 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
378 System.out.format(Locale.US, "%-15s %-23s %-23s " +
379 "%10.3e %10.3e %10.3e " +
380 "%10.3e %10.3e %10.3e " +
381 "%10.3e %10.3e %10.3e " +
382 "%10.3e %10.3e %10.3e%n",
383 stationName, measurement.getDate(), date,
384 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
385 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
386 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
387 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
388 }
389 }
390 }
391 });
392
393
394 if (printResults) {
395 System.out.format(Locale.US, "%-15s %-23s %-23s " +
396 "%10s %10s %10s " +
397 "%10s %10s %10s " +
398 "%10s %10s %10s " +
399 "%10s %10s %10s%n",
400 "Station", "Measurement Date", "State Date",
401 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
402 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
403 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
404 }
405
406
407 propagator.propagate(context.initialOrbit.getDate());
408
409
410 measurements.sort(Comparator.naturalOrder());
411
412
413 propagator.propagate(measurements.get(measurements.size()-1).getDate());
414
415
416 final double relErrorsP[] = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
417 final double relErrorsV[] = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
418
419 final double errorsPMedian = new Median().evaluate(relErrorsP);
420 final double errorsPMean = new Mean().evaluate(relErrorsP);
421 final double errorsPMax = new Max().evaluate(relErrorsP);
422 final double errorsVMedian = new Median().evaluate(relErrorsV);
423 final double errorsVMean = new Mean().evaluate(relErrorsV);
424 final double errorsVMax = new Max().evaluate(relErrorsV);
425
426
427 if (printResults) {
428 System.out.println();
429 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
430 errorsPMedian, errorsPMean, errorsPMax);
431 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
432 errorsVMedian, errorsVMean, errorsVMax);
433 }
434
435 Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
436 Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
437 Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
438 Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
439 Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
440 Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
441 }
442
443 void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults,
444 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
445
446 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
447
448 final NumericalPropagatorBuilder propagatorBuilder =
449 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
450 1.0e-6, 60.0, 0.001);
451
452
453 for (final GroundStation station : context.stations) {
454 station.getClockOffsetDriver().setSelected(true);
455 station.getEastOffsetDriver().setSelected(true);
456 station.getNorthOffsetDriver().setSelected(true);
457 station.getZenithOffsetDriver().setSelected(true);
458 }
459 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
460 propagatorBuilder);
461 final List<ObservedMeasurement<?>> measurements =
462 EstimationTestUtils.createMeasurements(propagator,
463 new RangeMeasurementCreator2(context),
464 1.0, 3.0, 300.0);
465
466
467 final List<Double> relErrorList = new ArrayList<Double>();
468
469
470
471 propagator.setStepHandler(interpolator -> {
472
473 for (final ObservedMeasurement<?> measurement : measurements) {
474
475
476 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
477 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
478 ) {
479
480
481 final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(SaastamoinenModel.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 Assert.assertEquals(1, measurement.getDimension());
515 Assert.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) {
523 return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
524 }
525 }, 3, 20.0 * drivers[i].getScale());
526 final double ref = dMkdP.value(drivers[i]);
527
528 if (printResults) {
529 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
530 }
531
532 final double relError = FastMath.abs((ref-gradient[0])/ref);
533 relErrorList.add(relError);
534
535 }
536 if (printResults) {
537 System.out.format(Locale.US, "%n");
538 }
539
540 }
541 }
542 });
543
544
545 propagator.propagate(context.initialOrbit.getDate());
546
547
548 measurements.sort(Comparator.naturalOrder());
549
550
551 if (printResults) {
552 System.out.format(Locale.US, "%-15s %-23s %-23s " +
553 "%10s %10s %10s %10s %10s %10s %10s %10s%n",
554 "Station", "Measurement Date", "State Date",
555 "Δt", "rel Δt",
556 "ΔdQx", "rel ΔdQx",
557 "ΔdQy", "rel ΔdQy",
558 "ΔdQz", "rel ΔdQz");
559 }
560
561
562 propagator.propagate(measurements.get(measurements.size()-1).getDate());
563
564
565 final double relErrors[] = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
566
567
568 final double relErrorsMedian = new Median().evaluate(relErrors);
569 final double relErrorsMean = new Mean().evaluate(relErrors);
570 final double relErrorsMax = new Max().evaluate(relErrors);
571
572
573 if (printResults) {
574 System.out.println();
575 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
576 relErrorsMedian, relErrorsMean, relErrorsMax);
577 }
578
579 Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
580 Assert.assertEquals(0.0, relErrorsMean, refErrorsMean);
581 Assert.assertEquals(0.0, relErrorsMax, refErrorsMax);
582
583 }
584
585 }