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