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