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.Assert;
29 import org.junit.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.RangeMeasurementCreator;
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.PositionAngle;
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 Assert.fail("an exception should have been thrown");
64 } catch (OrekitException oe) {
65 Assert.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 RangeMeasurementCreator(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(PositionAngle.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 Assert.fail("an exception should have been thrown");
122 } catch (OrekitException oe) {
123 Assert.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 PositionAngle positionAngle = PositionAngle.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 RangeMeasurementCreator(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 DSSTPropagator referencePropagator = propagatorBuilder.
164 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
165
166
167 final Orbit refOrbit = referencePropagator.
168 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
169
170
171 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
172 0., 0., 0., 0., 0., 0.
173 });
174
175
176 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
177 final double[][] dYdC = new double[6][6];
178 initialOrbit.getJacobianWrtCartesian(PositionAngle.MEAN, dYdC);
179 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
180
181
182 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
183
184
185 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
186
187
188 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
189 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
190 build();
191 final Observer observer = new Observer();
192 kalman.setObserver(observer);
193
194
195 final double expectedDeltaPos = 0.;
196 final double posEps = 1.0e-15;
197 final double expectedDeltaVel = 0.;
198 final double velEps = 1.0e-15;
199 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
200 refOrbit, positionAngle,
201 expectedDeltaPos, posEps,
202 expectedDeltaVel, velEps);
203
204 Assert.assertEquals(0.0, observer.getMeanResidual(), 4.98e-8);
205 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
206 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
207 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
208 Assert.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
209 Assert.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
210 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
211 Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
212 Assert.assertNotNull(kalman.getPhysicalEstimatedState());
213 }
214
215
216
217
218
219
220
221
222 @Test
223 public void testRangeWithZonal() {
224
225
226 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
227
228
229 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
230 final PositionAngle positionAngle = PositionAngle.MEAN;
231 final boolean perfectStart = true;
232 final double minStep = 120.0;
233 final double maxStep = 1200.0;
234 final double dP = 1.;
235
236
237 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
238 builder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
239
240
241 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
242 final List<ObservedMeasurement<?>> measurements =
243 DSSTEstimationTestUtils.createMeasurements(propagator,
244 new RangeMeasurementCreator(context),
245 0.0, 6.0, 60.0);
246 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
247
248
249 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
250 propagatorBuilder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
251
252
253 final DSSTPropagator referencePropagator = propagatorBuilder.
254 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
255
256
257 final Orbit refOrbit = referencePropagator.
258 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
259
260 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
261 aDriver.setValue(aDriver.getValue() + 1.2);
262
263
264
265 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
266 100., 100., 100., 1e-2, 1e-2, 1e-2
267 });
268
269
270 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
271 final double[][] dYdC = new double[6][6];
272 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
273 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
274
275
276 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
277
278
279 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
280
281
282 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
283 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
284 build();
285 final Observer observer = new Observer();
286 kalman.setObserver(observer);
287
288
289 final double expectedDeltaPos = 0.;
290 final double posEps = 6.2e-2;
291 final double expectedDeltaVel = 0.;
292 final double velEps = 2.0e-5;
293 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
294 refOrbit, positionAngle,
295 expectedDeltaPos, posEps,
296 expectedDeltaVel, velEps);
297
298 Assert.assertEquals(0.0, observer.getMeanResidual(), 8.51e-3);
299 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
300 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
301 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
302 Assert.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
303 Assert.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
304 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
305 Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
306 Assert.assertNotNull(kalman.getPhysicalEstimatedState());
307 }
308
309
310
311
312
313
314
315
316
317 @Test
318 public void testRangeWithTesseral() {
319
320
321 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
322
323
324 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
325 final PositionAngle positionAngle = PositionAngle.MEAN;
326 final boolean perfectStart = true;
327 final double minStep = 120.0;
328 final double maxStep = 1200.0;
329 final double dP = 1.;
330
331
332 final UnnormalizedSphericalHarmonicsProvider gravityField = GravityFieldFactory.getUnnormalizedProvider(2, 2);
333 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
334 builder.addForceModel(new DSSTZonal(gravityField));
335 builder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
336 gravityField.getMaxDegree(),
337 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
338 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
339
340
341 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
342 final List<ObservedMeasurement<?>> measurements =
343 DSSTEstimationTestUtils.createMeasurements(propagator,
344 new RangeMeasurementCreator(context),
345 0.0, 6.0, 60.0);
346 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
347
348
349 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
350 propagatorBuilder.addForceModel(new DSSTZonal(gravityField));
351 propagatorBuilder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
352 gravityField.getMaxDegree(),
353 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
354 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
355
356
357 final DSSTPropagator referencePropagator = propagatorBuilder.
358 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
359
360
361 final Orbit refOrbit = referencePropagator.
362 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
363
364 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
365 aDriver.setValue(aDriver.getValue() + 1.2);
366
367
368
369 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
370 100., 100., 100., 1e-2, 1e-2, 1e-2
371 });
372
373
374 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
375 final double[][] dYdC = new double[6][6];
376 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
377 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
378
379
380 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
381
382
383 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
384
385
386 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
387 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
388 build();
389 final Observer observer = new Observer();
390 kalman.setObserver(observer);
391
392
393 final double expectedDeltaPos = 0.;
394 final double posEps = 7.7e-2;
395 final double expectedDeltaVel = 0.;
396 final double velEps = 2.5e-5;
397 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
398 refOrbit, positionAngle,
399 expectedDeltaPos, posEps,
400 expectedDeltaVel, velEps);
401
402 Assert.assertEquals(0.0, observer.getMeanResidual(), 8.81e-3);
403 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
404 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
405 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
406 Assert.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
407 Assert.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
408 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
409 Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
410 Assert.assertNotNull(kalman.getPhysicalEstimatedState());
411 }
412
413
414 public static class Observer implements KalmanObserver {
415
416
417 private StreamingStatistics stats;
418
419
420 public Observer() {
421 this.stats = new StreamingStatistics();
422 }
423
424
425 @Override
426 public void evaluationPerformed(final KalmanEstimation estimation) {
427
428
429 final EstimatedMeasurement<?> estimatedMeasurement = estimation.getPredictedMeasurement();
430
431
432 if (estimatedMeasurement.getObservedMeasurement() instanceof Range) {
433 final double[] estimated = estimatedMeasurement.getEstimatedValue();
434 final double[] observed = estimatedMeasurement.getObservedValue();
435
436 final double res = observed[0] - estimated[0];
437 stats.addValue(res);
438 }
439
440 }
441
442
443
444
445 public double getMeanResidual() {
446 return stats.getMean();
447 }
448
449 }
450
451 @Test
452 public void testWithEstimatedPropagationParameters() {
453
454
455 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
456
457
458 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
459 final PositionAngle positionAngle = PositionAngle.MEAN;
460 final boolean perfectStart = true;
461 final double minStep = 120.0;
462 final double maxStep = 1200.0;
463 final double dP = 1.;
464
465
466 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
467 final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
468 zonal.getParametersDrivers().get(0).setSelected(true);
469 builder.addForceModel(zonal);
470
471
472 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
473 final List<ObservedMeasurement<?>> measurements =
474 DSSTEstimationTestUtils.createMeasurements(propagator,
475 new RangeMeasurementCreator(context),
476 0.0, 6.0, 60.0);
477 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
478
479
480 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
481 propagatorBuilder.addForceModel(zonal);
482
483
484 final DSSTPropagator referencePropagator = propagatorBuilder.
485 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
486
487
488 final Orbit refOrbit = referencePropagator.
489 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
490
491
492
493 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
494 100., 100., 100., 1e-2, 1e-2, 1e-2
495 });
496
497
498 final RealMatrix propagationP = MatrixUtils.createRealDiagonalMatrix(new double [] {
499 1.0e-10
500 });
501
502
503 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
504 final double[][] dYdC = new double[6][6];
505 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
506 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
507 final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
508
509
510 final RealMatrix initialP = MatrixUtils.createRealMatrix(7, 7);
511 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
512 initialP.setSubMatrix(propagationP.getData(), 6, 6);
513
514
515 RealMatrix Q = MatrixUtils.createRealMatrix(7, 7);
516
517
518 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
519 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
520 build();
521 final Observer observer = new Observer();
522 kalman.setObserver(observer);
523
524
525 final double expectedDeltaPos = 0.;
526 final double posEps = 4.9e-2;
527 final double expectedDeltaVel = 0.;
528 final double velEps = 1.6e-5;
529 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
530 refOrbit, positionAngle,
531 expectedDeltaPos, posEps,
532 expectedDeltaVel, velEps);
533
534 Assert.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
535 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
536 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
537 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
538 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(true).getNbParams());
539 Assert.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
540 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
541 Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
542 Assert.assertNotNull(kalman.getPhysicalEstimatedState());
543 }
544
545 @Test
546 public void testWithEstimatedMeasurementParameters() {
547
548
549 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
550
551
552 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
553 final PositionAngle positionAngle = PositionAngle.MEAN;
554 final boolean perfectStart = true;
555 final double minStep = 120.0;
556 final double maxStep = 1200.0;
557 final double dP = 1.;
558
559
560 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
561 final DSSTForceModel zonal = new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0));
562 builder.addForceModel(zonal);
563
564
565 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
566 final ParameterDriversList estimatedDrivers = new ParameterDriversList();
567 final double groundClockDrift = 4.8e-9;
568 for (final GroundStation station : context.stations) {
569 station.getClockOffsetDriver().setValue(groundClockDrift);
570 station.getClockOffsetDriver().setSelected(true);
571 estimatedDrivers.add(station.getClockOffsetDriver());
572 }
573 final List<ObservedMeasurement<?>> measurements =
574 DSSTEstimationTestUtils.createMeasurements(propagator,
575 new RangeMeasurementCreator(context),
576 0.0, 6.0, 60.0);
577 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
578
579
580 final DynamicOutlierFilter<Range> filter = new DynamicOutlierFilter<>(10, 1.0);
581 for (ObservedMeasurement<?> measurement : measurements) {
582 Range range = (Range) measurement;
583 range.addModifier(filter);
584 }
585
586
587 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
588 propagatorBuilder.addForceModel(zonal);
589
590
591 final DSSTPropagator referencePropagator = propagatorBuilder.
592 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
593
594
595 final Orbit refOrbit = referencePropagator.
596 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
597
598
599
600 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
601 100., 100., 100., 1e-2, 1e-2, 1e-2
602 });
603
604
605 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
606 final double[][] dYdC = new double[6][6];
607 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
608 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
609 final RealMatrix orbitalP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
610
611
612 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
613 initialP.setSubMatrix(orbitalP.getData(), 0, 0);
614
615
616 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
617
618
619 final RealMatrix measurementP = MatrixUtils.createRealDiagonalMatrix(new double [] {
620 1.0e-15, 1.0e-15
621 });
622 final RealMatrix measurementQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
623 1.0e-25, 1.0e-25
624 });
625
626
627 final SemiAnalyticalKalmanEstimator kalman = new SemiAnalyticalKalmanEstimatorBuilder().
628 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
629 estimatedMeasurementsParameters(estimatedDrivers, new ConstantProcessNoise(measurementP, measurementQ)).
630 build();
631 final Observer observer = new Observer();
632 kalman.setObserver(observer);
633
634
635 final double expectedDeltaPos = 0.;
636 final double posEps = 4.9e-2;
637 final double expectedDeltaVel = 0.;
638 final double velEps = 1.6e-5;
639 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
640 refOrbit, positionAngle,
641 expectedDeltaPos, posEps,
642 expectedDeltaVel, velEps);
643
644 Assert.assertEquals(0.0, observer.getMeanResidual(), 1.79e-3);
645 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
646 Assert.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
647 Assert.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
648 Assert.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
649 Assert.assertEquals(2, kalman.getEstimatedMeasurementsParameters().getNbParams());
650 Assert.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
651 Assert.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
652 Assert.assertNotNull(kalman.getPhysicalEstimatedState());
653 }
654
655 }