1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.sequential;
18
19 import java.util.List;
20
21 import org.hipparchus.exception.LocalizedCoreFormats;
22 import org.hipparchus.exception.MathRuntimeException;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.QRDecomposer;
25 import org.hipparchus.linear.RealMatrix;
26 import org.hipparchus.stat.descriptive.StreamingStatistics;
27 import org.hipparchus.util.FastMath;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.Test;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.errors.OrekitMessages;
32 import org.orekit.estimation.DSSTContext;
33 import org.orekit.estimation.DSSTEstimationTestUtils;
34 import org.orekit.estimation.measurements.EstimatedMeasurement;
35 import org.orekit.estimation.measurements.GroundStation;
36 import org.orekit.estimation.measurements.ObservedMeasurement;
37 import org.orekit.estimation.measurements.Range;
38 import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
39 import org.orekit.estimation.measurements.modifiers.DynamicOutlierFilter;
40 import org.orekit.forces.gravity.potential.GravityFieldFactory;
41 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
42 import org.orekit.orbits.Orbit;
43 import org.orekit.orbits.OrbitType;
44 import org.orekit.orbits.PositionAngleType;
45 import org.orekit.propagation.PropagationType;
46 import org.orekit.propagation.Propagator;
47 import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
48 import org.orekit.propagation.semianalytical.dsst.DSSTPropagator;
49 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
50 import org.orekit.propagation.semianalytical.dsst.forces.DSSTTesseral;
51 import org.orekit.propagation.semianalytical.dsst.forces.DSSTZonal;
52 import org.orekit.time.AbsoluteDate;
53 import org.orekit.utils.Constants;
54 import org.orekit.utils.ParameterDriver;
55 import org.orekit.utils.ParameterDriversList;
56
57 public class SemiAnalyticalKalmanEstimatorTest {
58
59 @Test
60 public void testMissingPropagatorBuilder() {
61 try {
62 new SemiAnalyticalKalmanEstimatorBuilder().build();
63 Assertions.fail("an exception should have been thrown");
64 } catch (OrekitException oe) {
65 Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
66 }
67 }
68
69 @Test
70 public void testMathRuntimeException() {
71
72
73 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
74
75 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
76 final boolean perfectStart = true;
77 final double minStep = 120.0;
78 final double maxStep = 1200.0;
79 final double dP = 1.;
80
81
82 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
83
84
85 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
86 final List<ObservedMeasurement<?>> measurements =
87 DSSTEstimationTestUtils.createMeasurements(propagator,
88 new TwoWayRangeMeasurementCreator(context),
89 0.0, 6.0, 60.0);
90
91 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
92
93
94 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
95 0., 0., 0., 0., 0., 0.
96 });
97
98
99 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
100 final double[][] dYdC = new double[6][6];
101 initialOrbit.getJacobianWrtCartesian(PositionAngleType.MEAN, dYdC);
102 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
103
104
105 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
106
107
108 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
109
110
111 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
112 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
113 decomposer(new QRDecomposer(1.0e-15)).
114 build();
115 kalman.setObserver(estimation -> {
116 throw new MathRuntimeException(LocalizedCoreFormats.INTERNAL_ERROR, "me");
117 });
118
119 try {
120 kalman.processMeasurements(measurements);
121 Assertions.fail("an exception should have been thrown");
122 } catch (OrekitException oe) {
123 Assertions.assertEquals(LocalizedCoreFormats.INTERNAL_ERROR, oe.getSpecifier());
124 }
125 }
126
127
128
129
130
131
132
133
134 @Test
135 public void testKeplerianRange() {
136
137
138 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
139
140
141 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
142 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
143 final boolean perfectStart = true;
144 final double minStep = 120.0;
145 final double maxStep = 1200.0;
146 final double dP = 1.;
147
148
149 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
150
151
152 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
153 final List<ObservedMeasurement<?>> measurements =
154 DSSTEstimationTestUtils.createMeasurements(propagator,
155 new TwoWayRangeMeasurementCreator(context),
156 0.0, 6.0, 60.0);
157 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
158
159
160 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
161
162
163 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
164
165
166 final Orbit refOrbit = referencePropagator.
167 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
168
169
170 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
171 0., 0., 0., 0., 0., 0.
172 });
173
174
175 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
176 final double[][] dYdC = new double[6][6];
177 initialOrbit.getJacobianWrtCartesian(PositionAngleType.MEAN, dYdC);
178 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
179
180
181 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
182
183
184 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
185
186
187 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
188 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
189 build();
190 final Observer observer = new Observer();
191 kalman.setObserver(observer);
192
193
194 final double expectedDeltaPos = 0.;
195 final double posEps = 1.0e-15;
196 final double expectedDeltaVel = 0.;
197 final double velEps = 1.0e-15;
198 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
199 refOrbit, positionAngleType,
200 expectedDeltaPos, posEps,
201 expectedDeltaVel, velEps);
202
203 Assertions.assertEquals(0.0, observer.getMeanResidual(), 6e-8);
204 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
205 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
206 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
207 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
208 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
209 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
210 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
211 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
212 }
213
214
215
216
217
218
219
220
221 @Test
222 public void testRangeWithZonal() {
223
224
225 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
226
227
228 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
229 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
230 final boolean perfectStart = true;
231 final double minStep = 120.0;
232 final double maxStep = 1200.0;
233 final double dP = 1.;
234
235
236 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
237 builder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
238
239
240 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
241 final List<ObservedMeasurement<?>> measurements =
242 DSSTEstimationTestUtils.createMeasurements(propagator,
243 new TwoWayRangeMeasurementCreator(context),
244 0.0, 6.0, 60.0);
245 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
246
247
248 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
249 propagatorBuilder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
250
251
252 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
253
254
255 final Orbit refOrbit = referencePropagator.
256 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
257
258 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
259 aDriver.setValue(aDriver.getValue() + 1.2);
260
261
262
263 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
264 100., 100., 100., 1e-2, 1e-2, 1e-2
265 });
266
267
268 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
269 final double[][] dYdC = new double[6][6];
270 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
271 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
272
273
274 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
275
276
277 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
278
279
280 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
281 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
282 build();
283 final Observer observer = new Observer();
284 kalman.setObserver(observer);
285
286
287 final double expectedDeltaPos = 0.;
288 final double posEps = 6.2e-2;
289 final double expectedDeltaVel = 0.;
290 final double velEps = 2.0e-5;
291 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
292 refOrbit, positionAngleType,
293 expectedDeltaPos, posEps,
294 expectedDeltaVel, velEps);
295
296 Assertions.assertEquals(0.0, observer.getMeanResidual(), 8.51e-3);
297 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
298 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
299 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
300 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
301 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
302 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
303 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
304 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
305 }
306
307
308
309
310
311
312
313
314
315 @Test
316 public void testRangeWithTesseral() {
317
318
319 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
320
321
322 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
323 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
324 final boolean perfectStart = true;
325 final double minStep = 120.0;
326 final double maxStep = 1200.0;
327 final double dP = 1.;
328
329
330 final UnnormalizedSphericalHarmonicsProvider gravityField = GravityFieldFactory.getUnnormalizedProvider(2, 2);
331 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
332 builder.addForceModel(new DSSTZonal(gravityField));
333 builder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
334 gravityField.getMaxDegree(),
335 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
336 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
337
338
339 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
340 final List<ObservedMeasurement<?>> measurements =
341 DSSTEstimationTestUtils.createMeasurements(propagator,
342 new TwoWayRangeMeasurementCreator(context),
343 0.0, 6.0, 60.0);
344 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
345
346
347 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
348 propagatorBuilder.addForceModel(new DSSTZonal(gravityField));
349 propagatorBuilder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
350 gravityField.getMaxDegree(),
351 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
352 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
353
354
355 final DSSTPropagator referencePropagator = propagatorBuilder.
356 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
357
358
359 final Orbit refOrbit = referencePropagator.
360 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
361
362 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
363 aDriver.setValue(aDriver.getValue() + 1.2);
364
365
366
367 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
368 100., 100., 100., 1e-2, 1e-2, 1e-2
369 });
370
371
372 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
373 final double[][] dYdC = new double[6][6];
374 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
375 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
376
377
378 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
379
380
381 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
382
383
384 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
385 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
386 build();
387 final Observer observer = new Observer();
388 kalman.setObserver(observer);
389
390
391 final double expectedDeltaPos = 0.;
392 final double posEps = 7.7e-2;
393 final double expectedDeltaVel = 0.;
394 final double velEps = 2.5e-5;
395 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
396 refOrbit, positionAngleType,
397 expectedDeltaPos, posEps,
398 expectedDeltaVel, velEps);
399
400 Assertions.assertEquals(0.0, observer.getMeanResidual(), 8.81e-3);
401 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
402 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
403 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
404 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
405 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
406 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
407 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
408 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
409 }
410
411
412 public static class Observer implements KalmanObserver {
413
414
415 private final StreamingStatistics stats;
416
417
418 public Observer() {
419 this.stats = new StreamingStatistics();
420 }
421
422
423 @Override
424 public void evaluationPerformed(final KalmanEstimation estimation) {
425
426
427 final EstimatedMeasurement<?> estimatedMeasurement = estimation.getPredictedMeasurement();
428
429
430 if (estimatedMeasurement.getObservedMeasurement().getMeasurementType().equals(Range.MEASUREMENT_TYPE)) {
431 final double[] estimated = estimatedMeasurement.getEstimatedValue();
432 final double[] observed = estimatedMeasurement.getObservedValue();
433
434 final double res = observed[0] - estimated[0];
435 stats.addValue(res);
436 }
437
438 }
439
440
441
442
443 public double getMeanResidual() {
444 return stats.getMean();
445 }
446
447 }
448
449 @Test
450 public void testWithEstimatedPropagationParameters() {
451
452
453 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
454
455
456 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
457 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
458 final boolean perfectStart = true;
459 final double minStep = 120.0;
460 final double maxStep = 1200.0;
461 final double dP = 1.;
462
463
464 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
465 final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
466 zonal.getParametersDrivers().get(0).setSelected(true);
467 builder.addForceModel(zonal);
468
469
470 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
471 final List<ObservedMeasurement<?>> measurements =
472 DSSTEstimationTestUtils.createMeasurements(propagator,
473 new TwoWayRangeMeasurementCreator(context),
474 0.0, 6.0, 60.0);
475 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
476
477
478 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
479 propagatorBuilder.addForceModel(zonal);
480
481
482 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
483
484
485 final Orbit refOrbit = referencePropagator.
486 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
487
488
489
490 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
491 100., 100., 100., 1e-2, 1e-2, 1e-2
492 });
493
494
495 final RealMatrix propagationP = MatrixUtils.createRealDiagonalMatrix(new double [] {
496 1.0e-10
497 });
498
499
500 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
501 final double[][] dYdC = new double[6][6];
502 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
503 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
504 final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
505
506
507 final RealMatrix initialP = MatrixUtils.createRealMatrix(7, 7);
508 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
509 initialP.setSubMatrix(propagationP.getData(), 6, 6);
510
511
512 RealMatrix Q = MatrixUtils.createRealMatrix(7, 7);
513
514
515 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
516 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
517 build();
518 final Observer observer = new Observer();
519 kalman.setObserver(observer);
520
521
522 final double expectedDeltaPos = 0.;
523 final double posEps = 4.9e-2;
524 final double expectedDeltaVel = 0.;
525 final double velEps = 1.6e-5;
526 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
527 refOrbit, positionAngleType,
528 expectedDeltaPos, posEps,
529 expectedDeltaVel, velEps);
530
531 Assertions.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
532 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
533 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
534 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
535 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(true).getNbParams());
536 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
537 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
538 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
539 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
540 }
541
542 @Test
543 public void testWithEstimatedMeasurementParameters() {
544
545
546 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
547
548
549 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
550 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
551 final boolean perfectStart = true;
552 final double minStep = 120.0;
553 final double maxStep = 1200.0;
554 final double dP = 1.;
555
556
557 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
558 final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
559 builder.addForceModel(zonal);
560
561
562 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
563 final ParameterDriversList estimatedDrivers = new ParameterDriversList();
564 final double groundClockDrift = 4.8e-9;
565 for (final GroundStation station : context.stations) {
566 station.getClockOffsetDriver().setValue(groundClockDrift);
567 station.getClockOffsetDriver().setSelected(true);
568 estimatedDrivers.add(station.getClockOffsetDriver());
569 }
570 final List<ObservedMeasurement<?>> measurements =
571 DSSTEstimationTestUtils.createMeasurements(propagator,
572 new TwoWayRangeMeasurementCreator(context),
573 0.0, 6.0, 60.0);
574 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
575
576
577 final DynamicOutlierFilter<Range> filter = new DynamicOutlierFilter<>(10, 1.0);
578 for (ObservedMeasurement<?> measurement : measurements) {
579 Range range = (Range) measurement;
580 range.addModifier(filter);
581 }
582
583
584 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
585 propagatorBuilder.addForceModel(zonal);
586
587
588 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
589
590
591 final Orbit refOrbit = referencePropagator.
592 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
593
594
595
596 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
597 100., 100., 100., 1e-2, 1e-2, 1e-2
598 });
599
600
601 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
602 final double[][] dYdC = new double[6][6];
603 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
604 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
605 final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
606
607
608 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
609 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
610
611
612 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
613
614
615 final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double [] {
616 1.0e-15, 1.0e-15
617 });
618 final RealMatrix measurementQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
619 1.0e-25, 1.0e-25
620 });
621
622
623 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
624 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
625 estimatedMeasurementsParameters(estimatedDrivers, new ConstantProcessNoise(measurementP, measurementQ)).
626 build();
627 final Observer observer = new Observer();
628 kalman.setObserver(observer);
629
630
631 final double expectedDeltaPos = 0.;
632 final double posEps = 4.9e-2;
633 final double expectedDeltaVel = 0.;
634 final double velEps = 1.6e-5;
635 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
636 refOrbit, positionAngleType,
637 expectedDeltaPos, posEps,
638 expectedDeltaVel, velEps);
639
640 Assertions.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
641 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
642 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
643 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
644 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
645 Assertions.assertEquals(2, kalman.getEstimatedMeasurementsParameters().getNbParams());
646 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
647 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
648 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
649 }
650
651 }