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.List;
21 import java.util.Locale;
22 import java.util.Map;
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.TurnAroundRangeTroposphericDelayModifier;
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.ParameterFunction;
45 import org.orekit.utils.StateFunction;
46 import org.orekit.utils.TimeStampedPVCoordinates;
47
48 public class TurnAroundRangeTest {
49
50
51
52
53
54
55 @Test
56 public void testValues() {
57 boolean printResults = false;
58 if (printResults) {
59 System.out.println("\nTest TAR Values\n");
60 }
61
62 this.genericTestValues(printResults);
63 }
64
65
66
67
68
69 @Test
70 public void testStateDerivatives() {
71
72 boolean printResults = false;
73 if (printResults) {
74 System.out.println("\nTest TAR State Derivatives - Finite Differences comparison\n");
75 }
76
77 boolean isModifier = false;
78 double refErrorsPMedian = 1.4e-6;
79 double refErrorsPMean = 1.4e-06;
80 double refErrorsPMax = 2.6e-06;
81 double refErrorsVMedian = 8.2e-05;
82 double refErrorsVMean = 3.6e-04;
83 double refErrorsVMax = 1.4e-02;
84
85 this.genericTestStateDerivatives(isModifier, printResults,
86 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
87 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
88 }
89
90
91
92
93
94 @Test
95 public void testStateDerivativesWithModifier() {
96
97 boolean printResults = false;
98 if (printResults) {
99 System.out.println("\nTest TAR State Derivatives with Modifier - Finite Differences comparison\n");
100 }
101
102 boolean isModifier = true;
103 double refErrorsPMedian = 1.4e-6;
104 double refErrorsPMean = 1.4e-06;
105 double refErrorsPMax = 2.6e-06;
106 double refErrorsVMedian = 8.2e-05;
107 double refErrorsVMean = 3.6e-04;
108 double refErrorsVMax = 1.4e-2;
109
110 this.genericTestStateDerivatives(isModifier, printResults,
111 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
112 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
113 }
114
115
116
117
118
119 @Test
120 public void testParameterDerivatives() {
121
122
123 boolean printResults = false;
124
125 if (printResults) {
126 System.out.println("\nTest TAR Parameter Derivatives - Finite Differences comparison\n");
127 }
128
129 boolean isModifier = false;
130 double refErrorQMMedian = 2.6e-6;
131 double refErrorQMMean = 2.5e-6;
132 double refErrorQMMax = 5.6e-6;
133 double refErrorQSMedian = 3.7e-7;
134 double refErrorQSMean = 3.6e-7;
135 double refErrorQSMax = 7.7e-7;
136 this.genericTestParameterDerivatives(isModifier, printResults,
137 refErrorQMMedian, refErrorQMMean, refErrorQMMax,
138 refErrorQSMedian, refErrorQSMean, refErrorQSMax);
139
140 }
141
142
143
144
145
146 @Test
147 public void testParameterDerivativesWithModifier() {
148
149
150 boolean printResults = false;
151
152 if (printResults) {
153 System.out.println("\nTest TAR Parameter Derivatives with Modifier - Finite Differences comparison\n");
154 }
155
156 boolean isModifier = true;
157 double refErrorQMMedian = 2.6e-6;
158 double refErrorQMMean = 2.5e-6;
159 double refErrorQMMax = 5.6e-6;
160 double refErrorQSMedian = 3.7e-7;
161 double refErrorQSMean = 3.6e-7;
162 double refErrorQSMax = 7.7e-7;
163 this.genericTestParameterDerivatives(isModifier, printResults,
164 refErrorQMMedian, refErrorQMMean, refErrorQMMax,
165 refErrorQSMedian, refErrorQSMean, refErrorQSMax);
166 }
167
168
169
170
171
172 void genericTestValues(final boolean printResults) {
173
174 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
175
176
177 final NumericalPropagatorBuilder propagatorBuilder =
178 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
179 1.0e-6, 60.0, 0.001);
180
181
182 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
183 propagatorBuilder);
184 final List<ObservedMeasurement<?>> measurements =
185 EstimationTestUtils.createMeasurements(propagator,
186 new TurnAroundRangeMeasurementCreator(context),
187 1.0, 3.0, 300.0);
188 propagator.clearStepHandlers();
189
190 double[] absoluteErrors = new double[measurements.size()];
191 double[] relativeErrors = new double[measurements.size()];
192 int index = 0;
193
194
195 if (printResults) {
196 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s %17s %17s %13s %13s%n",
197 "Primary Station", "secondary Station",
198 "Measurement Date", "State Date",
199 "TAR observed [m]", "TAR estimated [m]",
200 "|ΔTAR| [m]", "rel |ΔTAR|");
201 }
202
203
204 for (final ObservedMeasurement<?> measurement : measurements) {
205
206 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
207 final AbsoluteDate date = measurement.getDate().shiftedBy(meanDelay);
208 final SpacecraftState state = propagator.propagate(date);
209
210
211 final double TARobserved = measurement.getObservedValue()[0];
212 final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0, new SpacecraftState[] { state });
213 final double TARestimated = estimated.getEstimatedValue()[0];
214
215 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
216 Assert.assertEquals(5, participants.length);
217 Assert.assertEquals(0.5 * Constants.SPEED_OF_LIGHT * participants[4].getDate().durationFrom(participants[0].getDate()),
218 estimated.getEstimatedValue()[0],
219 2.0e-8);
220
221 absoluteErrors[index] = TARestimated-TARobserved;
222 relativeErrors[index] = FastMath.abs(absoluteErrors[index])/FastMath.abs(TARobserved);
223 index++;
224
225
226 if (printResults) {
227 final AbsoluteDate measurementDate = measurement.getDate();
228
229 String primaryStationName = ((TurnAroundRange) measurement).getPrimaryStation().getBaseFrame().getName();
230 String secondaryStationName = ((TurnAroundRange) measurement).getSecondaryStation().getBaseFrame().getName();
231 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s %17.6f %17.6f %13.6e %13.6e%n",
232 primaryStationName, secondaryStationName, measurementDate, date,
233 TARobserved, TARestimated,
234 FastMath.abs(TARestimated-TARobserved),
235 FastMath.abs((TARestimated-TARobserved)/TARobserved));
236 }
237
238 }
239
240
241 final double absErrorsMedian = new Median().evaluate(absoluteErrors);
242 final double absErrorsMin = new Min().evaluate(absoluteErrors);
243 final double absErrorsMax = new Max().evaluate(absoluteErrors);
244 final double relErrorsMedian = new Median().evaluate(relativeErrors);
245 final double relErrorsMax = new Max().evaluate(relativeErrors);
246
247
248 if (printResults) {
249 System.out.println();
250 System.out.println("Absolute errors median: " + absErrorsMedian);
251 System.out.println("Absolute errors min : " + absErrorsMin);
252 System.out.println("Absolute errors max : " + absErrorsMax);
253 System.out.println("Relative errors median: " + relErrorsMedian);
254 System.out.println("Relative errors max : " + relErrorsMax);
255 }
256
257
258 Assert.assertEquals(0.0, absErrorsMedian, 1.4e-7);
259 Assert.assertEquals(0.0, absErrorsMin, 5.0e-7);
260 Assert.assertEquals(0.0, absErrorsMax, 4.9e-7);
261 Assert.assertEquals(0.0, relErrorsMedian, 8.9e-15);
262 Assert.assertEquals(0.0, relErrorsMax , 2.9e-14);
263 }
264
265 void genericTestStateDerivatives(final boolean isModifier, final boolean printResults,
266 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
267 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
268
269 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
270
271
272 final NumericalPropagatorBuilder propagatorBuilder =
273 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
274 1.0e-6, 60.0, 0.001);
275
276
277 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
278 propagatorBuilder);
279 final List<ObservedMeasurement<?>> measurements =
280 EstimationTestUtils.createMeasurements(propagator,
281 new TurnAroundRangeMeasurementCreator(context),
282 1.0, 3.0, 300.0);
283 propagator.clearStepHandlers();
284
285 double[] errorsP = new double[3 * measurements.size()];
286 double[] errorsV = new double[3 * measurements.size()];
287 int indexP = 0;
288 int indexV = 0;
289
290
291 if (printResults) {
292 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s " +
293 "%10s %10s %10s " +
294 "%10s %10s %10s " +
295 "%10s %10s %10s " +
296 "%10s %10s %10s%n",
297 "Primary Station", "secondary Station",
298 "Measurement Date", "State Date",
299 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
300 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
301 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
302 }
303
304
305 for (final ObservedMeasurement<?> measurement : measurements) {
306
307
308 final TurnAroundRangeTroposphericDelayModifier modifier =
309 new TurnAroundRangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
310 if (isModifier) {
311 ((TurnAroundRange) measurement).addModifier(modifier);
312 }
313
314
315
316
317
318
319
320
321 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
322 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
323 final SpacecraftState state = propagator.propagate(date);
324 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
325
326
327 final double[][] jacobianRef;
328
329
330 jacobianRef = Differentiation.differentiate(new StateFunction() {
331 public double[] value(final SpacecraftState state) {
332 return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue();
333 }
334 }, measurement.getDimension(), propagator.getAttitudeProvider(),
335 OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(state);
336
337 Assert.assertEquals(jacobianRef.length, jacobian.length);
338 Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
339
340 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
341 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
342 for (int i = 0; i < jacobian.length; ++i) {
343 for (int j = 0; j < jacobian[i].length; ++j) {
344 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
345 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
346
347 if (j < 3) {
348 errorsP[indexP++] = dJacobianRelative[i][j];
349 } else {
350 errorsV[indexV++] = dJacobianRelative[i][j];
351 }
352 }
353 }
354
355 if (printResults) {
356 String primaryStationName = ((TurnAroundRange) measurement).getPrimaryStation().getBaseFrame().getName();
357 String secondaryStationName = ((TurnAroundRange) measurement).getSecondaryStation().getBaseFrame().getName();
358 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s " +
359 "%10.3e %10.3e %10.3e " +
360 "%10.3e %10.3e %10.3e " +
361 "%10.3e %10.3e %10.3e " +
362 "%10.3e %10.3e %10.3e%n",
363 primaryStationName, secondaryStationName, measurement.getDate(), date,
364 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
365 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
366 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
367 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
368 }
369 }
370
371
372 final double errorsPMedian = new Median().evaluate(errorsP);
373 final double errorsPMean = new Mean().evaluate(errorsP);
374 final double errorsPMax = new Max().evaluate(errorsP);
375 final double errorsVMedian = new Median().evaluate(errorsV);
376 final double errorsVMean = new Mean().evaluate(errorsV);
377 final double errorsVMax = new Max().evaluate(errorsV);
378
379
380
381 if (printResults) {
382 System.out.println();
383 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
384 errorsPMedian, errorsPMean, errorsPMax);
385 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
386 errorsVMedian, errorsVMean, errorsVMax);
387 }
388
389 Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
390 Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
391 Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
392 Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
393 Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
394 Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
395 }
396
397
398 void genericTestParameterDerivatives(final boolean isModifier, final boolean printResults,
399 final double refErrorQMMedian, final double refErrorQMMean, final double refErrorQMMax,
400 final double refErrorQSMedian, final double refErrorQSMean, final double refErrorQSMax) {
401
402 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
403
404 final NumericalPropagatorBuilder propagatorBuilder =
405 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
406 1.0e-6, 60.0, 0.001);
407
408
409 for (Map.Entry<GroundStation, GroundStation> entry : context.TARstations.entrySet()) {
410 final GroundStation primaryStation = entry.getKey();
411 final GroundStation secondaryStation = entry.getValue();
412 primaryStation.getClockOffsetDriver().setSelected(true);
413 primaryStation.getEastOffsetDriver().setSelected(true);
414 primaryStation.getNorthOffsetDriver().setSelected(true);
415 primaryStation.getZenithOffsetDriver().setSelected(true);
416 secondaryStation.getClockOffsetDriver().setSelected(false);
417 secondaryStation.getEastOffsetDriver().setSelected(true);
418 secondaryStation.getNorthOffsetDriver().setSelected(true);
419 secondaryStation.getZenithOffsetDriver().setSelected(true);
420 }
421 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
422 propagatorBuilder);
423 final List<ObservedMeasurement<?>> measurements =
424 EstimationTestUtils.createMeasurements(propagator,
425 new TurnAroundRangeMeasurementCreator(context),
426 1.0, 3.0, 300.0);
427 propagator.clearStepHandlers();
428
429
430 if (printResults) {
431 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s " +
432 "%10s %10s %10s " +
433 "%10s %10s %10s " +
434 "%10s %10s %10s " +
435 "%10s %10s %10s%n",
436 "Primary Station", "secondary Station",
437 "Measurement Date", "State Date",
438 "ΔdQMx", "rel ΔdQMx",
439 "ΔdQMy", "rel ΔdQMy",
440 "ΔdQMz", "rel ΔdQMz",
441 "ΔdQSx", "rel ΔdQSx",
442 "ΔdQSy", "rel ΔdQSy",
443 "ΔdQSz", "rel ΔdQSz");
444 }
445
446
447 final List<Double> relErrorQMList = new ArrayList<Double>();
448 final List<Double> relErrorQSList = new ArrayList<Double>();
449
450
451 for (final ObservedMeasurement<?> measurement : measurements) {
452
453
454 final TurnAroundRangeTroposphericDelayModifier modifier = new TurnAroundRangeTroposphericDelayModifier(SaastamoinenModel.getStandardModel());
455 if (isModifier) {
456 ((TurnAroundRange) measurement).addModifier(modifier);
457 }
458
459
460 final GroundStation primaryStationParameter = ((TurnAroundRange) measurement).getPrimaryStation();
461 final GroundStation secondaryStationParameter = ((TurnAroundRange) measurement).getSecondaryStation();
462
463
464
465
466
467
468
469
470 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
471 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
472 final SpacecraftState state = propagator.propagate(date);
473 final ParameterDriver[] drivers = new ParameterDriver[] {
474 primaryStationParameter.getEastOffsetDriver(),
475 primaryStationParameter.getNorthOffsetDriver(),
476 primaryStationParameter.getZenithOffsetDriver(),
477 secondaryStationParameter.getEastOffsetDriver(),
478 secondaryStationParameter.getNorthOffsetDriver(),
479 secondaryStationParameter.getZenithOffsetDriver(),
480 };
481
482
483 if (printResults) {
484 String primaryStationName = primaryStationParameter.getBaseFrame().getName();
485 String secondaryStationName = secondaryStationParameter.getBaseFrame().getName();
486 System.out.format(Locale.US, "%-15s %-15s %-23s %-23s ",
487 primaryStationName, secondaryStationName, measurement.getDate(), date);
488 }
489
490
491 for (int i = 0; i < 6; ++i) {
492 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(drivers[i]);
493 Assert.assertEquals(1, measurement.getDimension());
494 Assert.assertEquals(1, gradient.length);
495
496
497 final ParameterFunction dMkdP =
498 Differentiation.differentiate(new ParameterFunction() {
499
500 @Override
501 public double value(final ParameterDriver parameterDriver) {
502 return measurement.estimate(0, 0, new SpacecraftState[] { state }).getEstimatedValue()[0];
503 }
504 }, 3, 20.0 * drivers[i].getScale());
505 final double ref = dMkdP.value(drivers[i]);
506
507
508 double dGradient = gradient[0] - ref;
509 double dGradientRelative = FastMath.abs(dGradient/ref);
510
511
512 if (printResults) {
513 System.out.format(Locale.US, "%10.3e %10.3e ", dGradient, dGradientRelative);
514 }
515
516
517 if (i<3) { relErrorQMList.add(dGradientRelative);}
518 else { relErrorQSList.add(dGradientRelative);}
519
520 }
521 if (printResults) {
522 System.out.format(Locale.US, "%n");
523 }
524 }
525
526
527 final double relErrorQM[] = relErrorQMList.stream().mapToDouble(Double::doubleValue).toArray();
528 final double relErrorQS[] = relErrorQSList.stream().mapToDouble(Double::doubleValue).toArray();
529
530
531 final double relErrorsQMMedian = new Median().evaluate(relErrorQM);
532 final double relErrorsQMMean = new Mean().evaluate(relErrorQM);
533 final double relErrorsQMMax = new Max().evaluate(relErrorQM);
534
535 final double relErrorsQSMedian = new Median().evaluate(relErrorQS);
536 final double relErrorsQSMean = new Mean().evaluate(relErrorQS);
537 final double relErrorsQSMax = new Max().evaluate(relErrorQS);
538
539
540
541 if (printResults) {
542 System.out.println();
543 System.out.format(Locale.US, "Relative errors dR/dQ primary station -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
544 relErrorsQMMedian, relErrorsQMMean, relErrorsQMMax);
545 System.out.format(Locale.US, "Relative errors dR/dQ secondary station -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
546 relErrorsQSMedian, relErrorsQSMean, relErrorsQSMax);
547 }
548
549
550 Assert.assertEquals(0.0, relErrorsQMMedian, refErrorQMMedian);
551 Assert.assertEquals(0.0, relErrorsQMMean, refErrorQMMean);
552 Assert.assertEquals(0.0, relErrorsQMMax, refErrorQMMax);
553 Assert.assertEquals(0.0, relErrorsQSMedian, refErrorQSMedian);
554 Assert.assertEquals(0.0, relErrorsQSMean, refErrorQSMean);
555 Assert.assertEquals(0.0, relErrorsQSMax, refErrorQSMax);
556
557 }
558 }