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.lang.reflect.InvocationTargetException;
20 import java.lang.reflect.Method;
21 import java.util.ArrayList;
22 import java.util.Comparator;
23 import java.util.List;
24
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.linear.RealMatrix;
28 import org.junit.Assert;
29 import org.junit.Test;
30 import org.orekit.attitudes.LofOffset;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.estimation.Context;
34 import org.orekit.estimation.EstimationTestUtils;
35 import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
36 import org.orekit.estimation.measurements.AngularRaDecMeasurementCreator;
37 import org.orekit.estimation.measurements.InterSatellitesRangeMeasurementCreator;
38 import org.orekit.estimation.measurements.MultiplexedMeasurement;
39 import org.orekit.estimation.measurements.ObservableSatellite;
40 import org.orekit.estimation.measurements.ObservedMeasurement;
41 import org.orekit.estimation.measurements.PVMeasurementCreator;
42 import org.orekit.estimation.measurements.Position;
43 import org.orekit.estimation.measurements.PositionMeasurementCreator;
44 import org.orekit.estimation.measurements.Range;
45 import org.orekit.estimation.measurements.RangeMeasurementCreator;
46 import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
47 import org.orekit.estimation.measurements.modifiers.OnBoardAntennaRangeModifier;
48 import org.orekit.frames.LOFType;
49 import org.orekit.orbits.CartesianOrbit;
50 import org.orekit.orbits.KeplerianOrbit;
51 import org.orekit.orbits.Orbit;
52 import org.orekit.orbits.OrbitType;
53 import org.orekit.orbits.PositionAngle;
54 import org.orekit.propagation.BoundedPropagator;
55 import org.orekit.propagation.EphemerisGenerator;
56 import org.orekit.propagation.Propagator;
57 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
58 import org.orekit.propagation.numerical.NumericalPropagator;
59 import org.orekit.time.AbsoluteDate;
60 import org.orekit.utils.ParameterDriver;
61 import org.orekit.utils.ParameterDriversList;
62 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
63 import org.orekit.utils.TimeStampedPVCoordinates;
64
65 public class KalmanEstimatorTest {
66
67 @Test
68 public void testMissingPropagatorBuilder() {
69 try {
70 new KalmanEstimatorBuilder().
71 build();
72 Assert.fail("an exception should have been thrown");
73 } catch (OrekitException oe) {
74 Assert.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
75 }
76 }
77
78
79
80
81
82 @Test
83 public void testKeplerianPV() {
84
85
86 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
87
88
89 final OrbitType orbitType = OrbitType.KEPLERIAN;
90 final PositionAngle positionAngle = PositionAngle.TRUE;
91 final boolean perfectStart = true;
92 final double minStep = 1.e-6;
93 final double maxStep = 60.;
94 final double dP = 1.;
95 final NumericalPropagatorBuilder propagatorBuilder =
96 context.createBuilder(orbitType, positionAngle, perfectStart,
97 minStep, maxStep, dP);
98
99
100 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
101 propagatorBuilder);
102 final List<ObservedMeasurement<?>> measurements =
103 EstimationTestUtils.createMeasurements(propagator,
104 new PVMeasurementCreator(),
105 0.0, 3.0, 300.0);
106
107 final NumericalPropagator referencePropagator = propagatorBuilder.
108 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
109
110
111 final Orbit refOrbit = referencePropagator.
112 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
113
114
115 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
116 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
117 });
118
119
120 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
121 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
122 });
123
124
125
126 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
127 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
128 build();
129
130
131 final double expectedDeltaPos = 0.;
132 final double posEps = 5.80e-8;
133 final double expectedDeltaVel = 0.;
134 final double velEps = 2.28e-11;
135 final double[] expectedsigmasPos = {0.998872, 0.933655, 0.997516};
136 final double sigmaPosEps = 1e-6;
137 final double[] expectedSigmasVel = {9.478853e-4, 9.910788e-4, 5.0438709e-4};
138 final double sigmaVelEps = 1e-10;
139 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
140 refOrbit, positionAngle,
141 expectedDeltaPos, posEps,
142 expectedDeltaVel, velEps,
143 expectedsigmasPos, sigmaPosEps,
144 expectedSigmasVel, sigmaVelEps);
145 }
146
147
148
149
150
151 @Test
152 public void testKeplerianRange() {
153
154
155 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
156
157
158 final OrbitType orbitType = OrbitType.KEPLERIAN;
159 final PositionAngle positionAngle = PositionAngle.TRUE;
160 final boolean perfectStart = true;
161 final double minStep = 1.e-6;
162 final double maxStep = 60.;
163 final double dP = 1.;
164 final NumericalPropagatorBuilder propagatorBuilder =
165 context.createBuilder(orbitType, positionAngle, perfectStart,
166 minStep, maxStep, dP);
167
168
169 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
170 propagatorBuilder);
171 final List<ObservedMeasurement<?>> measurements =
172 EstimationTestUtils.createMeasurements(propagator,
173 new RangeMeasurementCreator(context),
174 1.0, 4.0, 60.0);
175
176
177 final NumericalPropagator referencePropagator = propagatorBuilder.
178 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
179
180
181 final Orbit refOrbit = referencePropagator.
182 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
183
184
185 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
186 aDriver.setValue(aDriver.getValue() + 1.2);
187 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
188
189
190
191 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
192 100., 100., 100., 1e-2, 1e-2, 1e-2
193 });
194
195
196 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
197 final double[][] dYdC = new double[6][6];
198 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
199 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
200
201
202 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
203
204
205 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
206
207
208 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
209 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
210 build();
211
212
213 final double expectedDeltaPos = 0.;
214 final double posEps = 1.77e-4;
215 final double expectedDeltaVel = 0.;
216 final double velEps = 7.93e-8;
217 final double[] expectedSigmasPos = {0.742488, 0.281914, 0.563213};
218 final double sigmaPosEps = 1e-6;
219 final double[] expectedSigmasVel = {2.206636e-4, 1.306656e-4, 1.293981e-4};
220 final double sigmaVelEps = 1e-10;
221 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
222 refOrbit, positionAngle,
223 expectedDeltaPos, posEps,
224 expectedDeltaVel, velEps,
225 expectedSigmasPos, sigmaPosEps,
226 expectedSigmasVel, sigmaVelEps);
227 }
228
229
230
231
232
233 @Test
234 public void testKeplerianRangeWithOnBoardAntennaOffset() {
235
236
237 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
238
239
240 final OrbitType orbitType = OrbitType.KEPLERIAN;
241 final PositionAngle positionAngle = PositionAngle.TRUE;
242 final boolean perfectStart = true;
243 final double minStep = 1.e-6;
244 final double maxStep = 60.;
245 final double dP = 1.;
246 final NumericalPropagatorBuilder propagatorBuilder =
247 context.createBuilder(orbitType, positionAngle, perfectStart,
248 minStep, maxStep, dP);
249 propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
250
251
252 final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
253
254
255 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
256 propagatorBuilder);
257 final List<ObservedMeasurement<?>> measurements =
258 EstimationTestUtils.createMeasurements(propagator,
259 new RangeMeasurementCreator(context, antennaPhaseCenter),
260 1.0, 3.0, 300.0);
261
262
263 final OnBoardAntennaRangeModifier obaModifier = new OnBoardAntennaRangeModifier(antennaPhaseCenter);
264 for (final ObservedMeasurement<?> range : measurements) {
265 ((Range) range).addModifier(obaModifier);
266 }
267
268
269 final NumericalPropagator referencePropagator = propagatorBuilder.
270 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
271
272
273 final Orbit refOrbit = referencePropagator.
274 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
275
276
277 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
278 aDriver.setValue(aDriver.getValue() + 1.2);
279 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
280
281
282
283 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
284 10., 10., 10., 1e-3, 1e-3, 1e-3
285 });
286
287
288 final Orbit initialOrbit = OrbitType.KEPLERIAN.convertType(context.initialOrbit);
289 final double[][] dYdC = new double[6][6];
290 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
291 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
292
293
294 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
295
296
297 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
298
299
300 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
301 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
302 build();
303
304
305 final double expectedDeltaPos = 0.;
306 final double posEps = 4.57e-3;
307 final double expectedDeltaVel = 0.;
308 final double velEps = 7.29e-6;
309 final double[] expectedSigmasPos = {1.105198, 0.930797, 1.254581};
310 final double sigmaPosEps = 1e-6;
311 final double[] expectedSigmasVel = {6.193757e-4, 4.088798e-4, 3.299140e-4};
312 final double sigmaVelEps = 1e-10;
313 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
314 refOrbit, positionAngle,
315 expectedDeltaPos, posEps,
316 expectedDeltaVel, velEps,
317 expectedSigmasPos, sigmaPosEps,
318 expectedSigmasVel, sigmaVelEps);
319 }
320
321
322
323
324
325 @Test
326 public void testCartesianRangeRate() {
327
328
329 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
330
331
332 final OrbitType orbitType = OrbitType.CARTESIAN;
333 final PositionAngle positionAngle = PositionAngle.TRUE;
334 final boolean perfectStart = true;
335 final double minStep = 1.e-6;
336 final double maxStep = 60.;
337 final double dP = 1.;
338 final NumericalPropagatorBuilder propagatorBuilder =
339 context.createBuilder(orbitType, positionAngle, perfectStart,
340 minStep, maxStep, dP);
341
342
343 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
344 propagatorBuilder);
345 final double satClkDrift = 3.2e-10;
346 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
347 final List<ObservedMeasurement<?>> measurements =
348 EstimationTestUtils.createMeasurements(propagator,
349 creator,
350 1.0, 3.0, 300.0);
351
352
353 final NumericalPropagator referencePropagator = propagatorBuilder.
354 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
355
356
357 final Orbit refOrbit = referencePropagator.
358 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
359
360
361
362 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
363 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
364 });
365
366
367 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
368 final double[][] dYdC = new double[6][6];
369 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
370 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
371
372
373 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
374
375
376 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
377 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
378 });
379 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
380
381
382 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
383 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
384 build();
385
386
387 final double expectedDeltaPos = 0.;
388 final double posEps = 1.5e-6;
389 final double expectedDeltaVel = 0.;
390 final double velEps = 5.1e-10;
391 final double[] expectedSigmasPos = {0.324407, 1.347014, 1.743326};
392 final double sigmaPosEps = 1e-6;
393 final double[] expectedSigmasVel = {2.85688e-4, 5.765933e-4, 5.056124e-4};
394 final double sigmaVelEps = 1e-10;
395 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
396 refOrbit, positionAngle,
397 expectedDeltaPos, posEps,
398 expectedDeltaVel, velEps,
399 expectedSigmasPos, sigmaPosEps,
400 expectedSigmasVel, sigmaVelEps);
401 }
402
403
404
405
406
407 @Test
408 public void testCircularAzimuthElevation() {
409
410
411 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
412
413
414 final OrbitType orbitType = OrbitType.CIRCULAR;
415 final PositionAngle positionAngle = PositionAngle.TRUE;
416 final boolean perfectStart = true;
417 final double minStep = 1.e-6;
418 final double maxStep = 60.;
419 final double dP = 1.;
420 final NumericalPropagatorBuilder propagatorBuilder =
421 context.createBuilder(orbitType, positionAngle, perfectStart,
422 minStep, maxStep, dP);
423
424
425 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
426 propagatorBuilder);
427 final List<ObservedMeasurement<?>> measurements =
428 EstimationTestUtils.createMeasurements(propagator,
429 new AngularAzElMeasurementCreator(context),
430 1.0, 4.0, 60.0);
431
432
433 final NumericalPropagator referencePropagator = propagatorBuilder.
434 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
435
436
437 final Orbit refOrbit = referencePropagator.
438 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
439
440
441 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
442 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
443 });
444
445
446 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
447 final double[][] dYdC = new double[6][6];
448 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
449 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
450
451
452 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
453
454
455 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
456 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
457 });
458 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
459
460
461 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
462 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
463 build();
464
465
466 final double expectedDeltaPos = 0.;
467 final double posEps = 4.78e-7;
468 final double expectedDeltaVel = 0.;
469 final double velEps = 1.54e-10;
470 final double[] expectedSigmasPos = {0.356902, 1.297507, 1.798551};
471 final double sigmaPosEps = 1e-6;
472 final double[] expectedSigmasVel = {2.468745e-4, 5.810027e-4, 3.887394e-4};
473 final double sigmaVelEps = 1e-10;
474 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
475 refOrbit, positionAngle,
476 expectedDeltaPos, posEps,
477 expectedDeltaVel, velEps,
478 expectedSigmasPos, sigmaPosEps,
479 expectedSigmasVel, sigmaVelEps);
480 }
481
482
483
484
485
486 @Test
487 public void testEquinoctialRightAscensionDeclination() {
488
489
490 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
491
492
493 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
494 final PositionAngle positionAngle = PositionAngle.TRUE;
495 final boolean perfectStart = true;
496 final double minStep = 1.e-6;
497 final double maxStep = 60.;
498 final double dP = 1.;
499 final NumericalPropagatorBuilder propagatorBuilder =
500 context.createBuilder(orbitType, positionAngle, perfectStart,
501 minStep, maxStep, dP);
502
503
504 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
505 propagatorBuilder);
506 final List<ObservedMeasurement<?>> measurements =
507 EstimationTestUtils.createMeasurements(propagator,
508 new AngularRaDecMeasurementCreator(context),
509 1.0, 4.0, 60.0);
510
511
512 final NumericalPropagator referencePropagator = propagatorBuilder.
513 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
514
515
516 final Orbit refOrbit = referencePropagator.
517 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
518
519
520 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
521 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
522 });
523
524
525 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
526 final double[][] dYdC = new double[6][6];
527 initialOrbit.getJacobianWrtCartesian(positionAngle, dYdC);
528 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
529
530
531 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
532
533
534 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
535 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
536 });
537 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
538
539
540 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
541 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
542 build();
543
544
545 final double expectedDeltaPos = 0.;
546 final double posEps = 4.8e-7;
547 final double expectedDeltaVel = 0.;
548 final double velEps = 1.6e-10;
549 final double[] expectedSigmasPos = {0.356902, 1.297508, 1.798552};
550 final double sigmaPosEps = 1e-6;
551 final double[] expectedSigmasVel = {2.468746e-4, 5.810028e-4, 3.887394e-4};
552 final double sigmaVelEps = 1e-10;
553 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
554 refOrbit, positionAngle,
555 expectedDeltaPos, posEps,
556 expectedDeltaVel, velEps,
557 expectedSigmasPos, sigmaPosEps,
558 expectedSigmasVel, sigmaVelEps);
559 }
560
561
562
563
564
565 @Test
566 public void testKeplerianRangeAzElAndRangeRate() {
567
568
569 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
570
571
572 final OrbitType orbitType = OrbitType.KEPLERIAN;
573 final PositionAngle positionAngle = PositionAngle.TRUE;
574 final boolean perfectStart = true;
575 final double minStep = 1.e-6;
576 final double maxStep = 60.;
577 final double dP = 1.;
578 final NumericalPropagatorBuilder measPropagatorBuilder =
579 context.createBuilder(orbitType, positionAngle, perfectStart,
580 minStep, maxStep, dP);
581
582
583 final Propagator rangePropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
584 measPropagatorBuilder);
585 final List<ObservedMeasurement<?>> rangeMeasurements =
586 EstimationTestUtils.createMeasurements(rangePropagator,
587 new RangeMeasurementCreator(context),
588 0.0, 4.0, 300.0);
589
590 final Propagator angularPropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
591 measPropagatorBuilder);
592 final List<ObservedMeasurement<?>> angularMeasurements =
593 EstimationTestUtils.createMeasurements(angularPropagator,
594 new AngularAzElMeasurementCreator(context),
595 0.0, 4.0, 500.0);
596
597 final Propagator rangeRatePropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
598 measPropagatorBuilder);
599 final List<ObservedMeasurement<?>> rangeRateMeasurements =
600 EstimationTestUtils.createMeasurements(rangeRatePropagator,
601 new RangeRateMeasurementCreator(context, false, 3.2e-10),
602 0.0, 4.0, 700.0);
603
604
605 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
606 measurements.addAll(rangeMeasurements);
607 measurements.addAll(angularMeasurements);
608 measurements.addAll(rangeRateMeasurements);
609 measurements.sort(Comparator.naturalOrder());
610
611
612 final NumericalPropagator referencePropagator = measPropagatorBuilder.
613 buildPropagator(measPropagatorBuilder.getSelectedNormalizedParameters());
614
615
616 final Orbit refOrbit = referencePropagator.
617 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
618
619
620 final NumericalPropagatorBuilder propagatorBuilder =
621 context.createBuilder(orbitType, positionAngle, false,
622 minStep, maxStep, dP);
623
624
625
626 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
627 1e6, 1e6, 1e6, 1e-4, 1e-4, 1e-4
628 });
629
630
631 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
632 final double[][] dYdC = new double[6][6];
633 initialOrbit.getJacobianWrtCartesian(positionAngle, dYdC);
634 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
635
636
637 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
638
639
640 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
641 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
642 });
643 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
644
645
646 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
647 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
648 build();
649
650
651 final double expectedDeltaPos = 0.;
652 final double posEps = 2.94e-2;
653 final double expectedDeltaVel = 0.;
654 final double velEps = 5.8e-6;
655 final double[] expectedSigmasPos = {1.747575, 0.666887, 1.696202};
656 final double sigmaPosEps = 1e-6;
657 final double[] expectedSigmasVel = {5.413689e-4, 4.088394e-4, 4.315366e-4};
658 final double sigmaVelEps = 1e-10;
659 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
660 refOrbit, positionAngle,
661 expectedDeltaPos, posEps,
662 expectedDeltaVel, velEps,
663 expectedSigmasPos, sigmaPosEps,
664 expectedSigmasVel, sigmaVelEps);
665 }
666
667
668
669
670 @Test
671 public void testKeplerianRangeAndRangeRate() {
672
673
674 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
675
676
677 final OrbitType orbitType = OrbitType.KEPLERIAN;
678 final PositionAngle positionAngle = PositionAngle.TRUE;
679 final boolean perfectStart = true;
680 final double minStep = 1.e-6;
681 final double maxStep = 60.;
682 final double dP = 1.;
683 final NumericalPropagatorBuilder propagatorBuilder =
684 context.createBuilder(orbitType, positionAngle, perfectStart,
685 minStep, maxStep, dP);
686
687
688 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
689 propagatorBuilder);
690 final List<ObservedMeasurement<?>> measurementsRange =
691 EstimationTestUtils.createMeasurements(propagator,
692 new RangeMeasurementCreator(context),
693 1.0, 3.0, 300.0);
694
695 final List<ObservedMeasurement<?>> measurementsRangeRate =
696 EstimationTestUtils.createMeasurements(propagator,
697 new RangeRateMeasurementCreator(context, false, 3.2e-10),
698 1.0, 3.0, 300.0);
699
700
701 final List<ObservedMeasurement<?>> measurements = new ArrayList<ObservedMeasurement<?>>();
702 measurements.addAll(measurementsRange);
703 measurements.addAll(measurementsRangeRate);
704
705
706 final NumericalPropagator referencePropagator = propagatorBuilder.
707 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
708
709
710 final Orbit refOrbit = referencePropagator.
711 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
712
713
714
715 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
716 1e-2, 1e-2, 1e-2, 1e-8, 1e-8, 1e-8
717 });
718
719
720 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
721 final double[][] dYdC = new double[6][6];
722 initialOrbit.getJacobianWrtCartesian(PositionAngle.TRUE, dYdC);
723 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
724
725
726 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
727
728
729 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
730 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
731 });
732 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
733
734
735 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
736 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
737 build();
738
739
740 final double expectedDeltaPos = 0.;
741 final double posEps = 1.2e-6;
742 final double expectedDeltaVel = 0.;
743 final double velEps = 4.2e-10;
744 final double[] expectedSigmasPos = {0.341528, 8.175341, 4.634528};
745 final double sigmaPosEps = 1e-6;
746 final double[] expectedSigmasVel = {1.167859e-3, 1.036492e-3, 2.834413e-3};
747 final double sigmaVelEps = 1e-9;
748 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
749 refOrbit, positionAngle,
750 expectedDeltaPos, posEps,
751 expectedDeltaVel, velEps,
752 expectedSigmasPos, sigmaPosEps,
753 expectedSigmasVel, sigmaVelEps);
754 }
755
756 @Test
757 public void testMultiSat() {
758
759 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
760
761 final NumericalPropagatorBuilder propagatorBuilder1 =
762 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
763 1.0e-6, 60.0, 1.0);
764 final NumericalPropagatorBuilder propagatorBuilder2 =
765 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
766 1.0e-6, 60.0, 1.0);
767 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
768
769
770 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
771 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
772 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
773 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
774 context.initialOrbit.getFrame(),
775 context.initialOrbit.getMu());
776 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
777 propagatorBuilder2);
778 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
779 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
780 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
781 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
782 propagatorBuilder1);
783 final double localClockOffset = 0.137e-6;
784 final double remoteClockOffset = 469.0e-6;
785 final List<ObservedMeasurement<?>> measurements =
786 EstimationTestUtils.createMeasurements(propagator1,
787 new InterSatellitesRangeMeasurementCreator(ephemeris,
788 localClockOffset,
789 remoteClockOffset),
790 1.0, 3.0, 300.0);
791
792
793 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
794 propagatorBuilder1);
795 measurements.addAll(EstimationTestUtils.createMeasurements(propagator1,
796 new RangeMeasurementCreator(context),
797 1.0, 3.0, 60.0));
798 measurements.sort(Comparator.naturalOrder());
799
800
801 final RealMatrix processNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
802 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
803 });
804 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
805 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
806 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
807 build();
808
809 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
810 ParameterDriver a0Driver = parameters.get(0);
811 Assert.assertEquals("a[0]", a0Driver.getName());
812 a0Driver.setValue(a0Driver.getValue() + 1.2);
813 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
814
815 ParameterDriver a1Driver = parameters.get(6);
816 Assert.assertEquals("a[1]", a1Driver.getName());
817 a1Driver.setValue(a1Driver.getValue() - 5.4);
818 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
819
820 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
821 parameters.get( 7).getValue(),
822 parameters.get( 8).getValue(),
823 parameters.get( 9).getValue(),
824 parameters.get(10).getValue(),
825 parameters.get(11).getValue(),
826 PositionAngle.TRUE,
827 closeOrbit.getFrame(),
828 closeOrbit.getDate(),
829 closeOrbit.getMu());
830 Assert.assertEquals(4.7246,
831 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
832 before.getPVCoordinates().getPosition()),
833 1.0e-3);
834 Assert.assertEquals(0.0010514,
835 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
836 before.getPVCoordinates().getVelocity()),
837 1.0e-6);
838
839 Orbit[] refOrbits = new Orbit[] {
840 propagatorBuilder1.
841 buildPropagator(propagatorBuilder1.getSelectedNormalizedParameters()).
842 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
843 propagatorBuilder2.
844 buildPropagator(propagatorBuilder2.getSelectedNormalizedParameters()).
845 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
846 };
847 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
848 refOrbits, new PositionAngle[] { PositionAngle.TRUE, PositionAngle.TRUE },
849 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
850 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
851 new double[][] {
852 { 6.9e5, 6.0e5, 12.5e5 },
853 { 6.8e5, 5.9e5, 12.7e5 }
854 }, new double[] { 1e4, 1e4 },
855 new double[][] {
856 { 5.0e2, 5.3e2, 1.4e2 },
857 { 5.0e2, 5.3e2, 1.4e2 }
858 }, new double[] { 1.0e1, 1.0e1 });
859
860
861
862 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
863 if (driver.getName().startsWith("a[")) {
864
865 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
866 } else {
867
868 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
869 }
870 }
871
872 }
873
874
875
876
877 @Test
878 public void testWrappedException() {
879
880
881 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
882
883
884 final OrbitType orbitType = OrbitType.KEPLERIAN;
885 final PositionAngle positionAngle = PositionAngle.TRUE;
886 final boolean perfectStart = true;
887 final double minStep = 1.e-6;
888 final double maxStep = 60.;
889 final double dP = 1.;
890 final NumericalPropagatorBuilder propagatorBuilder =
891 context.createBuilder(orbitType, positionAngle, perfectStart,
892 minStep, maxStep, dP);
893
894
895 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
896 propagatorBuilder);
897 final List<ObservedMeasurement<?>> measurements =
898 EstimationTestUtils.createMeasurements(propagator,
899 new RangeMeasurementCreator(context),
900 1.0, 3.0, 300.0);
901
902 final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
903 kalmanBuilder.addPropagationConfiguration(propagatorBuilder,
904 new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6)));
905 final KalmanEstimator kalman = kalmanBuilder.build();
906 kalman.setObserver(estimation -> {
907 throw new DummyException();
908 });
909
910
911 try {
912
913 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
914 context.initialOrbit, positionAngle,
915 0., 0.,
916 0., 0.,
917 new double[3], 0.,
918 new double[3], 0.);
919 } catch (DummyException de) {
920
921 }
922
923 }
924
925 @Test
926 public void testIssue695() {
927
928
929 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
930
931
932 final Position position = new Position(context.initialOrbit.getDate(),
933 new Vector3D(1.0, -1.0, 0.0),
934 0.5, 1.0, new ObservableSatellite(0));
935
936
937 final NumericalPropagatorBuilder propagatorBuilder =
938 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true, 1.e-6, 60., 1.);
939
940
941 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
942 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
943 });
944
945
946 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
947 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
948 });
949
950
951
952 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
953 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
954 build();
955
956
957 MeasurementDecorator decorated = null;
958
959
960 try {
961 Method decorate = KalmanEstimator.class.getDeclaredMethod("decorate",
962 ObservedMeasurement.class);
963 decorate.setAccessible(true);
964 decorated = (MeasurementDecorator) decorate.invoke(kalman, position);
965 } catch (NoSuchMethodException | IllegalAccessException |
966 IllegalArgumentException | InvocationTargetException e) {
967
968 }
969
970
971 Assert.assertEquals(0.0, decorated.getTime(), 1.0e-15);
972
973 final RealMatrix covariance = decorated.getCovariance();
974 for (int i = 0; i < covariance.getRowDimension(); i++) {
975 for (int j = 0; j < covariance.getColumnDimension(); j++) {
976 if (i == j) {
977 Assert.assertEquals(1.0, covariance.getEntry(i, j), 1.0e-15);
978 } else {
979 Assert.assertEquals(0.0, covariance.getEntry(i, j), 1.0e-15);
980 }
981 }
982 }
983
984 }
985
986 @Test
987 public void tesIssue696() {
988
989 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
990
991 final NumericalPropagatorBuilder propagatorBuilder1 =
992 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
993 1.0e-6, 60.0, 1.0);
994 final NumericalPropagatorBuilder propagatorBuilder2 =
995 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
996 1.0e-6, 60.0, 1.0);
997 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
998
999
1000 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
1001 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
1002 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
1003 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
1004 context.initialOrbit.getFrame(),
1005 context.initialOrbit.getMu());
1006 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
1007 propagatorBuilder2);
1008 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
1009 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
1010 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
1011 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1012 propagatorBuilder1);
1013 final double localClockOffset = 0.137e-6;
1014 final double remoteClockOffset = 469.0e-6;
1015 final InterSatellitesRangeMeasurementCreator creator = new InterSatellitesRangeMeasurementCreator(ephemeris,
1016 localClockOffset,
1017 remoteClockOffset);
1018
1019 final List<ObservedMeasurement<?>> measurements =
1020 EstimationTestUtils.createMeasurements(propagator1, creator,
1021 1.0, 3.0, 300.0);
1022
1023
1024 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1025 propagatorBuilder1);
1026 measurements.addAll(EstimationTestUtils.createMeasurements(propagator1,
1027 new RangeMeasurementCreator(context),
1028 1.0, 3.0, 60.0));
1029 measurements.sort(Comparator.naturalOrder());
1030
1031
1032 creator.getLocalSatellite().getClockOffsetDriver().setSelected(true);
1033 creator.getRemoteSatellite().getClockOffsetDriver().setSelected(true);
1034
1035
1036 final ParameterDriversList estimatedMeasurementParameters = new ParameterDriversList();
1037 estimatedMeasurementParameters.add(creator.getLocalSatellite().getClockOffsetDriver());
1038 estimatedMeasurementParameters.add(creator.getRemoteSatellite().getClockOffsetDriver());
1039
1040
1041 final RealMatrix processNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
1042 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10, 1.e-13, 1.e13
1043 });
1044 final RealMatrix measurementNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
1045 1.e-15, 1.e-15
1046 });
1047 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1048 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
1049 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
1050 estimatedMeasurementsParameters(estimatedMeasurementParameters, new ConstantProcessNoise(measurementNoiseMatrix)).
1051 build();
1052
1053 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
1054 ParameterDriver a0Driver = parameters.get(0);
1055 Assert.assertEquals("a[0]", a0Driver.getName());
1056 a0Driver.setValue(a0Driver.getValue() + 1.2);
1057 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1058
1059 ParameterDriver a1Driver = parameters.get(6);
1060 Assert.assertEquals("a[1]", a1Driver.getName());
1061 a1Driver.setValue(a1Driver.getValue() - 5.4);
1062 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1063
1064 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
1065 parameters.get( 7).getValue(),
1066 parameters.get( 8).getValue(),
1067 parameters.get( 9).getValue(),
1068 parameters.get(10).getValue(),
1069 parameters.get(11).getValue(),
1070 PositionAngle.TRUE,
1071 closeOrbit.getFrame(),
1072 closeOrbit.getDate(),
1073 closeOrbit.getMu());
1074 Assert.assertEquals(4.7246,
1075 Vector3D.distance(closeOrbit.getPVCoordinates().getPosition(),
1076 before.getPVCoordinates().getPosition()),
1077 1.0e-3);
1078 Assert.assertEquals(0.0010514,
1079 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
1080 before.getPVCoordinates().getVelocity()),
1081 1.0e-6);
1082
1083 Orbit[] refOrbits = new Orbit[] {
1084 propagatorBuilder1.
1085 buildPropagator(propagatorBuilder1.getSelectedNormalizedParameters()).
1086 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
1087 propagatorBuilder2.
1088 buildPropagator(propagatorBuilder2.getSelectedNormalizedParameters()).
1089 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
1090 };
1091 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1092 refOrbits, new PositionAngle[] { PositionAngle.TRUE, PositionAngle.TRUE },
1093 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
1094 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
1095 new double[][] {
1096 { 6.9e5, 6.0e5, 12.5e5 },
1097 { 6.8e5, 5.9e5, 12.7e5 }
1098 }, new double[] { 1e4, 1e4 },
1099 new double[][] {
1100 { 5.0e2, 5.3e2, 1.4e2 },
1101 { 5.0e2, 5.3e2, 1.4e2 }
1102 }, new double[] { 1.0e1, 1.0e1 });
1103
1104
1105
1106 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
1107 if (driver.getName().startsWith("a[")) {
1108
1109 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
1110 } else {
1111
1112 Assert.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
1113 }
1114 }
1115
1116 }
1117
1118 @Test
1119 public void tesIssue850() {
1120
1121 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1122
1123
1124 final NumericalPropagatorBuilder propagatorBuilder1 =
1125 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
1126 1.0e-6, 60.0, 1.0);
1127 propagatorBuilder1.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
1128
1129 final NumericalPropagatorBuilder propagatorBuilder2 =
1130 context.createBuilder(OrbitType.KEPLERIAN, PositionAngle.TRUE, true,
1131 1.0e-6, 60.0, 1.0);
1132 propagatorBuilder2.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
1133
1134
1135 final Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1136 propagatorBuilder1);
1137
1138 final List<ObservedMeasurement<?>> measurements1 =
1139 EstimationTestUtils.createMeasurements(propagator1,
1140 new PositionMeasurementCreator(),
1141 0.0, 3.0, 300.0);
1142
1143 final Propagator propagator2 = EstimationTestUtils.createPropagator(context.initialOrbit,
1144 propagatorBuilder2);
1145
1146 final List<ObservedMeasurement<?>> measurements2 =
1147 EstimationTestUtils.createMeasurements(propagator2,
1148 new PositionMeasurementCreator(),
1149 0.0, 3.0, 300.0);
1150
1151
1152 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
1153 measurements.add(measurements1.get(0));
1154 measurements.add(measurements2.get(0));
1155 final ObservedMeasurement<?> multiplexed = new MultiplexedMeasurement(measurements);
1156
1157
1158 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
1159 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
1160 });
1161
1162
1163 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
1164 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
1165 });
1166
1167
1168
1169 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1170 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(initialP, Q)).
1171 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(initialP, Q)).
1172 build();
1173
1174
1175 Propagator[] estimated = kalman.estimationStep(multiplexed);
1176 final Vector3D pos1 = estimated[0].getInitialState().getPVCoordinates().getPosition();
1177 final Vector3D pos2 = estimated[1].getInitialState().getPVCoordinates().getPosition();
1178
1179
1180 Assert.assertEquals(0.0, pos1.distance(pos2), 1.0e-12);
1181 Assert.assertEquals(0.0, pos1.distance(context.initialOrbit.getPVCoordinates().getPosition()), 1.0e-12);
1182 Assert.assertEquals(0.0, pos2.distance(context.initialOrbit.getPVCoordinates().getPosition()), 1.0e-12);
1183
1184 }
1185
1186 private static class DummyException extends OrekitException {
1187 private static final long serialVersionUID = 1L;
1188 public DummyException() {
1189 super(OrekitMessages.INTERNAL_ERROR);
1190 }
1191 }
1192 }
1193
1194