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.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.jupiter.api.Assertions;
30 import org.junit.jupiter.api.Test;
31 import org.orekit.Utils;
32 import org.orekit.bodies.GeodeticPoint;
33 import org.orekit.bodies.OneAxisEllipsoid;
34 import org.orekit.estimation.Context;
35 import org.orekit.estimation.EstimationTestUtils;
36 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
37 import org.orekit.estimation.measurements.GroundStation;
38 import org.orekit.estimation.measurements.ObservableSatellite;
39 import org.orekit.estimation.measurements.ObservedMeasurement;
40 import org.orekit.estimation.measurements.modifiers.PhaseIonosphericDelayModifier;
41 import org.orekit.estimation.measurements.modifiers.PhaseTroposphericDelayModifier;
42 import org.orekit.frames.FramesFactory;
43 import org.orekit.frames.TopocentricFrame;
44 import org.orekit.gnss.PredefinedGnssSignal;
45 import org.orekit.models.earth.ionosphere.IonosphericModel;
46 import org.orekit.models.earth.ionosphere.KlobucharIonoModel;
47 import org.orekit.models.earth.troposphere.EstimatedModel;
48 import org.orekit.models.earth.troposphere.NiellMappingFunctionModel;
49 import org.orekit.orbits.OrbitType;
50 import org.orekit.orbits.PositionAngleType;
51 import org.orekit.propagation.Propagator;
52 import org.orekit.propagation.SpacecraftState;
53 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
54 import org.orekit.time.AbsoluteDate;
55 import org.orekit.utils.Constants;
56 import org.orekit.utils.Differentiation;
57 import org.orekit.utils.IERSConventions;
58 import org.orekit.utils.ParameterDriver;
59 import org.orekit.utils.ParameterFunction;
60 import org.orekit.utils.TimeStampedPVCoordinates;
61
62 public class PhaseTest {
63
64
65
66
67
68 @Test
69 public void testValues() {
70 boolean printResults = false;
71 if (printResults) {
72 System.out.println("\nTest Phase Values\n");
73 }
74
75 this.genericTestValues(printResults);
76 }
77
78
79
80
81
82 @Test
83 public void testStateDerivatives() {
84
85 boolean printResults = false;
86 if (printResults) {
87 System.out.println("\nTest Range Phase Derivatives - Finite Differences Comparison\n");
88 }
89
90 double refErrorsPMedian = 5.4e-10;
91 double refErrorsPMean = 3.9e-09;
92 double refErrorsPMax = 2.2e-07;
93 double refErrorsVMedian = 2.4e-05;
94 double refErrorsVMean = 7.4e-05;
95 double refErrorsVMax = 2.1e-03;
96 this.genericTestStateDerivatives(printResults,
97 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
98 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
99 }
100
101
102
103
104
105 @Test
106 public void testStateDerivativesWithModifier() {
107
108 boolean printResults = false;
109 if (printResults) {
110 System.out.println("\nTest Phase State Derivatives with Modifier - Finite Differences Comparison\n");
111 }
112
113 double refErrorsPMedian = 5.4e-10;
114 double refErrorsPMean = 3.9e-09;
115 double refErrorsPMax = 2.2e-07;
116 double refErrorsVMedian = 2.4e-05;
117 double refErrorsVMean = 7.4e-05;
118 double refErrorsVMax = 2.1e-03;
119 this.genericTestStateDerivatives(printResults,
120 refErrorsPMedian, refErrorsPMean, refErrorsPMax,
121 refErrorsVMedian, refErrorsVMean, refErrorsVMax);
122 }
123
124
125
126
127
128 @Test
129 public void testParameterDerivatives() {
130
131
132 boolean printResults = false;
133
134 if (printResults) {
135 System.out.println("\nTest Phase Parameter Derivatives - Finite Differences Comparison\n");
136 }
137
138 double refErrorsMedian = 1.4e-10;
139 double refErrorsMean = 5.0e-8;
140 double refErrorsMax = 5.1e-6;
141 this.genericTestParameterDerivatives(printResults,
142 refErrorsMedian, refErrorsMean, refErrorsMax);
143
144 }
145
146
147
148
149
150 void genericTestValues(final boolean printResults) {
151
152 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
153
154 final NumericalPropagatorBuilder propagatorBuilder =
155 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
156 1.0e-6, 60.0, 0.001);
157
158
159 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
160 propagatorBuilder);
161 final double groundClockOffset = 12.0e-6;
162 for (final GroundStation station : context.stations) {
163 station.getClockOffsetDriver().setValue(groundClockOffset);
164 }
165 final int ambiguity = 1234;
166 final double satClockOffset = 345.0e-6;
167 final List<ObservedMeasurement<?>> measurements =
168 EstimationTestUtils.createMeasurements(propagator,
169 new PhaseMeasurementCreator(context,
170 PredefinedGnssSignal.E01,
171 ambiguity,
172 satClockOffset),
173 1.0, 3.0, 300.0);
174
175
176
177 final List<Double> absoluteErrors = new ArrayList<>();
178 final List<Double> relativeErrors = new ArrayList<>();
179
180
181 propagator.setStepHandler(interpolator -> {
182
183 for (final ObservedMeasurement<?> measurement : measurements) {
184
185
186 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
187 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
188
189
190
191
192
193
194
195 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
196 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
197 final SpacecraftState state = interpolator.getInterpolatedState(date);
198
199
200 final double phaseObserved = measurement.getObservedValue()[0];
201 final EstimatedMeasurementBase<?> estimated = measurement.estimateWithoutDerivatives(new SpacecraftState[] { state });
202
203 final TimeStampedPVCoordinates[] participants = estimated.getParticipants();
204 Assertions.assertEquals(2, participants.length);
205 final double dt = participants[1].getDate().durationFrom(participants[0].getDate());
206 Assertions.assertEquals(PredefinedGnssSignal.E01.getFrequency() * (dt + groundClockOffset - satClockOffset) + ambiguity,
207 estimated.getEstimatedValue()[0],
208 1.0e-7);
209
210 final double phaseEstimated = estimated.getEstimatedValue()[0];
211 final double absoluteError = phaseEstimated - phaseObserved;
212 absoluteErrors.add(absoluteError);
213 relativeErrors.add(FastMath.abs(absoluteError) / FastMath.abs(phaseObserved));
214
215
216 if (printResults) {
217 final AbsoluteDate measurementDate = measurement.getDate();
218 String stationName = ((Phase) measurement).getStation().getBaseFrame().getName();
219
220 System.out.format(Locale.US, "%-15s %-23s %-23s %19.6f %19.6f %13.6e %13.6e%n",
221 stationName, measurementDate, date,
222 phaseObserved, phaseEstimated,
223 FastMath.abs(phaseEstimated - phaseObserved),
224 FastMath.abs((phaseEstimated - phaseObserved) / phaseObserved));
225 }
226
227 }
228 }
229 });
230
231
232 if (printResults) {
233 System.out.format(Locale.US, "%-15s %-23s %-23s %19s %19s %13s %13s%n",
234 "Station", "Measurement Date", "State Date",
235 "Phase observed [cycle]", "Phase estimated [cycle]",
236 "ΔPhase [cycle]", "rel ΔPhase");
237 }
238
239
240 propagator.propagate(context.initialOrbit.getDate());
241
242
243 measurements.sort(Comparator.naturalOrder());
244
245
246 propagator.propagate(measurements.get(measurements.size()-1).getDate());
247
248
249 final double[] absErrors = absoluteErrors.stream().mapToDouble(Double::doubleValue).toArray();
250 final double[] relErrors = relativeErrors.stream().mapToDouble(Double::doubleValue).toArray();
251
252
253 final double absErrorsMedian = new Median().evaluate(absErrors);
254 final double absErrorsMin = new Min().evaluate(absErrors);
255 final double absErrorsMax = new Max().evaluate(absErrors);
256 final double relErrorsMedian = new Median().evaluate(relErrors);
257 final double relErrorsMax = new Max().evaluate(relErrors);
258
259
260 if (printResults) {
261 System.out.println();
262 System.out.println("Absolute errors median: " + absErrorsMedian);
263 System.out.println("Absolute errors min : " + absErrorsMin);
264 System.out.println("Absolute errors max : " + absErrorsMax);
265 System.out.println("Relative errors median: " + relErrorsMedian);
266 System.out.println("Relative errors max : " + relErrorsMax);
267 }
268
269 Assertions.assertEquals(0.0, absErrorsMedian, 1.4e-7);
270 Assertions.assertEquals(0.0, absErrorsMin, 1.3e-6);
271 Assertions.assertEquals(0.0, absErrorsMax, 1.5e-6);
272 Assertions.assertEquals(0.0, relErrorsMedian, 9.1e-15);
273 Assertions.assertEquals(0.0, relErrorsMax, 2.8e-14);
274
275
276 Assertions.assertEquals(Phase.MEASUREMENT_TYPE, measurements.get(0).getMeasurementType());
277 }
278
279 void genericTestStateDerivatives(final boolean printResults,
280 final double refErrorsPMedian, final double refErrorsPMean, final double refErrorsPMax,
281 final double refErrorsVMedian, final double refErrorsVMean, final double refErrorsVMax) {
282
283 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
284
285 final NumericalPropagatorBuilder propagatorBuilder =
286 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
287 1.0e-6, 60.0, 0.001);
288
289
290 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
291 propagatorBuilder);
292 final double groundClockOffset = 12.0e-6;
293 for (final GroundStation station : context.stations) {
294 station.getClockOffsetDriver().setValue(groundClockOffset);
295 }
296 final int ambiguity = 1234;
297 final double satClockOffset = 345.0e-6;
298 final List<ObservedMeasurement<?>> measurements =
299 EstimationTestUtils.createMeasurements(propagator,
300 new PhaseMeasurementCreator(context,
301 PredefinedGnssSignal.E01,
302 ambiguity,
303 satClockOffset),
304 1.0, 3.0, 300.0);
305
306
307
308 final List<Double> errorsP = new ArrayList<>();
309 final List<Double> errorsV = new ArrayList<>();
310
311
312 propagator.setStepHandler(interpolator -> {
313
314 for (final ObservedMeasurement<?> measurement : measurements) {
315
316
317 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
318 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
319
320
321
322
323
324
325
326
327 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
328 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
329 final SpacecraftState state = interpolator.getInterpolatedState(date);
330 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
331
332
333 final double[][] jacobianRef;
334
335
336 jacobianRef = Differentiation.differentiate(
337 state1 -> measurement.
338 estimateWithoutDerivatives(new SpacecraftState[] {
339 state1
340 }).
341 getEstimatedValue(), measurement.getDimension(), propagator.getAttitudeProvider(),
342 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(state);
343
344 Assertions.assertEquals(jacobianRef.length, jacobian.length);
345 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
346
347
348 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
349 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
350 for (int i = 0; i < jacobian.length; ++i) {
351 for (int j = 0; j < jacobian[i].length; ++j) {
352 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
353 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
354
355 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
356 } else { errorsV.add(dJacobianRelative[i][j]); }
357 }
358 }
359
360 if (printResults) {
361 String stationName = ((Phase) measurement).getStation().getBaseFrame().getName();
362 System.out.format(Locale.US, "%-15s %-23s %-23s " +
363 "%10.3e %10.3e %10.3e " +
364 "%10.3e %10.3e %10.3e " +
365 "%10.3e %10.3e %10.3e " +
366 "%10.3e %10.3e %10.3e%n",
367 stationName, measurement.getDate(), date,
368 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
369 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
370 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
371 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
372 }
373 }
374 }
375 });
376
377
378 if (printResults) {
379 System.out.format(Locale.US, "%-15s %-23s %-23s " +
380 "%10s %10s %10s " +
381 "%10s %10s %10s " +
382 "%10s %10s %10s " +
383 "%10s %10s %10s%n",
384 "Station", "Measurement Date", "State Date",
385 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
386 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
387 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
388 }
389
390
391 propagator.propagate(context.initialOrbit.getDate());
392
393
394 measurements.sort(Comparator.naturalOrder());
395
396
397 propagator.propagate(measurements.get(measurements.size()-1).getDate());
398
399
400 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
401 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
402
403 final double errorsPMedian = new Median().evaluate(relErrorsP);
404 final double errorsPMean = new Mean().evaluate(relErrorsP);
405 final double errorsPMax = new Max().evaluate(relErrorsP);
406 final double errorsVMedian = new Median().evaluate(relErrorsV);
407 final double errorsVMean = new Mean().evaluate(relErrorsV);
408 final double errorsVMax = new Max().evaluate(relErrorsV);
409
410
411 if (printResults) {
412 System.out.println();
413 System.out.format(Locale.US, "Relative errors dΦ/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
414 errorsPMedian, errorsPMean, errorsPMax);
415 System.out.format(Locale.US, "Relative errors dΦ/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
416 errorsVMedian, errorsVMean, errorsVMax);
417 }
418
419 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
420 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
421 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
422 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
423 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
424 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
425 }
426
427 void genericTestParameterDerivatives(final boolean printResults,
428 final double refErrorsMedian, final double refErrorsMean, final double refErrorsMax) {
429
430 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
431
432 final NumericalPropagatorBuilder propagatorBuilder =
433 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
434 1.0e-6, 60.0, 0.001);
435
436
437 final double groundClockOffset = 12.0e-6;
438 for (final GroundStation station : context.stations) {
439 station.getClockOffsetDriver().setValue(groundClockOffset);
440 }
441 final int ambiguity = 1234;
442 final double satClockOffset = 345.0e-6;
443 final PhaseMeasurementCreator creator = new PhaseMeasurementCreator(context,
444 PredefinedGnssSignal.E01,
445 ambiguity,
446 satClockOffset);
447 creator.getSatellite().getClockOffsetDriver().setSelected(true);
448 for (final GroundStation station : context.stations) {
449 station.getClockOffsetDriver().setSelected(true);
450 station.getEastOffsetDriver().setSelected(true);
451 station.getNorthOffsetDriver().setSelected(true);
452 station.getZenithOffsetDriver().setSelected(true);
453 }
454 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
455 propagatorBuilder);
456 final List<ObservedMeasurement<?>> measurements =
457 EstimationTestUtils.createMeasurements(propagator, creator, 1.0, 3.0, 300.0);
458
459
460 final List<Double> relErrorList = new ArrayList<>();
461
462
463 propagator.setStepHandler(interpolator -> {
464
465 for (final ObservedMeasurement<?> measurement : measurements) {
466
467
468 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
469 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
470
471
472 final GroundStation stationParameter = ((Phase) measurement).getStation();
473
474
475
476
477
478
479
480
481 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
482 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
483 final SpacecraftState state = interpolator.getInterpolatedState(date);
484 final ParameterDriver[] drivers = new ParameterDriver[] {
485 stationParameter.getClockOffsetDriver(),
486 stationParameter.getEastOffsetDriver(),
487 stationParameter.getNorthOffsetDriver(),
488 stationParameter.getZenithOffsetDriver(),
489 measurement.getSatellites().get(0).getClockOffsetDriver()
490 };
491
492 if (printResults) {
493 String stationName = ((Phase) measurement).getStation().getBaseFrame().getName();
494 System.out.format(Locale.US, "%-15s %-23s %-23s ",
495 stationName, measurement.getDate(), date);
496 }
497
498 for (final ParameterDriver driver : drivers) {
499 final double[] gradient = measurement.estimate(0, 0, new SpacecraftState[] { state }).getParameterDerivatives(driver);
500 Assertions.assertEquals(1, measurement.getDimension());
501 Assertions.assertEquals(1, gradient.length);
502
503
504 final ParameterFunction dMkdP =
505 Differentiation.differentiate(new ParameterFunction() {
506
507 @Override
508 public double value(final ParameterDriver parameterDriver, final AbsoluteDate date) {
509 return measurement.
510 estimateWithoutDerivatives(new SpacecraftState[] { state }).
511 getEstimatedValue()[0];
512 }
513 }, 3, 20.0 * driver.getScale());
514 final double ref = dMkdP.value(driver, date);
515
516 if (printResults) {
517 System.out.format(Locale.US, "%10.3e %10.3e ", gradient[0]-ref, FastMath.abs((gradient[0]-ref)/ref));
518 }
519
520 final double relError = FastMath.abs((ref-gradient[0])/ref);
521 relErrorList.add(relError);
522 Assertions.assertEquals(ref, gradient[0], 6.1e-5 * FastMath.abs(ref));
523 }
524 if (printResults) {
525 System.out.format(Locale.US, "%n");
526 }
527
528 }
529 }
530 });
531
532
533 propagator.propagate(context.initialOrbit.getDate());
534
535
536 measurements.sort(Comparator.naturalOrder());
537
538
539 if (printResults) {
540 System.out.format(Locale.US, "%-15s %-23s %-23s " +
541 "%10s %10s %10s %10s %10s %10s %10s %10s %10s %10s%n",
542 "Station", "Measurement Date", "State Date",
543 "Δt", "rel Δt",
544 "ΔdQx", "rel ΔdQx",
545 "ΔdQy", "rel ΔdQy",
546 "ΔdQz", "rel ΔdQz",
547 "Δtsat", "rel Δtsat");
548 }
549
550
551 propagator.propagate(measurements.get(measurements.size()-1).getDate());
552
553
554 final double[] relErrors = relErrorList.stream().mapToDouble(Double::doubleValue).toArray();
555
556
557 final double relErrorsMedian = new Median().evaluate(relErrors);
558 final double relErrorsMean = new Mean().evaluate(relErrors);
559 final double relErrorsMax = new Max().evaluate(relErrors);
560
561
562 if (printResults) {
563 System.out.println();
564 System.out.format(Locale.US, "Relative errors dR/dQ -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
565 relErrorsMedian, relErrorsMean, relErrorsMax);
566 }
567
568 Assertions.assertEquals(0.0, relErrorsMedian, refErrorsMedian);
569 Assertions.assertEquals(0.0, relErrorsMean, refErrorsMean);
570 Assertions.assertEquals(0.0, relErrorsMax, refErrorsMax);
571
572 }
573
574 @Test
575 public void testStateDerivativesWithTroposphericModifier() {
576
577 final boolean printResults = false;
578 final double refErrorsPMedian = 6.0e-10;
579 final double refErrorsPMean = 5.2e-9;
580 final double refErrorsPMax = 3.7e-7;
581 final double refErrorsVMedian = 2.4e-5;
582 final double refErrorsVMean = 8.9e-5;
583 final double refErrorsVMax = 2.7e-3;
584
585 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
586
587 final NumericalPropagatorBuilder propagatorBuilder =
588 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
589 1.0e-6, 60.0, 0.001);
590
591
592 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
593 propagatorBuilder);
594 final double groundClockOffset = 12.0e-6;
595 for (final GroundStation station : context.stations) {
596 station.getClockOffsetDriver().setValue(groundClockOffset);
597 }
598 final int ambiguity = 1234;
599 final double satClockOffset = 345.0e-6;
600 final List<ObservedMeasurement<?>> measurements =
601 EstimationTestUtils.createMeasurements(propagator,
602 new PhaseMeasurementCreator(context,
603 PredefinedGnssSignal.E01,
604 ambiguity,
605 satClockOffset),
606 1.0, 3.0, 300.0);
607
608
609
610 final List<Double> errorsP = new ArrayList<>();
611 final List<Double> errorsV = new ArrayList<>();
612
613
614 propagator.setStepHandler(interpolator -> {
615
616 for (final ObservedMeasurement<?> measurement : measurements) {
617
618
619 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
620 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
621
622 String stationName = ((Phase) measurement).getStation().getBaseFrame().getName();
623
624
625 final NiellMappingFunctionModel mappingFunction = new NiellMappingFunctionModel();
626 final EstimatedModel tropoModel = new EstimatedModel(mappingFunction, 5.0);
627 final PhaseTroposphericDelayModifier modifier = new PhaseTroposphericDelayModifier(tropoModel);
628 final List<ParameterDriver> parameters = modifier.getParametersDrivers();
629 parameters.get(0).setName(stationName + "/" + EstimatedModel.TOTAL_ZENITH_DELAY);
630 parameters.get(0).setSelected(true);
631 ((Phase) measurement).addModifier(modifier);
632
633
634
635
636
637
638
639
640 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
641 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
642 final SpacecraftState state = interpolator.getInterpolatedState(date);
643 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
644
645
646 final double[][] jacobianRef;
647
648
649 jacobianRef = Differentiation.differentiate(
650 state1 -> measurement.
651 estimateWithoutDerivatives(new SpacecraftState[] {
652 state1
653 }).
654 getEstimatedValue(), measurement.getDimension(), propagator.getAttitudeProvider(),
655 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(state);
656
657 Assertions.assertEquals(jacobianRef.length, jacobian.length);
658 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
659
660
661 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
662 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
663 for (int i = 0; i < jacobian.length; ++i) {
664 for (int j = 0; j < jacobian[i].length; ++j) {
665 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
666 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
667
668 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
669 } else { errorsV.add(dJacobianRelative[i][j]); }
670 }
671 }
672
673 if (printResults) {
674 System.out.format(Locale.US, "%-15s %-23s %-23s " +
675 "%10.3e %10.3e %10.3e " +
676 "%10.3e %10.3e %10.3e " +
677 "%10.3e %10.3e %10.3e " +
678 "%10.3e %10.3e %10.3e%n",
679 stationName, measurement.getDate(), date,
680 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
681 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
682 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
683 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
684 }
685 }
686 }
687 });
688
689
690 if (printResults) {
691 System.out.format(Locale.US, "%-15s %-23s %-23s " +
692 "%10s %10s %10s " +
693 "%10s %10s %10s " +
694 "%10s %10s %10s " +
695 "%10s %10s %10s%n",
696 "Station", "Measurement Date", "State Date",
697 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
698 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
699 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
700 }
701
702
703 propagator.propagate(context.initialOrbit.getDate());
704
705
706 measurements.sort(Comparator.naturalOrder());
707
708
709 propagator.propagate(measurements.get(measurements.size()-1).getDate());
710
711
712 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
713 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
714
715 final double errorsPMedian = new Median().evaluate(relErrorsP);
716 final double errorsPMean = new Mean().evaluate(relErrorsP);
717 final double errorsPMax = new Max().evaluate(relErrorsP);
718 final double errorsVMedian = new Median().evaluate(relErrorsV);
719 final double errorsVMean = new Mean().evaluate(relErrorsV);
720 final double errorsVMax = new Max().evaluate(relErrorsV);
721
722
723 if (printResults) {
724 System.out.println();
725 System.out.format(Locale.US, "Relative errors dΦ/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
726 errorsPMedian, errorsPMean, errorsPMax);
727 System.out.format(Locale.US, "Relative errors dΦ/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
728 errorsVMedian, errorsVMean, errorsVMax);
729 }
730
731 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
732 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
733 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
734 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
735 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
736 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
737 }
738
739 @Test
740 public void testStateDerivativesWithIonosphericModifier() {
741
742 final boolean printResults = false;
743 final double refErrorsPMedian = 6.7e-10;
744 final double refErrorsPMean = 3.0e-9;
745 final double refErrorsPMax = 9.0e-8;
746 final double refErrorsVMedian = 2.4e-5;
747 final double refErrorsVMean = 7.5e-5;
748 final double refErrorsVMax = 2.1e-3;
749
750 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
751
752 final NumericalPropagatorBuilder propagatorBuilder =
753 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
754 1.0e-6, 60.0, 0.001);
755
756
757 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
758 propagatorBuilder);
759 final double groundClockOffset = 12.0e-6;
760 for (final GroundStation station : context.stations) {
761 station.getClockOffsetDriver().setValue(groundClockOffset);
762 }
763 final int ambiguity = 1234;
764 final double satClockOffset = 345.0e-6;
765 final List<ObservedMeasurement<?>> measurements =
766 EstimationTestUtils.createMeasurements(propagator,
767 new PhaseMeasurementCreator(context,
768 PredefinedGnssSignal.E01,
769 ambiguity,
770 satClockOffset),
771 1.0, 3.0, 300.0);
772
773
774
775 final List<Double> errorsP = new ArrayList<>();
776 final List<Double> errorsV = new ArrayList<>();
777
778 final IonosphericModel model = new KlobucharIonoModel(new double[]{.3820e-07, .1490e-07, -.1790e-06, 0},
779 new double[]{.1430e+06, 0, -.3280e+06, .1130e+06});
780 final double frequency = PredefinedGnssSignal.G01.getFrequency();
781 final PhaseIonosphericDelayModifier modifier = new PhaseIonosphericDelayModifier(model, frequency);
782
783
784 propagator.setStepHandler(interpolator -> {
785
786 for (final ObservedMeasurement<?> measurement : measurements) {
787
788 final Phase phase = (Phase) measurement;
789
790 if ((measurement.getDate().durationFrom(interpolator.getPreviousState().getDate()) > 0.) &&
791 (measurement.getDate().durationFrom(interpolator.getCurrentState().getDate()) <= 0.)) {
792
793
794 phase.addModifier(modifier);
795
796
797
798
799
800
801
802
803 final double meanDelay = measurement.getObservedValue()[0] / Constants.SPEED_OF_LIGHT;
804 final AbsoluteDate date = measurement.getDate().shiftedBy(-0.75 * meanDelay);
805 final SpacecraftState state = interpolator.getInterpolatedState(date);
806 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
807
808
809 final double[][] jacobianRef;
810
811
812 jacobianRef = Differentiation.differentiate(
813 state1 -> measurement.
814 estimateWithoutDerivatives(new SpacecraftState[] {
815 state1
816 }).
817 getEstimatedValue(), measurement.getDimension(), propagator.getAttitudeProvider(),
818 OrbitType.CARTESIAN, PositionAngleType.TRUE, 2.0, 3).value(state);
819
820 Assertions.assertEquals(jacobianRef.length, jacobian.length);
821 Assertions.assertEquals(jacobianRef[0].length, jacobian[0].length);
822
823
824 double [][] dJacobian = new double[jacobian.length][jacobian[0].length];
825 double [][] dJacobianRelative = new double[jacobian.length][jacobian[0].length];
826 for (int i = 0; i < jacobian.length; ++i) {
827 for (int j = 0; j < jacobian[i].length; ++j) {
828 dJacobian[i][j] = jacobian[i][j] - jacobianRef[i][j];
829 dJacobianRelative[i][j] = FastMath.abs(dJacobian[i][j]/jacobianRef[i][j]);
830
831 if (j < 3) { errorsP.add(dJacobianRelative[i][j]);
832 } else { errorsV.add(dJacobianRelative[i][j]); }
833 }
834 }
835
836 if (printResults) {
837 System.out.format(Locale.US, "%-15s %-23s %-23s " +
838 "%10.3e %10.3e %10.3e " +
839 "%10.3e %10.3e %10.3e " +
840 "%10.3e %10.3e %10.3e " +
841 "%10.3e %10.3e %10.3e%n",
842 phase.getStation().getBaseFrame().getName(), measurement.getDate(), date,
843 dJacobian[0][0], dJacobian[0][1], dJacobian[0][2],
844 dJacobian[0][3], dJacobian[0][4], dJacobian[0][5],
845 dJacobianRelative[0][0], dJacobianRelative[0][1], dJacobianRelative[0][2],
846 dJacobianRelative[0][3], dJacobianRelative[0][4], dJacobianRelative[0][5]);
847 }
848 }
849 }
850 });
851
852
853 if (printResults) {
854 System.out.format(Locale.US, "%-15s %-23s %-23s " +
855 "%10s %10s %10s " +
856 "%10s %10s %10s " +
857 "%10s %10s %10s " +
858 "%10s %10s %10s%n",
859 "Station", "Measurement Date", "State Date",
860 "ΔdPx", "ΔdPy", "ΔdPz", "ΔdVx", "ΔdVy", "ΔdVz",
861 "rel ΔdPx", "rel ΔdPy", "rel ΔdPz",
862 "rel ΔdVx", "rel ΔdVy", "rel ΔdVz");
863 }
864
865
866 propagator.propagate(context.initialOrbit.getDate());
867
868
869 measurements.sort(Comparator.naturalOrder());
870
871
872 propagator.propagate(measurements.get(measurements.size()-1).getDate());
873
874
875 final double[] relErrorsP = errorsP.stream().mapToDouble(Double::doubleValue).toArray();
876 final double[] relErrorsV = errorsV.stream().mapToDouble(Double::doubleValue).toArray();
877
878 final double errorsPMedian = new Median().evaluate(relErrorsP);
879 final double errorsPMean = new Mean().evaluate(relErrorsP);
880 final double errorsPMax = new Max().evaluate(relErrorsP);
881 final double errorsVMedian = new Median().evaluate(relErrorsV);
882 final double errorsVMean = new Mean().evaluate(relErrorsV);
883 final double errorsVMax = new Max().evaluate(relErrorsV);
884
885
886 if (printResults) {
887 System.out.println();
888 System.out.format(Locale.US, "Relative errors dΦ/dP -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
889 errorsPMedian, errorsPMean, errorsPMax);
890 System.out.format(Locale.US, "Relative errors dΦ/dV -> Median: %6.3e / Mean: %6.3e / Max: %6.3e%n",
891 errorsVMedian, errorsVMean, errorsVMax);
892 }
893
894 Assertions.assertEquals(0.0, errorsPMedian, refErrorsPMedian);
895 Assertions.assertEquals(0.0, errorsPMean, refErrorsPMean);
896 Assertions.assertEquals(0.0, errorsPMax, refErrorsPMax);
897 Assertions.assertEquals(0.0, errorsVMedian, refErrorsVMedian);
898 Assertions.assertEquals(0.0, errorsVMean, refErrorsVMean);
899 Assertions.assertEquals(0.0, errorsVMax, refErrorsVMax);
900 }
901
902 @Test
903 public void testIssue734() {
904
905 Utils.setDataRoot("regular-data");
906
907
908 final OneAxisEllipsoid body = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING,
909 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
910 final TopocentricFrame topo = new TopocentricFrame(body,
911 new GeodeticPoint(FastMath.toRadians(51.8), FastMath.toRadians(102.2), 811.2),
912 "BADG");
913 final GroundStation station = new GroundStation(topo);
914
915
916 final Phase phase = new Phase(station, AbsoluteDate.J2000_EPOCH, 119866527.060,
917 PredefinedGnssSignal.G01.getWavelength(), 0.02, 1.0, new ObservableSatellite(0),
918 new AmbiguityCache());
919
920
921 Assertions.assertEquals(0.0, phase.getAmbiguityDriver().getValue(), Double.MIN_VALUE);
922 Assertions.assertFalse(phase.getAmbiguityDriver().isSelected());
923
924
925 phase.getAmbiguityDriver().setValue(1234.0);
926 phase.getAmbiguityDriver().setSelected(true);
927
928
929 Assertions.assertEquals(1234.0, phase.getAmbiguityDriver().getValue(), Double.MIN_VALUE);
930 Assertions.assertTrue(phase.getAmbiguityDriver().isSelected());
931 for (ParameterDriver driver : phase.getParametersDrivers()) {
932
933 if (driver instanceof AmbiguityDriver) {
934 Assertions.assertEquals(1234.0, phase.getAmbiguityDriver().getValue(), Double.MIN_VALUE);
935 Assertions.assertTrue(phase.getAmbiguityDriver().isSelected());
936 }
937 }
938
939 }
940
941 }