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.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.stat.descriptive.moment.Mean;
26 import org.hipparchus.stat.descriptive.rank.Max;
27 import org.hipparchus.stat.descriptive.rank.Median;
28 import org.hipparchus.stat.descriptive.rank.Min;
29 import org.hipparchus.util.FastMath;
30 import org.junit.jupiter.api.Assertions;
31 import org.junit.jupiter.api.Test;
32 import org.orekit.estimation.Context;
33 import org.orekit.estimation.EstimationTestUtils;
34 import org.orekit.orbits.CartesianOrbit;
35 import org.orekit.orbits.Orbit;
36 import org.orekit.orbits.OrbitType;
37 import org.orekit.orbits.PositionAngleType;
38 import org.orekit.propagation.BoundedPropagator;
39 import org.orekit.propagation.EphemerisGenerator;
40 import org.orekit.propagation.Propagator;
41 import org.orekit.propagation.SpacecraftState;
42 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
43 import org.orekit.time.AbsoluteDate;
44 import org.orekit.utils.Constants;
45 import org.orekit.utils.Differentiation;
46 import org.orekit.utils.ParameterDriver;
47 import org.orekit.utils.ParameterFunction;
48 import org.orekit.utils.TimeStampedPVCoordinates;
49 import org.orekit.utils.TimeSpanMap.Span;
50
51 public class InterSatellitesRangeTest {
52
53
54
55
56
57
58 @Test
59 public void testValues() {
60 boolean printResults = false;
61 if (printResults) {
62 System.out.println("\nTest inter-satellites Range Values\n");
63 }
64
65 this.genericTestValues(printResults);
66 }
67
68
69
70
71
72 @Test
73 public void testStateDerivativesEmitter() {
74
75 boolean printResults = false;
76 if (printResults) {
77 System.out.println("\nTest inter-satellites Range State Derivatives - Finite Differences Comparison\n");
78 }
79
80 double refErrorsPMedian = 1.6e-10;
81 double refErrorsPMean = 4.2e-10;
82 double refErrorsPMax = 1.6e-08;
83 double refErrorsVMedian = 1.7e-03;
84 double refErrorsVMean = 3.6e-03;
85 double refErrorsVMax = 7.9e-02;
86 this.genericTestStateDerivatives(printResults, 0,
87 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
88 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
89 }
90
91
92
93
94
95 @Test
96 public void testStateDerivativesTransit() {
97
98 boolean printResults = false;
99 if (printResults) {
100 System.out.println("\nTest inter-satellites Range State Derivatives - Finite Differences Comparison\n");
101 }
102
103 double refErrorsPMedian = 1.6e-10;
104 double refErrorsPMean = 4.2e-10;
105 double refErrorsPMax = 1.6e-08;
106 double refErrorsVMedian = 7.2e-04;
107 double refErrorsVMean = 1.3e-03;
108 double refErrorsVMax = 2.0e-02;
109 this.genericTestStateDerivatives(printResults, 1,
110 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
111 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
112 }
113
114
115
116
117
118 void genericTestValues(final boolean printResults) {
119
120 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
121
122 final NumericalPropagatorBuilder propagatorBuilder =
123 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
124 1.0e-6, 60.0, 0.001);
125
126
127 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
128 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
129 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
130 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
131 context.initialOrbit.getFrame(),
132 context.initialOrbit.getMu());
133 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
134 propagatorBuilder);
135 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
136 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
137 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
138 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
139 propagatorBuilder);
140
141 final double localClockOffset = 0.137e-6;
142 final double remoteClockOffset = 469.0e-6;
143 final List<ObservedMeasurement<?>> measurements =
144 EstimationTestUtils.createMeasurements(propagator,
145 new InterSatellitesRangeMeasurementCreator(ephemeris, localClockOffset, remoteClockOffset),
146 1.0, 3.0, 300.0);
147
148
149
150 final List<Double> absoluteErrors = new ArrayList<>();
151 final List<Double> relativeErrors = new ArrayList<>();
152
153
154
155 propagator.setStepHandler(interpolator -> {
156
157 for (final ObservedMeasurement<?> measurement : measurements) {
158
159
160 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
161 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
162 ) {
163
164
165
166
167
168 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
169 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
170 final SpacecraftState state = interpolator.getInterpolatedState(date);
171
172
173 final double RangeObserved = measurement.getObservedValue()[0];
174 final EstimatedMeasurementBase<?> estimated = measurement.
175 estimateWithoutDerivatives(new SpacecraftState[] {
176 state,
177 ephemeris.propagate(state.getDate())
178 });
179
180 final InterSatellitesRange isr = (InterSatellitesRange) estimated.getObservedMeasurement();
181 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
182 if (!isr.isTwoWay()) {
183 final double dt = participants[1].getDate().durationFrom(participants[0].getDate());
184 Assertions.assertEquals((dt + localClockOffset - remoteClockOffset) * Constants.SPEED_OF_LIGHT,
185 estimated.getEstimatedValue()[0],
186 1.0e-7);
187 } else {
188 final double dt = participants[2].getDate().durationFrom(participants[0].getDate());
189 Assertions.assertEquals(3, participants.length);
190 Assertions.assertEquals(0.5 * dt * Constants.SPEED_OF_LIGHT,
191 estimated.getEstimatedValue()[0],
192 1.0e-7);
193 }
194
195 final double RangeEstimated = estimated.getEstimatedValue()[0];
196 final double absoluteError = RangeEstimated-RangeObserved;
197 absoluteErrors.add(absoluteError);
198 relativeErrors.add(FastMath.abs(absoluteError)/FastMath.abs(RangeObserved));
199
200
201 if (printResults) {
202 final AbsoluteDate measurementDate = measurement.getDate();
203
204 System.out.format(Locale.US, "%-23s %-23s %19.6f %19.6f %13.6e %13.6e%n",
205 measurementDate, date,
206 RangeObserved, RangeEstimated,
207 FastMath.abs(RangeEstimated-RangeObserved),
208 FastMath.abs((RangeEstimated-RangeObserved)/RangeObserved));
209 }
210
211 }
212 }
213 });
214
215
216 if (printResults) {
217 System.out.format(Locale.US, "%-23s %-23s %19s %19s %13s %13s%n",
218 "Measurement Date", "State Date",
219 "Range observed [m]", "Range estimated [m]",
220 "ΔRange [m]", "rel ΔRange");
221 }
222
223
224 propagator.propagate(context.initialOrbit.getDate());
225
226
227 measurements.sort(Comparator.naturalOrder());
228
229
230 propagator.propagate(measurements.get(measurements.size()-1).getDate());
231
232
233 final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
234 final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
235
236
237 final double absErrorsMedian = new Median().evaluate(absErrors);
238 final double absErrorsMin = new Min().evaluate(absErrors);
239 final double absErrorsMax = new Max().evaluate(absErrors);
240 final double relErrorsMedian = new Median().evaluate(relErrors);
241 final double relErrorsMax = new Max().evaluate(relErrors);
242
243
244 if (printResults) {
245 System.out.println();
246 System.out.println("Absolute errors median: " + absErrorsMedian);
247 System.out.println("Absolute errors min : " + absErrorsMin);
248 System.out.println("Absolute errors max : " + absErrorsMax);
249 System.out.println("Relative errors median: " + relErrorsMedian);
250 System.out.println("Relative errors max : " + relErrorsMax);
251 }
252
253 Assertions.assertEquals(0.0, absErrorsMedian, 2.2e-7);
254 Assertions.assertEquals(0.0, absErrorsMin, 1.4e-6);
255 Assertions.assertEquals(0.0, absErrorsMax, 2.0e-7);
256 Assertions.assertEquals(0.0, relErrorsMedian, 4.1e-12);
257 Assertions.assertEquals(0.0, relErrorsMax, 5.0e-11);
258
259
260 Assertions.assertEquals(InterSatellitesRange.MEASUREMENT_TYPE,
261 measurements.get(0).getMeasurementType());
262 }
263
264 void genericTestStateDerivatives(final boolean printResults, final int index,
265 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
266 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
267
268 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
269
270 final NumericalPropagatorBuilder propagatorBuilder =
271 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
272 1.0e-6, 60.0, 0.001);
273
274
275 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
276 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
277 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
278 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
279 context.initialOrbit.getFrame(),
280 context.initialOrbit.getMu());
281 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
282 propagatorBuilder);
283 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
284 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
285 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
286 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
287 propagatorBuilder);
288
289 final double localClockOffset = 0.137e-6;
290 final double remoteClockOffset = 469.0e-6;
291 final List<ObservedMeasurement<?>> measurements =
292 EstimationTestUtils.createMeasurements(propagator,
293 new InterSatellitesRangeMeasurementCreator(ephemeris, localClockOffset, remoteClockOffset),
294 1.0, 3.0, 300.0);
295
296
297
298 final List<Double> errorsP = new ArrayList<>();
299 final List<Double> errorsV = new ArrayList<>();
300
301
302
303 propagator.setStepHandler(interpolator -> {
304
305 for (final ObservedMeasurement<?> measurement : measurements) {
306
307
308 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
309 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
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[] states = {
320 interpolator.getInterpolatedState(date),
321 ephemeris.propagate(date)
322 };
323 final double[][] jacobian = measurement.
324 estimate(0, 0, states).
325 getStateDerivatives(index);
326
327
328 final double[][] jacobianRef;
329
330
331 jacobianRef = Differentiation.differentiate(state1 -> {
332 final SpacecraftState[] s = states.clone();
333 s[index] = state1;
334 return measurement.estimateWithoutDerivatives(s).getEstimatedValue();
335 }, measurement.getDimension(), propagator.getAttitudeProvider(),
336 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(states[index]);
337
338 Assertions.assertEquals(jacobianRef.length, jacobian.length);
339 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
340
341
342 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
343 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
344 for (int i = 0; i < jacobian.length; ++i) {
345 for (int j = 0; j < jacobian[i].length; ++j) {
346 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
347 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
348
349 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
350 } else { errorsV.add(dJacobianRelative[i][j]); }
351 }
352 }
353
354 if (printResults) {
355 System.out.format(Locale.US, "%-23s %-23s " +
356 "%10.3e %10.3e %10.3e " +
357 "%10.3e %10.3e %10.3e " +
358 "%10.3e %10.3e %10.3e " +
359 "%10.3e %10.3e %10.3e%n",
360 measurement.getDate(), date,
361 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
362 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
363 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
364 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
365 }
366 }
367 }
368 });
369
370
371 if (printResults) {
372 System.out.format(Locale.US, "%-23s %-23s " +
373 "%10s %10s %10s " +
374 "%10s %10s %10s " +
375 "%10s %10s %10s " +
376 "%10s %10s %10s%n",
377 "Measurement Date", "State Date",
378 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
379 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
380 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
381 }
382
383
384 propagator.propagate(context.initialOrbit.getDate());
385
386
387 measurements.sort(Comparator.naturalOrder());
388
389
390 propagator.propagate(measurements.get(measurements.size()-1).getDate());
391
392
393 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
394 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
395
396 final double errorsPMedian = new Median().evaluate(relErrorsP);
397 final double errorsPMean = new Mean().evaluate(relErrorsP);
398 final double errorsPMax = new Max().evaluate(relErrorsP);
399 final double errorsVMedian = new Median().evaluate(relErrorsV);
400 final double errorsVMean = new Mean().evaluate(relErrorsV);
401 final double errorsVMax = new Max().evaluate(relErrorsV);
402
403
404 if (printResults) {
405 System.out.println();
406 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
407 errorsPMedian, errorsPMean, errorsPMax);
408 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
409 errorsVMedian, errorsVMean, errorsVMax);
410 }
411
412 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
413 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
414 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
415 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
416 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
417 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
418 }
419
420
421
422
423
424 @Test
425 public void testParameterDerivatives() {
426
427
428 double refErrorsMedian = 1.0e-12;
429 double refErrorsMean = 4.0e-9;
430 double refErrorsMax = 2.0e-7;
431 this.genericTestParameterDerivatives(refErrorsMedian, refErrorsMean, refErrorsMax);
432
433 }
434
435 void genericTestParameterDerivatives(final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
436
437 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
438
439 final NumericalPropagatorBuilder propagatorBuilder =
440 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
441 1.0e-6, 60.0, 0.001);
442
443
444 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
445 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
446 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
447 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
448 context.initialOrbit.getFrame(),
449 context.initialOrbit.getMu());
450 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit, propagatorBuilder);
451 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
452 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
453 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
454
455
456 final double localClockOffset = 0.137e-6;
457 final double remoteClockOffset = 469.0e-6;
458 final InterSatellitesRangeMeasurementCreator creator = new InterSatellitesRangeMeasurementCreator(ephemeris,
459 localClockOffset,
460 remoteClockOffset);
461 creator.getLocalSatellite().getClockOffsetDriver().setSelected(true);
462 creator.getRemoteSatellite().getClockOffsetDriver().setSelected(true);
463
464 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
465 propagatorBuilder);
466 final List<ObservedMeasurement<?>> measurements =
467 EstimationTestUtils.createMeasurements(propagator, creator, 1.0, 3.0, 300.0);
468
469
470 final List<Double> relErrorList = new ArrayList<>();
471
472
473
474 propagator.setStepHandler(interpolator -> {
475
476 for (final ObservedMeasurement<?> measurement : measurements) {
477
478
479 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
480 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
481
482
483
484
485
486
487
488
489 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
490 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
491 final SpacecraftState[] states = {
492 interpolator.getInterpolatedState(date),
493 ephemeris.propagate(date)
494 };
495 ParameterDriver[] drivers = new ParameterDriver[] {
496 measurement.getSatellites().get(0).getClockOffsetDriver(),
497 measurement.getSatellites().get(1).getClockOffsetDriver()
498 };
499
500
501 if (((InterSatellitesRange) measurement).isTwoWay()) {
502 drivers = new ParameterDriver[] {
503 measurement.getSatellites().get(0).getClockOffsetDriver()
504 };
505 }
506
507 for (int i = 0; i < drivers.length; ++i) {
508 for (Span<String> span = drivers[i].getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
509
510 final double[] gradient = measurement.
511 estimate(0, 0, states).getParameterDerivatives(drivers[i], span.getStart());
512 Assertions.assertEquals(1, measurement.getDimension());
513 Assertions.assertEquals(1, gradient.length);
514
515
516 final ParameterFunction dMkdP =
517 Differentiation.differentiate(new ParameterFunction() {
518
519 @Override
520 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
521 return measurement.
522 estimateWithoutDerivatives(states).
523 getEstimatedValue()[0];
524 }
525 }, 3, 20.0 * drivers[i].getScale());
526 final double ref = dMkdP.value(drivers[i], span.getStart());
527
528 final double relError = FastMath.abs((ref-gradient[0])/ref);
529 relErrorList.add(relError);
530 }
531 }
532
533 }
534 }
535 });
536
537
538 propagator.propagate(context.initialOrbit.getDate());
539
540
541 measurements.sort(Comparator.naturalOrder());
542
543
544 propagator.propagate(measurements.get(measurements.size()-1).getDate());
545
546
547 final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
548
549
550 final double relErrorsMedian = new Median().evaluate(relErrors);
551 final double relErrorsMean = new Mean().evaluate(relErrors);
552 final double relErrorsMax = new Max().evaluate(relErrors);
553
554 Assertions.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
555 Assertions.assertEquals(0.0, relErrorsMean, refErrorsMean);
556 Assertions.assertEquals(0.0, relErrorsMax, refErrorsMax);
557
558 }
559
560 }