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.Assert;
31 import org.junit.Test;
32 import org.orekit.estimation.Context;
33 import org.orekit.estimation.EstimationTestUtils;
34 import org.orekit.estimation.measurements.EstimatedMeasurement;
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.PositionAngle;
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.StateFunction;
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 double refErrorsPMedian = 1.8e-10;
82 double refErrorsPMean = 5.1e-10;
83 double refErrorsPMax = 3.3e-08;
84 double refErrorsVMedian = 2.3e-03;
85 double refErrorsVMean = 6.7e-03;
86 double refErrorsVMax = 2.3e-01;
87 this.genericTestStateDerivatives(printResults, 0,
88 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
89 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
90 }
91
92
93
94
95
96 @Test
97 public void testParameterDerivatives() {
98
99
100 boolean printResults = false;
101
102 if (printResults) {
103 System.out.println("\nTest One-way GNSS range Derivatives - Finite Differences Comparison\n");
104 }
105
106 double refErrorsMedian = 1.0e-15;
107 double refErrorsMean = 1.0e-15;
108 double refErrorsMax = 1.0e-15;
109 this.genericTestParameterDerivatives(printResults,
110 refErrorsMedian, refErrorsMean, refErrorsMax);
111
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 OneWayGNSSRangeCreator(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 propagator.setStepHandler(interpolator -> {
155
156 for (final ObservedMeasurement<?> measurement : measurements) {
157
158
159 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
160 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
161 ) {
162
163
164
165
166
167 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
168 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
169 final SpacecraftState state = interpolator.getInterpolatedState(date);
170
171
172 final double rangeObserved = measurement.getObservedValue()[0];
173 final EstimatedMeasurement<?> estimated = measurement.estimate(0, 0,
174 new SpacecraftState[] {
175 state,
176 ephemeris.propagate(state.getDate())
177 });
178
179 final double rangeEstimated = estimated.getEstimatedValue()[0];
180 final double absoluteError = rangeEstimated-rangeObserved;
181 absoluteErrors.add(absoluteError);
182 relativeErrors.add(FastMath.abs(absoluteError)/FastMath.abs(rangeObserved));
183
184
185 if (printResults) {
186 final AbsoluteDate measurementDate = measurement.getDate();
187
188 System.out.format(Locale.US, "%-23s %-23s %19.6f %19.6f %13.6e %13.6e%n",
189 measurementDate, date,
190 rangeObserved, rangeEstimated,
191 FastMath.abs(rangeEstimated-rangeObserved),
192 FastMath.abs((rangeEstimated-rangeObserved)/rangeObserved));
193 }
194
195 }
196 }
197 });
198
199
200 if (printResults) {
201 System.out.format(Locale.US, "%-23s %-23s %19s %19s %19s %19s%n",
202 "Measurement Date", "State Date",
203 "Range observed [m]", "Range estimated [m]",
204 "ΔRange [m]", "rel ΔRange");
205 }
206
207
208 propagator.propagate(context.initialOrbit.getDate());
209
210
211 measurements.sort(Comparator.naturalOrder());
212
213
214 propagator.propagate(measurements.get(measurements.size()-1).getDate());
215
216
217 final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
218 final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
219
220
221 final double absErrorsMedian = new Median().evaluate(absErrors);
222 final double absErrorsMin = new Min().evaluate(absErrors);
223 final double absErrorsMax = new Max().evaluate(absErrors);
224 final double relErrorsMedian = new Median().evaluate(relErrors);
225 final double relErrorsMax = new Max().evaluate(relErrors);
226
227
228 if (printResults) {
229 System.out.println();
230 System.out.println("Absolute errors median: " + absErrorsMedian);
231 System.out.println("Absolute errors min : " + absErrorsMin);
232 System.out.println("Absolute errors max : " + absErrorsMax);
233 System.out.println("Relative errors median: " + relErrorsMedian);
234 System.out.println("Relative errors max : " + relErrorsMax);
235 }
236
237 Assert.assertEquals(0.0, absErrorsMedian, 1.3e-7);
238 Assert.assertEquals(0.0, absErrorsMin, 6.5e-6);
239 Assert.assertEquals(0.0, absErrorsMax, 1.7e-7);
240 Assert.assertEquals(0.0, relErrorsMedian, 5.3e-12);
241 Assert.assertEquals(0.0, relErrorsMax, 7.6e-11);
242 }
243
244 void genericTestStateDerivatives(final boolean printResults, final int index,
245 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
246 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
247
248 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
249
250 final NumericalPropagatorBuilder propagatorBuilder =
251 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
252 1.0e-6, 60.0, 0.001);
253
254
255 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
256 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
257 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
258 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
259 context.initialOrbit.getFrame(),
260 context.initialOrbit.getMu());
261 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
262 propagatorBuilder);
263 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
264 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
265 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
266 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
267 propagatorBuilder);
268
269 final double localClockOffset = 0.137e-6;
270 final double remoteClockOffset = 469.0e-6;
271 final List<ObservedMeasurement<?>> measurements =
272 EstimationTestUtils.createMeasurements(propagator,
273 new OneWayGNSSRangeCreator(ephemeris, localClockOffset, remoteClockOffset),
274 1.0, 3.0, 300.0);
275
276
277
278 final List<Double> errorsP = new ArrayList<Double>();
279 final List<Double> errorsV = new ArrayList<Double>();
280
281
282 propagator.setStepHandler(interpolator -> {
283
284 for (final ObservedMeasurement<?> measurement : measurements) {
285
286
287 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
288 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)
289 ) {
290
291
292
293
294
295
296 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
297 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
298 final SpacecraftState[] states = {
299 interpolator.getInterpolatedState(date),
300 ephemeris.propagate(date)
301 };
302 final double[][] jacobian = measurement.estimate(0, 0, states).getStateDerivatives(index);
303
304
305 final double[][] jacobianRef;
306
307
308 jacobianRef = Differentiation.differentiate(new StateFunction() {
309 public double[] value(final SpacecraftState state) {
310 final SpacecraftState[] s = states.clone();
311 s[index] = state;
312 return measurement.estimate(0, 0, s).getEstimatedValue();
313 }
314 }, measurement.getDimension(), propagator.getAttitudeProvider(),
315 OrbitType.CARTESIAN, PositionAngle.TRUE, 2.0, 3).value(states[index]);
316
317 Assert.assertEquals(jacobianRef.length, jacobian.length);
318 Assert.assertEquals(jacobianRef[0].length, jacobian[0].length);
319
320
321 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
322 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
323 for (int i = 0; i < jacobian.length; ++i) {
324 for (int j = 0; j < jacobian[i].length; ++j) {
325 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
326 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
327
328 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
329 } else { errorsV.add(dJacobianRelative[i][j]); }
330 }
331 }
332
333 if (printResults) {
334 System.out.format(Locale.US, "%-23s %-23s " +
335 "%10.3e %10.3e %10.3e " +
336 "%10.3e %10.3e %10.3e " +
337 "%10.3e %10.3e %10.3e " +
338 "%10.3e %10.3e %10.3e%n",
339 measurement.getDate(), date,
340 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
341 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
342 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
343 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
344 }
345 }
346 }
347 });
348
349
350 if (printResults) {
351 System.out.format(Locale.US, "%-23s %-23s " +
352 "%10s %10s %10s " +
353 "%10s %10s %10s " +
354 "%10s %10s %10s " +
355 "%10s %10s %10s%n",
356 "Measurement Date", "State Date",
357 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
358 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
359 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
360 }
361
362
363 propagator.propagate(context.initialOrbit.getDate());
364
365
366 measurements.sort(Comparator.naturalOrder());
367
368
369 propagator.propagate(measurements.get(measurements.size()-1).getDate());
370
371
372 final double relErrorsP[] = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
373 final double relErrorsV[] = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
374
375 final double errorsPMedian = new Median().evaluate(relErrorsP);
376 final double errorsPMean = new Mean().evaluate(relErrorsP);
377 final double errorsPMax = new Max().evaluate(relErrorsP);
378 final double errorsVMedian = new Median().evaluate(relErrorsV);
379 final double errorsVMean = new Mean().evaluate(relErrorsV);
380 final double errorsVMax = new Max().evaluate(relErrorsV);
381
382
383 if (printResults) {
384 System.out.println();
385 System.out.format(Locale.US, "Relative errors dR/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
386 errorsPMedian, errorsPMean, errorsPMax);
387 System.out.format(Locale.US, "Relative errors dR/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
388 errorsVMedian, errorsVMean, errorsVMax);
389 }
390
391 Assert.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
392 Assert.assertEquals(0.0, errorsPMean, refErrorsPMean);
393 Assert.assertEquals(0.0, errorsPMax, refErrorsPMax);
394 Assert.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
395 Assert.assertEquals(0.0, errorsVMean, refErrorsVMean);
396 Assert.assertEquals(0.0, errorsVMax, refErrorsVMax);
397 }
398
399 void genericTestParameterDerivatives(final boolean printResults,
400 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
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 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
410 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
411 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
412 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
413 context.initialOrbit.getFrame(),
414 context.initialOrbit.getMu());
415 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit, propagatorBuilder);
416 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
417 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
418 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
419
420
421 final double localClockOffset = 0.137e-6;
422 final double remoteClockOffset = 469.0e-6;
423 final OneWayGNSSRangeCreator creator = new OneWayGNSSRangeCreator(ephemeris, localClockOffset, remoteClockOffset);
424 creator.getLocalSatellite().getClockOffsetDriver().setSelected(true);
425
426 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
427 propagatorBuilder);
428 final List<ObservedMeasurement<?>> measurements =
429 EstimationTestUtils.createMeasurements(propagator, creator, 1.0, 3.0, 300.0);
430
431
432 final List<Double> relErrorList = new ArrayList<Double>();
433
434
435 propagator.setStepHandler(interpolator -> {
436
437 for (final ObservedMeasurement<?> measurement : measurements) {
438
439
440 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
441 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
442
443
444
445
446
447
448
449
450 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
451 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
452 final SpacecraftState[] states = {
453 interpolator.getInterpolatedState(date),
454 ephemeris.propagate(date)
455 };
456 final ParameterDriver[] drivers = new ParameterDriver[] {
457 measurement.getSatellites().get(0).getClockOffsetDriver(),
458 };
459
460 for (int i = 0; i < drivers.length; ++i) {
461 final double[] gradient = measurement.estimate(0, 0, states).getParameterDerivatives(drivers[i]);
462 Assert.assertEquals(1, measurement.getDimension());
463 Assert.assertEquals(1, gradient.length);
464
465
466 final ParameterFunction dMkdP =
467 Differentiation.differentiate(new ParameterFunction() {
468
469 @Override
470 public double value(final ParameterDriver parameterDriver) {
471 return measurement.estimate(0, 0, states).getEstimatedValue()[0];
472 }
473 }, 3, 20.0 * drivers[i].getScale());
474 final double ref = dMkdP.value(drivers[i]);
475
476 if (printResults) {
477 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
478 }
479
480 final double relError = FastMath.abs((ref-gradient[0])/ref);
481 relErrorList.add(relError);
482 }
483 if (printResults) {
484 System.out.format(Locale.US, "%n");
485 }
486
487 }
488 }
489 });
490
491
492 propagator.propagate(context.initialOrbit.getDate());
493
494
495 measurements.sort(Comparator.naturalOrder());
496
497
498 if (printResults) {
499 System.out.format(Locale.US, "%-15s %-23s %-23s " +
500 "%10s %10s %10s %10s %10s %10s %10s %10s %10s %10s%n",
501 "Station", "Measurement Date", "State Date",
502 "Δt", "rel Δt",
503 "ΔdQx", "rel ΔdQx",
504 "ΔdQy", "rel ΔdQy",
505 "ΔdQz", "rel ΔdQz",
506 "Δtsat", "rel Δtsat");
507 }
508
509
510 propagator.propagate(measurements.get(measurements.size()-1).getDate());
511
512
513 final double relErrors[] = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
514
515
516 final double relErrorsMedian = new Median().evaluate(relErrors);
517 final double relErrorsMean = new Mean().evaluate(relErrors);
518 final double relErrorsMax = new Max().evaluate(relErrors);
519
520
521 if (printResults) {
522 System.out.println();
523 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
524 relErrorsMedian, relErrorsMean, relErrorsMax);
525 }
526
527 Assert.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
528 Assert.assertEquals(0.0, relErrorsMean, refErrorsMean);
529 Assert.assertEquals(0.0, relErrorsMax, refErrorsMax);
530
531 }
532
533 }