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.Assert;
31 import org.junit.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.PositionAngle;
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.StateFunction;
49 import org.orekit.utils.TimeStampedPVCoordinates;
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, PositionAngle.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<Double>();
151 final List<Double> relativeErrors = new ArrayList<Double>();
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 EstimatedMeasurement<?> estimated = measurement.estimate(0, 0,
175 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 Assert.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 Assert.assertEquals(3, participants.length);
190 Assert.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 Assert.assertEquals(0.0, absErrorsMedian, 2.2e-7);
254 Assert.assertEquals(0.0, absErrorsMin, 1.4e-6);
255 Assert.assertEquals(0.0, absErrorsMax, 2.0e-7);
256 Assert.assertEquals(0.0, relErrorsMedian, 4.1e-12);
257 Assert.assertEquals(0.0, relErrorsMax, 5.0e-11);
258
259 }
260
261 void genericTestStateDerivatives(final boolean printResults, final int index,
262 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
263 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
264
265 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
266
267 final NumericalPropagatorBuilder propagatorBuilder =
268 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
269 1.0e-6, 60.0, 0.001);
270
271
272 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
273 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
274 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
275 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
276 context.initialOrbit.getFrame(),
277 context.initialOrbit.getMu());
278 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
279 propagatorBuilder);
280 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
281 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
282 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
283 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
284 propagatorBuilder);
285
286 final double localClockOffset = 0.137e-6;
287 final double remoteClockOffset = 469.0e-6;
288 final List<ObservedMeasurement<?>> measurements =
289 EstimationTestUtils.createMeasurements(propagator,
290 new InterSatellitesRangeMeasurementCreator(ephemeris, localClockOffset, remoteClockOffset),
291 1.0, 3.0, 300.0);
292
293
294
295 final List<Double> errorsP = new ArrayList<Double>();
296 final List<Double> errorsV = new ArrayList<Double>();
297
298
299
300 propagator.setStepHandler(interpolator -> {
301
302 for (final ObservedMeasurement<?> measurement : measurements) {
303
304
305 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
306 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
307 ) {
308
309
310
311
312
313
314 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
315 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
316 final SpacecraftState[] states = {
317 interpolator.getInterpolatedState(date),
318 ephemeris.propagate(date)
319 };
320 final double[][] jacobian = measurement.estimate(0, 0, states).getStateDerivatives(index);
321
322
323 final double[][] jacobianRef;
324
325
326 jacobianRef = Differentiation.differentiate(new StateFunction() {
327 public double[] value(final SpacecraftState state) {
328 final SpacecraftState[] s = states.clone();
329 s[index] = state;
330 return measurement.estimate(0, 0, s).getEstimatedValue();
331 }
332 }, measurement.getDimension(), propagator.getAttitudeProvider(),
333 OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(states[index]);
334
335 Assert.assertEquals(jacobianRef.length, jacobian.length);
336 Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
337
338
339 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
340 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
341 for (int i = 0; i < jacobian.length; ++i) {
342 for (int j = 0; j < jacobian[i].length; ++j) {
343 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
344 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
345
346 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
347 } else { errorsV.add(dJacobianRelative[i][j]); }
348 }
349 }
350
351 if (printResults) {
352 System.out.format(Locale.US, "%-23s %-23s " +
353 "%10.3e %10.3e %10.3e " +
354 "%10.3e %10.3e %10.3e " +
355 "%10.3e %10.3e %10.3e " +
356 "%10.3e %10.3e %10.3e%n",
357 measurement.getDate(), date,
358 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
359 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
360 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
361 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
362 }
363 }
364 }
365 });
366
367
368 if (printResults) {
369 System.out.format(Locale.US, "%-23s %-23s " +
370 "%10s %10s %10s " +
371 "%10s %10s %10s " +
372 "%10s %10s %10s " +
373 "%10s %10s %10s%n",
374 "Measurement Date", "State Date",
375 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
376 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
377 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
378 }
379
380
381 propagator.propagate(context.initialOrbit.getDate());
382
383
384 measurements.sort(Comparator.naturalOrder());
385
386
387 propagator.propagate(measurements.get(measurements.size()-1).getDate());
388
389
390 final double relErrorsP[] = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
391 final double relErrorsV[] = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
392
393 final double errorsPMedian = new Median().evaluate(relErrorsP);
394 final double errorsPMean = new Mean().evaluate(relErrorsP);
395 final double errorsPMax = new Max().evaluate(relErrorsP);
396 final double errorsVMedian = new Median().evaluate(relErrorsV);
397 final double errorsVMean = new Mean().evaluate(relErrorsV);
398 final double errorsVMax = new Max().evaluate(relErrorsV);
399
400
401 if (printResults) {
402 System.out.println();
403 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
404 errorsPMedian, errorsPMean, errorsPMax);
405 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
406 errorsVMedian, errorsVMean, errorsVMax);
407 }
408
409 Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
410 Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
411 Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
412 Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
413 Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
414 Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
415 }
416
417
418
419
420
421 @Test
422 public void testParameterDerivatives() {
423
424
425 double refErrorsMedian = 1.0e-12;
426 double refErrorsMean = 4.0e-9;
427 double refErrorsMax = 2.0e-7;
428 this.genericTestParameterDerivatives(refErrorsMedian, refErrorsMean, refErrorsMax);
429
430 }
431
432 void genericTestParameterDerivatives(final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
433
434 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
435
436 final NumericalPropagatorBuilder propagatorBuilder =
437 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
438 1.0e-6, 60.0, 0.001);
439
440
441 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
442 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
443 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
444 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
445 context.initialOrbit.getFrame(),
446 context.initialOrbit.getMu());
447 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit, propagatorBuilder);
448 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
449 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
450 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
451
452
453 final double localClockOffset = 0.137e-6;
454 final double remoteClockOffset = 469.0e-6;
455 final InterSatellitesRangeMeasurementCreator creator = new InterSatellitesRangeMeasurementCreator(ephemeris,
456 localClockOffset,
457 remoteClockOffset);
458 creator.getLocalSatellite().getClockOffsetDriver().setSelected(true);
459 creator.getRemoteSatellite().getClockOffsetDriver().setSelected(true);
460
461 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
462 propagatorBuilder);
463 final List<ObservedMeasurement<?>> measurements =
464 EstimationTestUtils.createMeasurements(propagator, creator, 1.0, 3.0, 300.0);
465
466
467 final List<Double> relErrorList = new ArrayList<Double>();
468
469
470
471 propagator.setStepHandler(interpolator -> {
472
473 for (final ObservedMeasurement<?> measurement : measurements) {
474
475
476 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
477 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
478
479
480
481
482
483
484
485
486 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
487 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
488 final SpacecraftState[] states = {
489 interpolator.getInterpolatedState(date),
490 ephemeris.propagate(date)
491 };
492 ParameterDriver[] drivers = new ParameterDriver[] {
493 measurement.getSatellites().get(0).getClockOffsetDriver(),
494 measurement.getSatellites().get(1).getClockOffsetDriver()
495 };
496
497
498 if (((InterSatellitesRange) measurement).isTwoWay()) {
499 drivers = new ParameterDriver[] {
500 measurement.getSatellites().get(0).getClockOffsetDriver()
501 };
502 }
503
504 for (int i = 0; i < drivers.length; ++i) {
505 final double[] gradient = measurement.estimate(0, 0, states).getParameterDerivatives(drivers[i]);
506 Assert.assertEquals(1, measurement.getDimension());
507 Assert.assertEquals(1, gradient.length);
508
509
510 final ParameterFunction dMkdP =
511 Differentiation.differentiate(new ParameterFunction() {
512
513 @Override
514 public double value(final ParameterDriver parameterDriver) {
515 return measurement.estimate(0, 0, states).getEstimatedValue()[0];
516 }
517 }, 3, 20.0 * drivers[i].getScale());
518 final double ref = dMkdP.value(drivers[i]);
519
520 final double relError = FastMath.abs((ref-gradient[0])/ref);
521 relErrorList.add(relError);
522 }
523
524 }
525 }
526 });
527
528
529 propagator.propagate(context.initialOrbit.getDate());
530
531
532 measurements.sort(Comparator.naturalOrder());
533
534
535 propagator.propagate(measurements.get(measurements.size()-1).getDate());
536
537
538 final double relErrors[] = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
539
540
541 final double relErrorsMedian = new Median().evaluate(relErrors);
542 final double relErrorsMean = new Mean().evaluate(relErrors);
543 final double relErrorsMax = new Max().evaluate(relErrors);
544
545 Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
546 Assert.assertEquals(0.0, relErrorsMean, refErrorsMean);
547 Assert.assertEquals(0.0, relErrorsMax, refErrorsMax);
548
549 }
550
551 }