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.Field;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.geometry.euclidean.threed.Vector3D;
27 import org.hipparchus.stat.descriptive.moment.Mean;
28 import org.hipparchus.stat.descriptive.rank.Max;
29 import org.hipparchus.stat.descriptive.rank.Median;
30 import org.hipparchus.stat.descriptive.rank.Min;
31 import org.hipparchus.util.Binary64;
32 import org.hipparchus.util.Binary64Field;
33 import org.hipparchus.util.FastMath;
34 import org.junit.jupiter.api.Assertions;
35 import org.junit.jupiter.api.Test;
36 import org.orekit.estimation.Context;
37 import org.orekit.estimation.EstimationTestUtils;
38 import org.orekit.estimation.measurements.modifiers.RangeTroposphericDelayModifier;
39 import org.orekit.models.earth.troposphere.EstimatedModel;
40 import org.orekit.models.earth.troposphere.ModifiedSaastamoinenModel;
41 import org.orekit.models.earth.troposphere.NiellMappingFunctionModel;
42 import org.orekit.models.earth.troposphere.TroposphericModelUtils;
43 import org.orekit.orbits.OrbitType;
44 import org.orekit.orbits.PositionAngleType;
45 import org.orekit.propagation.FieldSpacecraftState;
46 import org.orekit.propagation.Propagator;
47 import org.orekit.propagation.SpacecraftState;
48 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
49 import org.orekit.time.AbsoluteDate;
50 import org.orekit.time.FieldAbsoluteDate;
51 import org.orekit.utils.Constants;
52 import org.orekit.utils.Differentiation;
53 import org.orekit.utils.PVCoordinates;
54 import org.orekit.utils.ParameterDriver;
55 import org.orekit.utils.ParameterFunction;
56 import org.orekit.utils.TimeStampedFieldPVCoordinates;
57 import org.orekit.utils.TimeStampedPVCoordinates;
58
59 class RangeTest {
60
61
62
63
64
65 @Test
66 void testValues() {
67 boolean printResults = false;
68 if (printResults) {
69 System.out.println("\nTest Range Values\n");
70 }
71
72 this.genericTestValues(printResults);
73 }
74
75
76
77
78
79 @Test
80 void testStateDerivatives() {
81
82 boolean printResults = false;
83 if (printResults) {
84 System.out.println("\nTest Range State Derivatives - Finite Differences Comparison\n");
85 }
86
87 boolean isModifier = false;
88 double refErrorsPMedian = 6.7e-10;
89 double refErrorsPMean = 3.1e-09;
90 double refErrorsPMax = 1.0e-07;
91 double refErrorsVMedian = 2.1e-04;
92 double refErrorsVMean = 1.3e-03;
93 double refErrorsVMax = 5.2e-02;
94 this.genericTestStateDerivatives(isModifier, printResults,
95 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
96 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
97 }
98
99
100
101
102
103 @Test
104 void testStateDerivativesWithModifier() {
105
106 boolean printResults = false;
107 if (printResults) {
108 System.out.println("\nTest Range State Derivatives with Modifier - Finite Differences Comparison\n");
109 }
110
111 boolean isModifier = true;
112 double refErrorsPMedian = 7.9e-10;
113 double refErrorsPMean = 2.6e-09;
114 double refErrorsPMax = 9.3e-08;
115 double refErrorsVMedian = 2.1e-04;
116 double refErrorsVMean = 1.3e-03;
117 double refErrorsVMax = 5.2e-02;
118 this.genericTestStateDerivatives(isModifier, printResults,
119 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
120 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
121 }
122
123
124
125
126
127 @Test
128 void testParameterDerivatives() {
129
130
131 boolean printResults = false;
132
133 if (printResults) {
134 System.out.println("\nTest Range Parameter Derivatives - Finite Differences Comparison\n");
135 }
136
137 boolean isModifier = false;
138 double refErrorsMedian = 5.8e-9;
139 double refErrorsMean = 6.4e-8;
140 double refErrorsMax = 5.1e-6;
141 this.genericTestParameterDerivatives(isModifier, printResults,
142 refErrorsMedian, refErrorsMean, refErrorsMax);
143
144 }
145
146
147
148
149
150 @Test
151 void testParameterDerivativesWithModifier() {
152
153
154 boolean printResults = false;
155
156 if (printResults) {
157 System.out.println("\nTest Range Parameter Derivatives with Modifier - Finite Differences Comparison\n");
158 }
159
160 boolean isModifier = true;
161 double refErrorsMedian = 2.9e-8;
162 double refErrorsMean = 4.9e-7;
163 double refErrorsMax = 1.5e-5;
164 this.genericTestParameterDerivatives(isModifier, printResults,
165 refErrorsMedian, refErrorsMean, refErrorsMax);
166
167 }
168
169
170
171
172
173 @Test
174 void testParameterDerivativesWithEstimatedModifier() {
175
176
177 boolean printResults = false;
178
179 if (printResults) {
180 System.out.println("\nTest Range Parameter Derivatives with Estimated Modifier - Finite Differences Comparison\n");
181 }
182
183 boolean isModifier = true;
184 double refErrorsMedian = 1.2e-9;
185 double refErrorsMean = 1.9e-9;
186 double refErrorsMax = 6.9e-9;
187 this.genericTestEstimatedParameterDerivatives(isModifier, printResults,
188 refErrorsMedian, refErrorsMean, refErrorsMax);
189
190 }
191
192 @Test
193 public void testSignalTimeOfFlight() {
194
195 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
196
197 final NumericalPropagatorBuilder propagatorBuilder =
198 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
199 1.0e-6, 60.0, 0.001);
200
201
202 final Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
203 propagatorBuilder);
204 final List<ObservedMeasurement<?>> measurements =
205 EstimationTestUtils.createMeasurements(propagator1,
206 new TwoWayRangeMeasurementCreator(context),
207 1.0, 3.0, 300.0);
208
209 final Propagator propagator2 = EstimationTestUtils.createPropagator(context.initialOrbit,
210 propagatorBuilder);
211 for (final ObservedMeasurement<?> measurement : measurements) {
212
213 final GroundStation stationParameter = ((Range) measurement).getStation();
214
215 final AbsoluteDate datemeas = measurement.getDate();
216 SpacecraftState state = propagator2.propagate(datemeas);
217
218
219 final TimeStampedPVCoordinates staPV = stationParameter.getOffsetToInertial(state.getFrame(), datemeas, false).
220 transformPVCoordinates(new TimeStampedPVCoordinates(datemeas, PVCoordinates.ZERO));
221 final double downDelayE = AbstractMeasurement.signalTimeOfFlightAdjustableEmitter(state.getPVCoordinates(),
222 staPV.getPosition(),
223 datemeas, state.getFrame());
224 final Vector3D satPosE = propagator2.propagate(datemeas.shiftedBy(-downDelayE)).getPosition();
225 Assertions.assertEquals(Vector3D.distance(satPosE, staPV.getPosition()), downDelayE * Constants.SPEED_OF_LIGHT, 2.0e-7);
226
227
228 final Field<Binary64> field = Binary64Field.getInstance();
229 final FieldAbsoluteDate<Binary64> datemeasF = new FieldAbsoluteDate<>(field, datemeas);
230 final FieldSpacecraftState<Binary64> stateF = new FieldSpacecraftState<>(field, state);
231 final TimeStampedFieldPVCoordinates<Binary64> staPVF = new TimeStampedFieldPVCoordinates<>(field, staPV);
232 final Binary64 downDelayEF = AbstractMeasurement.signalTimeOfFlightAdjustableEmitter(stateF.getPVCoordinates(),
233 staPVF.getPosition(),
234 datemeasF, state.getFrame());
235 final FieldVector3D<Binary64> satPosEF =
236 new FieldVector3D<>(field, propagator2.propagate(datemeas.shiftedBy(-downDelayEF.getReal())).getPosition());
237 Assertions.assertEquals(FieldVector3D.distance(satPosEF, staPVF.getPosition()).getReal(),
238 downDelayEF.multiply(Constants.SPEED_OF_LIGHT).getReal(),
239 2.0e-7);
240
241
242 final double downDelayR = AbstractMeasurement.signalTimeOfFlightAdjustableReceiver(state.getPVCoordinates().getPosition(),
243 state.getDate(),
244 staPV,
245 datemeas, state.getFrame());
246 final Vector3D staPosR = stationParameter.
247 getOffsetToInertial(state.getFrame(), state.getDate().shiftedBy(downDelayR), false).
248 transformPosition(Vector3D.ZERO);
249 Assertions.assertEquals(Vector3D.distance(state.getPVCoordinates().getPosition(), staPosR),
250 downDelayR * Constants.SPEED_OF_LIGHT, 2.0e-7);
251
252
253 final Binary64 downDelayRF = AbstractMeasurement.signalTimeOfFlightAdjustableReceiver(stateF.getPVCoordinates().getPosition(),
254 stateF.getDate(),
255 staPVF,
256 datemeasF, state.getFrame());
257 final FieldVector3D<Binary64> staPosRF = new FieldVector3D<>(field,
258 stationParameter.
259 getOffsetToInertial(state.getFrame(),
260 state.getDate().shiftedBy(downDelayRF.getReal()),
261 false).
262 transformPosition(Vector3D.ZERO));
263 Assertions.assertEquals(FieldVector3D.distance(stateF.getPVCoordinates().getPosition(), staPosRF).getReal(),
264 downDelayRF.multiply(Constants.SPEED_OF_LIGHT).getReal(),
265 2.0e-7);
266
267 }
268
269 }
270
271
272
273
274
275 void genericTestValues(final boolean printResults) {
276
277 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
278
279 final NumericalPropagatorBuilder propagatorBuilder =
280 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
281 1.0e-6, 60.0, 0.001);
282
283
284 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
285 propagatorBuilder);
286 final double groundClockOffset = 1.234e-3;
287 for (final GroundStation station : context.stations) {
288 station.getClockOffsetDriver().setValue(groundClockOffset);
289 }
290 final List<ObservedMeasurement<?>> measurements =
291 EstimationTestUtils.createMeasurements(propagator,
292 new TwoWayRangeMeasurementCreator(context),
293 1.0, 3.0, 300.0);
294
295
296
297 final List<Double> absoluteErrors = new ArrayList<>();
298 final List<Double> relativeErrors = new ArrayList<>();
299
300
301
302 propagator.setStepHandler(interpolator -> {
303
304 for (final ObservedMeasurement<?> measurement : measurements) {
305
306
307 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
308 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
309 ) {
310
311
312
313
314
315
316
317 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
318 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
319 final SpacecraftState state = interpolator.getInterpolatedState(date);
320
321
322 final double RangeObserved = measurement.getObservedValue()[0];
323 final EstimatedMeasurementBase<?> estimated = measurement.estimateWithoutDerivatives(new SpacecraftState[] { state });
324
325 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
326 Assertions.assertEquals(3, participants.length);
327 Assertions.assertEquals(0.5 * Constants.SPEED_OF_LIGHT * participants[2].getDate().durationFrom(participants[0].getDate()),
328 estimated.getEstimatedValue()[0],
329 2.0e-8);
330
331
332 double adjustment = state.getDate().durationFrom(estimated.getStates()[0].getDate());
333 Assertions.assertTrue(adjustment > 0.006);
334 Assertions.assertTrue(adjustment < 0.011);
335
336 final double RangeEstimated = estimated.getEstimatedValue()[0];
337 final double absoluteError = RangeEstimated-RangeObserved;
338 absoluteErrors.add(absoluteError);
339 relativeErrors.add(FastMath.abs(absoluteError)/FastMath.abs(RangeObserved));
340
341
342 if (printResults) {
343 final AbsoluteDate measurementDate = measurement.getDate();
344 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
345
346 System.out.format(Locale.US, "%-15s %-23s %-23s %19.6f %19.6f %13.6e %13.6e%n",
347 stationName, measurementDate, date,
348 RangeObserved, RangeEstimated,
349 FastMath.abs(RangeEstimated-RangeObserved),
350 FastMath.abs((RangeEstimated-RangeObserved)/RangeObserved));
351 }
352
353 }
354 }
355 });
356
357
358 if (printResults) {
359 System.out.format(Locale.US, "%-15s %-23s %-23s %19s %19s %13s %13s%n",
360 "Station", "Measurement Date", "State Date",
361 "Range observed [m]", "Range estimated [m]",
362 "ΔRange [m]", "rel ΔRange");
363 }
364
365
366 propagator.propagate(context.initialOrbit.getDate());
367
368
369 measurements.sort(Comparator.naturalOrder());
370
371
372 propagator.propagate(measurements.get(measurements.size()-1).getDate());
373
374
375 final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
376 final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
377
378
379 final double absErrorsMedian = new Median().evaluate(absErrors);
380 final double absErrorsMin = new Min().evaluate(absErrors);
381 final double absErrorsMax = new Max().evaluate(absErrors);
382 final double relErrorsMedian = new Median().evaluate(relErrors);
383 final double relErrorsMax = new Max().evaluate(relErrors);
384
385
386 if (printResults) {
387 System.out.println();
388 System.out.println("Absolute errors median: " + absErrorsMedian);
389 System.out.println("Absolute errors min : " + absErrorsMin);
390 System.out.println("Absolute errors max : " + absErrorsMax);
391 System.out.println("Relative errors median: " + relErrorsMedian);
392 System.out.println("Relative errors max : " + relErrorsMax);
393 }
394
395 Assertions.assertEquals(0.0, absErrorsMedian, 6.3e-8);
396 Assertions.assertEquals(0.0, absErrorsMin, 2.0e-7);
397 Assertions.assertEquals(0.0, absErrorsMax, 2.6e-7);
398 Assertions.assertEquals(0.0, relErrorsMedian, 8.5e-15);
399 Assertions.assertEquals(0.0, relErrorsMax, 2.9e-14);
400
401
402 Assertions.assertEquals(Range.MEASUREMENT_TYPE, measurements.get(0).getMeasurementType());
403 }
404
405 void genericTestStateDerivatives(final boolean isModifier, final boolean printResults,
406 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
407 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
408
409 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
410
411 final NumericalPropagatorBuilder propagatorBuilder =
412 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
413 1.0e-6, 60.0, 0.001);
414
415
416 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
417 propagatorBuilder);
418 final List<ObservedMeasurement<?>> measurements =
419 EstimationTestUtils.createMeasurements(propagator,
420 new TwoWayRangeMeasurementCreator(context),
421 1.0, 3.0, 300.0);
422
423
424
425 final List<Double> errorsP = new ArrayList<>();
426 final List<Double> errorsV = new ArrayList<>();
427
428
429
430 propagator.setStepHandler(interpolator -> {
431
432 for (final ObservedMeasurement<?> measurement : measurements) {
433
434
435 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
436 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
437 ) {
438
439
440 final RangeTroposphericDelayModifier modifier =
441 new RangeTroposphericDelayModifier(new ModifiedSaastamoinenModel(TroposphericModelUtils.STANDARD_ATMOSPHERE_PROVIDER));
442 if (isModifier) {
443 ((Range) measurement).addModifier(modifier);
444 }
445
446
447
448
449
450
451
452
453 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
454 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
455 final SpacecraftState state = interpolator.getInterpolatedState(date);
456 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
457
458
459 final double[][] jacobianRef;
460
461
462 jacobianRef = Differentiation.differentiate(state1 ->
463 measurement.
464 estimateWithoutDerivatives(new SpacecraftState[] { state1 }).
465 getEstimatedValue(),
466 measurement.getDimension(), propagator.getAttitudeProvider(),
467 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(state);
468
469 Assertions.assertEquals(jacobianRef.length, jacobian.length);
470 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
471
472
473 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
474 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
475 for (int i = 0; i < jacobian.length; ++i) {
476 for (int j = 0; j < jacobian[i].length; ++j) {
477 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
478 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
479
480 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
481 } else { errorsV.add(dJacobianRelative[i][j]); }
482 }
483 }
484
485 if (printResults) {
486 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
487 System.out.format(Locale.US, "%-15s %-23s %-23s " +
488 "%10.3e %10.3e %10.3e " +
489 "%10.3e %10.3e %10.3e " +
490 "%10.3e %10.3e %10.3e " +
491 "%10.3e %10.3e %10.3e%n",
492 stationName, measurement.getDate(), date,
493 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
494 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
495 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
496 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
497 }
498 }
499 }
500 });
501
502
503 if (printResults) {
504 System.out.format(Locale.US, "%-15s %-23s %-23s " +
505 "%10s %10s %10s " +
506 "%10s %10s %10s " +
507 "%10s %10s %10s " +
508 "%10s %10s %10s%n",
509 "Station", "Measurement Date", "State Date",
510 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
511 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
512 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
513 }
514
515
516 propagator.propagate(context.initialOrbit.getDate());
517
518
519 measurements.sort(Comparator.naturalOrder());
520
521
522 propagator.propagate(measurements.get(measurements.size()-1).getDate());
523
524
525 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
526 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
527
528 final double errorsPMedian = new Median().evaluate(relErrorsP);
529 final double errorsPMean = new Mean().evaluate(relErrorsP);
530 final double errorsPMax = new Max().evaluate(relErrorsP);
531 final double errorsVMedian = new Median().evaluate(relErrorsV);
532 final double errorsVMean = new Mean().evaluate(relErrorsV);
533 final double errorsVMax = new Max().evaluate(relErrorsV);
534
535
536 if (printResults) {
537 System.out.println();
538 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
539 errorsPMedian, errorsPMean, errorsPMax);
540 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
541 errorsVMedian, errorsVMean, errorsVMax);
542 }
543
544 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
545 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
546 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
547 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
548 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
549 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
550 }
551
552 void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults,
553 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
554
555 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
556
557 final NumericalPropagatorBuilder propagatorBuilder =
558 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
559 1.0e-6, 60.0, 0.001);
560
561
562 for (final GroundStation station : context.stations) {
563 station.getClockOffsetDriver().setSelected(true);
564 station.getEastOffsetDriver().setSelected(true);
565 station.getNorthOffsetDriver().setSelected(true);
566 station.getZenithOffsetDriver().setSelected(true);
567 }
568 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
569 propagatorBuilder);
570 final List<ObservedMeasurement<?>> measurements =
571 EstimationTestUtils.createMeasurements(propagator,
572 new TwoWayRangeMeasurementCreator(context),
573 1.0, 3.0, 300.0);
574
575
576 final List<Double> relErrorList = new ArrayList<>();
577
578
579
580 propagator.setStepHandler(interpolator -> {
581
582 for (final ObservedMeasurement<?> measurement : measurements) {
583
584
585 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
586 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
587 ) {
588
589
590 final RangeTroposphericDelayModifier modifier =
591 new RangeTroposphericDelayModifier(ModifiedSaastamoinenModel.getStandardModel());
592 if (isModifier) {
593 ((Range) measurement).addModifier(modifier);
594 }
595
596
597 final GroundStation station = ((Range) measurement).getStation();
598
599
600
601
602
603
604
605
606 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
607 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
608 final SpacecraftState state = interpolator.getInterpolatedState(date);
609 final ParameterDriver[] drivers = new ParameterDriver[] {
610 station.getClockOffsetDriver(),
611 station.getEastOffsetDriver(),
612 station.getNorthOffsetDriver(),
613 station.getZenithOffsetDriver()
614 };
615
616 if (printResults) {
617 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
618 System.out.format(Locale.US, "%-15s %-23s %-23s ",
619 stationName, measurement.getDate(), date);
620 }
621
622 for (int i = 0; i < drivers.length; ++i) {
623 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i], new AbsoluteDate());
624 Assertions.assertEquals(1, measurement.getDimension());
625 Assertions.assertEquals(1, gradient.length);
626
627
628 final ParameterFunction dMkdP =
629 Differentiation.differentiate(new ParameterFunction() {
630
631 @Override
632 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
633 return measurement.
634 estimateWithoutDerivatives(new SpacecraftState[] { state }).
635 getEstimatedValue()[0];
636 }
637 }, 3, 20.0 * drivers[i].getScale());
638 final double ref = dMkdP.value(drivers[i], date);
639
640 if (printResults) {
641 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
642 }
643
644 final double relError = FastMath.abs((ref-gradient[0])/ref);
645 relErrorList.add(relError);
646 Assertions.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref));
647 }
648 if (printResults) {
649 System.out.format(Locale.US, "%n");
650 }
651
652 }
653 }
654 });
655
656
657 propagator.propagate(context.initialOrbit.getDate());
658
659
660 measurements.sort(Comparator.naturalOrder());
661
662
663 if (printResults) {
664 System.out.format(Locale.US, "%-15s %-23s %-23s " +
665 "%10s %10s %10s %10s %10s %10s %10s %10s%n",
666 "Station", "Measurement Date", "State Date",
667 "Δt", "rel Δt",
668 "ΔdQx", "rel ΔdQx",
669 "ΔdQy", "rel ΔdQy",
670 "ΔdQz", "rel ΔdQz");
671 }
672
673
674 propagator.propagate(measurements.get(measurements.size()-1).getDate());
675
676
677 final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
678
679
680 final double relErrorsMedian = new Median().evaluate(relErrors);
681 final double relErrorsMean = new Mean().evaluate(relErrors);
682 final double relErrorsMax = new Max().evaluate(relErrors);
683
684
685 if (printResults) {
686 System.out.println();
687 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
688 relErrorsMedian, relErrorsMean, relErrorsMax);
689 }
690
691 Assertions.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
692 Assertions.assertEquals(0.0, relErrorsMean, refErrorsMean);
693 Assertions.assertEquals(0.0, relErrorsMax, refErrorsMax);
694
695 }
696
697 void genericTestEstimatedParameterDerivatives(final boolean isModifier, final boolean printResults,
698 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax)
699 {
700
701 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
702
703 final NumericalPropagatorBuilder propagatorBuilder =
704 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
705 1.0e-6, 60.0, 0.001);
706
707 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
708 propagatorBuilder);
709 final List<ObservedMeasurement<?>> measurements =
710 EstimationTestUtils.createMeasurements(propagator,
711 new TwoWayRangeMeasurementCreator(context),
712 1.0, 3.0, 300.0);
713
714
715 final List<Double> relErrorList = new ArrayList<>();
716
717
718
719 propagator.setStepHandler(interpolator -> {
720
721 for (final ObservedMeasurement<?> measurement : measurements) {
722
723
724 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
725 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
726 ) {
727
728 String stationName = ((Range) measurement).getStation().getBaseFrame().getName();
729
730
731 final NiellMappingFunctionModel mappingFunction = new NiellMappingFunctionModel();
732 final EstimatedModel tropoModel = new EstimatedModel(mappingFunction, 5.0);
733
734 final List<ParameterDriver> parameters = tropoModel.getParametersDrivers();
735 for (ParameterDriver driver : parameters) {
736 driver.setSelected(true);
737 }
738
739 parameters.get(0).setName(stationName + "/" + EstimatedModel.TOTAL_ZENITH_DELAY);
740 final RangeTroposphericDelayModifier modifier = new RangeTroposphericDelayModifier(tropoModel);
741 if (isModifier) {
742 ((Range) measurement).addModifier(modifier);
743 }
744
745
746
747
748
749
750
751
752 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
753 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
754 final SpacecraftState state = interpolator.getInterpolatedState(date);
755 final ParameterDriver[] drivers = new ParameterDriver[] {
756 parameters.get(0)
757 };
758
759 if (printResults) {
760 System.out.format(Locale.US, "%-15s %-23s %-23s ",
761 stationName, measurement.getDate(), date);
762 }
763
764 for (int i = 0; i < 1; ++i) {
765 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i], new AbsoluteDate());
766 Assertions.assertEquals(1, measurement.getDimension());
767 Assertions.assertEquals(1, gradient.length);
768
769
770 final ParameterFunction dMkdP =
771 Differentiation.differentiate(new ParameterFunction() {
772
773 @Override
774 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
775 return measurement.
776 estimateWithoutDerivatives(new SpacecraftState[] { state }).
777 getEstimatedValue()[0];
778 }
779 }, 3, 0.1 * drivers[i].getScale());
780 final double ref = dMkdP.value(drivers[i], date);
781
782 if (printResults) {
783 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
784 }
785
786 final double relError = FastMath.abs((ref-gradient[0])/ref);
787 relErrorList.add(relError);
788
789 }
790 if (printResults) {
791 System.out.format(Locale.US, "%n");
792 }
793
794 }
795 }
796 });
797
798
799 propagator.propagate(context.initialOrbit.getDate());
800
801
802 measurements.sort(Comparator.naturalOrder());
803
804
805 if (printResults) {
806 System.out.format(Locale.US, "%-15s %-23s %-23s " +
807 "%10s %10s %10s " +
808 "%10s %10s %10s%n ",
809 "Station", "Measurement Date", "State Date",
810 "ΔdDelay", "rel ΔdDelay",
811 "ΔdNorthG", "rel ΔdNorthG",
812 "ΔdEastG", "rel ΔdEastG");
813 }
814
815
816 propagator.propagate(measurements.get(measurements.size()-1).getDate());
817
818
819 final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
820
821
822 final double relErrorsMedian = new Median().evaluate(relErrors);
823 final double relErrorsMean = new Mean().evaluate(relErrors);
824 final double relErrorsMax = new Max().evaluate(relErrors);
825
826
827 if (printResults) {
828 System.out.println();
829 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
830 relErrorsMedian, relErrorsMean, relErrorsMax);
831 }
832
833 Assertions.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
834 Assertions.assertEquals(0.0, relErrorsMean, refErrorsMean);
835 Assertions.assertEquals(0.0, relErrorsMax, refErrorsMax);
836
837 }
838
839 }