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