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.Arrays;
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.jupiter.api.Assertions;
29 import org.junit.jupiter.api.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.Force;
36 import org.orekit.estimation.TLEEstimationTestUtils;
37 import org.orekit.estimation.measurements.*;
38 import org.orekit.estimation.measurements.modifiers.Bias;
39 import org.orekit.estimation.measurements.modifiers.PhaseCentersRangeModifier;
40 import org.orekit.frames.LOFType;
41 import org.orekit.gnss.antenna.FrequencyPattern;
42 import org.orekit.orbits.CartesianOrbit;
43 import org.orekit.orbits.KeplerianOrbit;
44 import org.orekit.orbits.Orbit;
45 import org.orekit.orbits.OrbitType;
46 import org.orekit.orbits.PositionAngleType;
47 import org.orekit.propagation.BoundedPropagator;
48 import org.orekit.propagation.EphemerisGenerator;
49 import org.orekit.propagation.Propagator;
50 import org.orekit.propagation.SpacecraftState;
51 import org.orekit.propagation.analytical.tle.TLE;
52 import org.orekit.propagation.analytical.tle.TLEPropagator;
53 import org.orekit.propagation.analytical.tle.generation.FixedPointTleGenerationAlgorithm;
54 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
55 import org.orekit.propagation.conversion.TLEPropagatorBuilder;
56 import org.orekit.time.AbsoluteDate;
57 import org.orekit.utils.ParameterDriver;
58 import org.orekit.utils.ParameterDriversList;
59 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
60 import org.orekit.utils.TimeStampedPVCoordinates;
61
62 public class KalmanEstimatorTest {
63
64 @Test
65 void testEstimationStepWithBStarOnly() {
66
67 TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
68 String line1 = "1 07276U 74026A 00055.48318287 .00000000 00000-0 22970+3 0 9994";
69 String line2 = "2 07276 71.6273 78.7838 1248323 14.0598 3.8405 4.72707036231812";
70 final TLE tle = new TLE(line1, line2);
71 final TLEPropagatorBuilder propagatorBuilder = new TLEPropagatorBuilder(tle,
72 PositionAngleType.TRUE, 1., new FixedPointTleGenerationAlgorithm());
73 for (final ParameterDriver driver: propagatorBuilder.getOrbitalParametersDrivers().getDrivers()) {
74 driver.setSelected(false);
75 }
76 propagatorBuilder.getPropagationParametersDrivers().getDrivers().get(0).setSelected(true);
77 final KalmanEstimatorBuilder builder = new KalmanEstimatorBuilder();
78 builder.addPropagationConfiguration(propagatorBuilder,
79 new ConstantProcessNoise(MatrixUtils.createRealMatrix(1, 1)));
80 final KalmanEstimator estimator = builder.build();
81 final AbsoluteDate measurementDate = tle.getDate().shiftedBy(1.0);
82 final TLEPropagator propagator = TLEPropagator.selectExtrapolator(tle);
83 final Position positionMeasurement = new Position(measurementDate, propagator.getPosition(measurementDate,
84 propagator.getFrame()), 1., 1., new ObservableSatellite(0));
85
86 Assertions.assertDoesNotThrow(() -> estimator.estimationStep(positionMeasurement));
87 }
88
89 @Test
90 public void testTwoOrbitalParameters() {
91
92
93 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
94
95
96 final OrbitType orbitType = OrbitType.KEPLERIAN;
97 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
98 final boolean perfectStart = true;
99 final double minStep = 1.e-6;
100 final double maxStep = 60.;
101 final double dP = 1.;
102 final NumericalPropagatorBuilder propagatorBuilder =
103 context.createBuilder(orbitType, positionAngleType, perfectStart,
104 minStep, maxStep, dP);
105
106
107 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
108 propagatorBuilder);
109 final AbsoluteDate measurementDate = context.initialOrbit.getDate().shiftedBy(600.0);
110 final SpacecraftState state = propagator.propagate(measurementDate);
111 final ObservedMeasurement<?> measurement = new PV(measurementDate,
112 state.getPosition().add(new Vector3D(10.0, -10.0, 5.0)),
113 state.getPVCoordinates().getVelocity().add(new Vector3D(-10.0, 5.0, -5.0)),
114 5.0, 5.0, 1.0, new ObservableSatellite(0));
115
116
117 propagatorBuilder.getOrbitalParametersDrivers().getDrivers()
118 .forEach(driver -> driver.setSelected(false));
119
120
121 propagatorBuilder.getOrbitalParametersDrivers().findByName("e").setSelected(true);
122 propagatorBuilder.getOrbitalParametersDrivers().findByName("v").setSelected(true);
123
124
125 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[]{
126 1e-2, 1e-5
127 });
128
129
130 final RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double[]{
131 1.e-8, 1.e-8
132 });
133
134
135 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
136 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
137 build();
138
139
140 kalman.estimationStep(measurement);
141
142
143 final KeplerianOrbit initialOrbit = (KeplerianOrbit) context.initialOrbit;
144 Assertions.assertEquals(initialOrbit.getA(),
145 propagatorBuilder.getOrbitalParametersDrivers().findByName("a").getValue());
146 Assertions.assertEquals(initialOrbit.getI(),
147 propagatorBuilder.getOrbitalParametersDrivers().findByName("i").getValue());
148 Assertions.assertEquals(initialOrbit.getRightAscensionOfAscendingNode(),
149 propagatorBuilder.getOrbitalParametersDrivers().findByName("Ω").getValue());
150 Assertions.assertEquals(initialOrbit.getPerigeeArgument(),
151 propagatorBuilder.getOrbitalParametersDrivers().findByName("ω").getValue());
152
153
154 Assertions.assertNotEquals(initialOrbit.getE(),
155 propagatorBuilder.getOrbitalParametersDrivers().findByName("e").getValue());
156 Assertions.assertNotEquals(initialOrbit.getTrueAnomaly(),
157 propagatorBuilder.getOrbitalParametersDrivers().findByName("v").getValue());
158 }
159
160 @Test
161 public void testTwoOrbitalParametersMulti() {
162
163
164 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
165
166
167 final OrbitType orbitType = OrbitType.KEPLERIAN;
168 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
169 final boolean perfectStart = true;
170 final double minStep = 1.e-6;
171 final double maxStep = 60.;
172 final double dP = 1.;
173 final NumericalPropagatorBuilder propagatorBuilder1 =
174 context.createBuilder(orbitType, positionAngleType, perfectStart,
175 minStep, maxStep, dP, Force.POTENTIAL);
176
177 final NumericalPropagatorBuilder propagatorBuilder2 =
178 context.createBuilder(orbitType, positionAngleType, perfectStart,
179 minStep, maxStep, dP, Force.POTENTIAL, Force.SOLAR_RADIATION_PRESSURE);
180
181
182 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
183 propagatorBuilder1);
184 final AbsoluteDate measurementDate = context.initialOrbit.getDate().shiftedBy(600.0);
185 final SpacecraftState state = propagator.propagate(measurementDate);
186 final ObservedMeasurement<?> measurement1 = new PV(measurementDate,
187 state.getPosition().add(new Vector3D(10.0, -10.0, 5.0)),
188 state.getPVCoordinates().getVelocity().add(new Vector3D(-10.0, 5.0, -5.0)),
189 5.0, 5.0, 1.0, new ObservableSatellite(0));
190 final ObservedMeasurement<?> measurement2 = new PV(measurementDate,
191 state.getPosition().add(new Vector3D(-10.0, 20.0, -1.0)),
192 state.getPVCoordinates().getVelocity().add(new Vector3D(10.0, 50.0, -10.0)),
193 5.0, 5.0, 1.0, new ObservableSatellite(1));
194 final ObservedMeasurement<?> combinedMeasurement =
195 new MultiplexedMeasurement(Arrays.asList(measurement1, measurement2));
196
197
198 propagatorBuilder1.getOrbitalParametersDrivers().getDrivers()
199 .forEach(driver -> driver.setSelected(false));
200 propagatorBuilder2.getOrbitalParametersDrivers().getDrivers()
201 .forEach(driver -> driver.setSelected(false));
202
203
204 propagatorBuilder1.getOrbitalParametersDrivers().findByName("e").setSelected(true);
205 propagatorBuilder1.getOrbitalParametersDrivers().findByName("v").setSelected(true);
206
207 propagatorBuilder2.getOrbitalParametersDrivers().findByName("e").setSelected(true);
208 propagatorBuilder2.getOrbitalParametersDrivers().findByName("v").setSelected(true);
209
210
211 propagatorBuilder2.getPropagationParametersDrivers().findByName("reflection coefficient").setSelected(true);
212
213
214 final double[] parameterValues1 = propagatorBuilder1.getPropagationParametersDrivers().getDrivers().stream()
215 .mapToDouble(ParameterDriver::getValue)
216 .toArray();
217 final double[] parameterValues2 = propagatorBuilder2.getPropagationParametersDrivers().getDrivers().stream()
218 .mapToDouble(ParameterDriver::getValue)
219 .toArray();
220
221
222
223 final Propagator referencePropagator1 = propagatorBuilder1.buildPropagator();
224 final KeplerianOrbit refOrbit1 = (KeplerianOrbit) referencePropagator1.propagate(measurementDate).getOrbit();
225
226 final Propagator referencePropagator2 = propagatorBuilder2.buildPropagator();
227 final KeplerianOrbit refOrbit2 = (KeplerianOrbit) referencePropagator2.propagate(measurementDate).getOrbit();
228
229
230 final RealMatrix initialP1 = MatrixUtils.createRealDiagonalMatrix(new double[]{
231 1e-2, 1e-5
232 });
233 final RealMatrix initialP2 = MatrixUtils.createRealDiagonalMatrix(new double[]{
234 1e-2, 1e-5, 1e-5
235 });
236
237
238 final RealMatrix Q1 = MatrixUtils.createRealDiagonalMatrix(new double[]{
239 1e-8, 1e-8
240 });
241 final RealMatrix Q2 = MatrixUtils.createRealDiagonalMatrix(new double[]{
242 1e-8, 1e-8, 1e-8
243 });
244
245
246 final KalmanEstimator kalman = new KalmanEstimatorBuilder()
247 .addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(initialP1, Q1))
248 .addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(initialP2, Q2))
249 .build();
250
251
252 kalman.estimationStep(combinedMeasurement);
253
254
255 Assertions.assertEquals(refOrbit1.getA(),
256 propagatorBuilder1.getOrbitalParametersDrivers().findByName("a[0]").getValue());
257 Assertions.assertEquals(refOrbit1.getI(),
258 propagatorBuilder1.getOrbitalParametersDrivers().findByName("i[0]").getValue());
259 Assertions.assertEquals(refOrbit1.getRightAscensionOfAscendingNode(),
260 propagatorBuilder1.getOrbitalParametersDrivers().findByName("Ω[0]").getValue());
261 Assertions.assertEquals(refOrbit1.getPerigeeArgument(),
262 propagatorBuilder1.getOrbitalParametersDrivers().findByName("ω[0]").getValue());
263
264 Assertions.assertEquals(refOrbit2.getA(),
265 propagatorBuilder2.getOrbitalParametersDrivers().findByName("a[1]").getValue());
266 Assertions.assertEquals(refOrbit2.getI(),
267 propagatorBuilder2.getOrbitalParametersDrivers().findByName("i[1]").getValue());
268 Assertions.assertEquals(refOrbit2.getRightAscensionOfAscendingNode(),
269 propagatorBuilder2.getOrbitalParametersDrivers().findByName("Ω[1]").getValue());
270 Assertions.assertEquals(refOrbit2.getPerigeeArgument(),
271 propagatorBuilder2.getOrbitalParametersDrivers().findByName("ω[1]").getValue());
272
273
274 Assertions.assertNotEquals(refOrbit1.getE(),
275 propagatorBuilder1.getOrbitalParametersDrivers().findByName("e[0]").getValue());
276 Assertions.assertNotEquals(refOrbit1.getTrueAnomaly(),
277 propagatorBuilder1.getOrbitalParametersDrivers().findByName("v[0]").getValue());
278
279 Assertions.assertNotEquals(refOrbit2.getE(),
280 propagatorBuilder2.getOrbitalParametersDrivers().findByName("e[1]").getValue());
281 Assertions.assertNotEquals(refOrbit2.getTrueAnomaly(),
282 propagatorBuilder2.getOrbitalParametersDrivers().findByName("v[1]").getValue());
283
284
285 final List<DelegatingDriver> drivers1 = propagatorBuilder1.getPropagationParametersDrivers().getDrivers();
286 for (int i = 0; i < parameterValues1.length; ++i) {
287 double postEstimation = drivers1.get(i).getValue();
288 Assertions.assertEquals(parameterValues1[i], postEstimation);
289 }
290
291 final List<DelegatingDriver> drivers2 = propagatorBuilder2.getPropagationParametersDrivers().getDrivers();
292 for (int i = 0; i < parameterValues2.length; ++i) {
293 double postEstimation = drivers2.get(i).getValue();
294 if (drivers2.get(i).getName().equals("reflection coefficient")) {
295 Assertions.assertNotEquals(parameterValues2[i], postEstimation);
296 } else {
297 Assertions.assertEquals(parameterValues2[i], postEstimation);
298 }
299 }
300 }
301
302 @Test
303 public void testWrongProcessCovarianceDimension() {
304
305 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
306
307
308 final OrbitType orbitType = OrbitType.KEPLERIAN;
309 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
310 final boolean perfectStart = true;
311 final double minStep = 1.e-6;
312 final double maxStep = 60.;
313 final double dP = 1.;
314 final NumericalPropagatorBuilder propagatorBuilder =
315 context.createBuilder(orbitType, positionAngleType, perfectStart,
316 minStep, maxStep, dP);
317
318
319 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
320 propagatorBuilder);
321 final AbsoluteDate measurementDate = context.initialOrbit.getDate().shiftedBy(600.0);
322 final SpacecraftState state = propagator.propagate(measurementDate);
323 final ObservedMeasurement<PV> measurement = new PV(measurementDate,
324 state.getPosition().add(new Vector3D(10.0, -10.0, 5.0)),
325 state.getPVCoordinates().getVelocity().add(new Vector3D(-10.0, 5.0, -5.0)),
326 5.0, 5.0, 1.0, new ObservableSatellite(0));
327
328
329 propagatorBuilder.getOrbitalParametersDrivers().getDrivers()
330 .forEach(driver -> driver.setSelected(false));
331
332
333 propagatorBuilder.getOrbitalParametersDrivers().findByName("e").setSelected(true);
334 propagatorBuilder.getOrbitalParametersDrivers().findByName("v").setSelected(true);
335
336
337 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[]{
338 1e-2, 1e-5
339 });
340 final RealMatrix badInitialP = MatrixUtils.createRealDiagonalMatrix(new double[]{
341 1e-2, 1e-5, 1e6
342 });
343
344
345 final RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double[]{
346 1.e-8, 1.e-8
347 });
348 final RealMatrix badQ = MatrixUtils.createRealDiagonalMatrix(new double[]{
349 1.e-8, 1.e-8, 1e-6
350 });
351 final RealMatrix measQ = MatrixUtils.createRealDiagonalMatrix(new double[]{
352 1.e-8
353 });
354
355
356 final KalmanEstimatorBuilder kalmanBuilderBadInitial = new KalmanEstimatorBuilder()
357 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(badInitialP, Q));
358
359
360 OrekitException thrown = Assertions.assertThrows(OrekitException.class, kalmanBuilderBadInitial::build);
361 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrown.getSpecifier());
362 Assertions.assertEquals(2, thrown.getParts()[0]);
363 Assertions.assertEquals(3, thrown.getParts()[1]);
364
365
366 final KalmanEstimator kalmanBadProcessNoise = new KalmanEstimatorBuilder()
367 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, badQ))
368 .build();
369
370
371 thrown = Assertions.assertThrows(OrekitException.class, () -> kalmanBadProcessNoise.estimationStep(measurement));
372 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrown.getSpecifier());
373 Assertions.assertEquals(2, thrown.getParts()[0]);
374 Assertions.assertEquals(3, thrown.getParts()[1]);
375
376
377 final KalmanEstimatorBuilder kalmanBadProcessNoiseWithMeasurementProcessNoise = new KalmanEstimatorBuilder()
378 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(badInitialP, badQ))
379 .estimatedMeasurementsParameters(new ParameterDriversList(), new ConstantProcessNoise(measQ, measQ));
380
381
382 thrown = Assertions.assertThrows(OrekitException.class, kalmanBadProcessNoiseWithMeasurementProcessNoise::build);
383 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrown.getSpecifier());
384 Assertions.assertEquals(2, thrown.getParts()[0]);
385 Assertions.assertEquals(3, thrown.getParts()[1]);
386
387
388 final Bias<PV> pvBias = new Bias<>(new String[]{"x bias"},
389 new double[]{0.0}, new double[]{1.0},
390 new double[]{1.0}, new double[]{1.0});
391 pvBias.getParameterDriver("x bias").setSelected(true);
392 measurement.addModifier(pvBias);
393
394
395 ParameterDriversList drivers = new ParameterDriversList();
396 drivers.add(pvBias.getParameterDriver("x bias"));
397 final KalmanEstimator estimatorBadMeasurementNoise = new KalmanEstimatorBuilder()
398 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, badQ))
399 .estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(measQ, measQ)).build();
400
401
402 thrown = Assertions.assertThrows(OrekitException.class, () -> estimatorBadMeasurementNoise.estimationStep(measurement));
403 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrown.getSpecifier());
404 Assertions.assertEquals(2, thrown.getParts()[0]);
405 Assertions.assertEquals(3, thrown.getParts()[1]);
406
407 }
408
409 @Test
410 public void testWrongMeasurementCovarianceDimension() {
411
412
413 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
414
415
416 final OrbitType orbitType = OrbitType.KEPLERIAN;
417 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
418 final boolean perfectStart = true;
419 final double minStep = 1.e-6;
420 final double maxStep = 60.;
421 final double dP = 1.;
422 final NumericalPropagatorBuilder propagatorBuilder =
423 context.createBuilder(orbitType, positionAngleType, perfectStart,
424 minStep, maxStep, dP);
425
426
427 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
428 propagatorBuilder);
429 final List<ObservedMeasurement<?>> measurements =
430 EstimationTestUtils.createMeasurements(propagator,
431 new TwoWayRangeMeasurementCreator(context),
432 1.0, 1.1, 60.0);
433
434
435 final Bias<Range> rangeBias = new Bias<>(new String[]{"range bias"}, new double[]{0.0}, new double[]{1.0},
436 new double[]{Double.NEGATIVE_INFINITY}, new double[]{Double.POSITIVE_INFINITY});
437 rangeBias.getParameterDriver("range bias").setSelected(true);
438 measurements.forEach(measurement -> ((Range) measurement).addModifier(rangeBias));
439
440 ParameterDriversList measurementParameters = new ParameterDriversList();
441 measurementParameters.add(rangeBias.getParameterDriver("range bias"));
442
443
444 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
445 aDriver.setValue(aDriver.getValue() + 1.2);
446 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
447
448
449
450 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
451 100., 100., 100., 1e-2, 1e-2, 1e-2
452 });
453
454
455 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
456 final double[][] dYdC = new double[6][6];
457 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
458 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
459
460
461 final RealMatrix initialPStateOnly = Jac.multiply(cartesianP.multiply(Jac.transpose()));
462 final RealMatrix initialP = MatrixUtils.createRealIdentityMatrix(7);
463 initialP.setSubMatrix(initialPStateOnly.getData(), 0, 0);
464
465
466 final RealMatrix QStateOnly = MatrixUtils.createRealMatrix(6, 6);
467 final RealMatrix Q = MatrixUtils.createRealMatrix(7, 7);
468
469
470 final KalmanEstimatorBuilder kalmanBuilderBadInitial = new KalmanEstimatorBuilder()
471 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialPStateOnly, Q))
472 .estimatedMeasurementsParameters(measurementParameters, null);
473
474
475 final OrekitException thrownBadInitial =
476 Assertions.assertThrows(OrekitException.class, kalmanBuilderBadInitial::build);
477 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrownBadInitial.getSpecifier());
478 Assertions.assertEquals(7, thrownBadInitial.getParts()[0]);
479 Assertions.assertEquals(6, thrownBadInitial.getParts()[1]);
480
481
482 final KalmanEstimator kalmanBadProcessNoise = new KalmanEstimatorBuilder()
483 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, QStateOnly))
484 .estimatedMeasurementsParameters(measurementParameters, null)
485 .build();
486
487
488 final OrekitException thrownBadQ = Assertions.assertThrows(OrekitException.class,
489 () -> kalmanBadProcessNoise.processMeasurements(measurements));
490 Assertions.assertEquals(OrekitMessages.WRONG_PROCESS_COVARIANCE_DIMENSION, thrownBadQ.getSpecifier());
491 Assertions.assertEquals(7, thrownBadQ.getParts()[0]);
492 Assertions.assertEquals(6, thrownBadQ.getParts()[1]);
493
494
495 final ConstantProcessNoise badMeasurementInitialNoise =
496 new ConstantProcessNoise(MatrixUtils.createRealIdentityMatrix(2),
497 MatrixUtils.createRealIdentityMatrix(1));
498 final ConstantProcessNoise badMeasurementProcessNoise =
499 new ConstantProcessNoise(MatrixUtils.createRealIdentityMatrix(1),
500 MatrixUtils.createRealIdentityMatrix(2));
501
502
503 final KalmanEstimatorBuilder kalmanBuilderBadInitialMeas = new KalmanEstimatorBuilder()
504 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialPStateOnly, QStateOnly))
505 .estimatedMeasurementsParameters(measurementParameters, badMeasurementInitialNoise);
506
507
508 final OrekitException thrownBadInitialMeas =
509 Assertions.assertThrows(OrekitException.class, kalmanBuilderBadInitialMeas::build);
510 Assertions.assertEquals(OrekitMessages.WRONG_MEASUREMENT_COVARIANCE_DIMENSION, thrownBadInitialMeas.getSpecifier());
511 Assertions.assertEquals(1, thrownBadInitialMeas.getParts()[0]);
512 Assertions.assertEquals(2, thrownBadInitialMeas.getParts()[1]);
513
514
515 final KalmanEstimator kalmanBadProcessNoiseMeas = new KalmanEstimatorBuilder()
516 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialPStateOnly, QStateOnly))
517 .estimatedMeasurementsParameters(measurementParameters, badMeasurementProcessNoise)
518 .build();
519
520
521 final OrekitException thrownBadQMeas = Assertions.assertThrows(OrekitException.class,
522 () -> kalmanBadProcessNoiseMeas.processMeasurements(measurements));
523 Assertions.assertEquals(OrekitMessages.WRONG_MEASUREMENT_COVARIANCE_DIMENSION, thrownBadQMeas.getSpecifier());
524 Assertions.assertEquals(1, thrownBadQMeas.getParts()[0]);
525 Assertions.assertEquals(2, thrownBadQMeas.getParts()[1]);
526
527 }
528
529 @Test
530 public void testMissingPropagatorBuilder() {
531 try {
532 new KalmanEstimatorBuilder().
533 build();
534 Assertions.fail("an exception should have been thrown");
535 } catch (OrekitException oe) {
536 Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
537 }
538 }
539
540
541
542
543
544 @Test
545 public void testKeplerianPV() {
546
547
548 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
549
550
551 final OrbitType orbitType = OrbitType.KEPLERIAN;
552 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
553 final boolean perfectStart = true;
554 final double minStep = 1.e-6;
555 final double maxStep = 60.;
556 final double dP = 1.;
557 final NumericalPropagatorBuilder propagatorBuilder =
558 context.createBuilder(orbitType, positionAngleType, perfectStart,
559 minStep, maxStep, dP);
560
561
562 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
563 propagatorBuilder);
564 final List<ObservedMeasurement<?>> measurements =
565 EstimationTestUtils.createMeasurements(propagator,
566 new PVMeasurementCreator(),
567 0.0, 3.0, 300.0);
568
569 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
570
571
572 final Orbit refOrbit = referencePropagator.
573 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
574
575
576 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
577 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5
578 });
579
580
581 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
582 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
583 });
584
585
586
587 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
588 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
589 build();
590
591
592 final double expectedDeltaPos = 0.;
593 final double posEps = 5.80e-8;
594 final double expectedDeltaVel = 0.;
595 final double velEps = 2.28e-11;
596 final double[] expectedsigmasPos = {0.998872, 0.933655, 0.997516};
597 final double sigmaPosEps = 1e-6;
598 final double[] expectedSigmasVel = {9.478853e-4, 9.910788e-4, 5.0438709e-4};
599 final double sigmaVelEps = 1e-10;
600 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
601 refOrbit, positionAngleType,
602 expectedDeltaPos, posEps,
603 expectedDeltaVel, velEps,
604 expectedsigmasPos, sigmaPosEps,
605 expectedSigmasVel, sigmaVelEps);
606 }
607
608
609
610
611
612 @Test
613 public void testKeplerianRange() {
614
615
616 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
617
618
619 final OrbitType orbitType = OrbitType.KEPLERIAN;
620 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
621 final boolean perfectStart = true;
622 final double minStep = 1.e-6;
623 final double maxStep = 60.;
624 final double dP = 1.;
625 final NumericalPropagatorBuilder propagatorBuilder =
626 context.createBuilder(orbitType, positionAngleType, perfectStart,
627 minStep, maxStep, dP);
628
629
630 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
631 propagatorBuilder);
632 final List<ObservedMeasurement<?>> measurements =
633 EstimationTestUtils.createMeasurements(propagator,
634 new TwoWayRangeMeasurementCreator(context),
635 1.0, 4.0, 60.0);
636
637
638 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
639
640
641 final Orbit refOrbit = referencePropagator.
642 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
643
644
645 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
646 aDriver.setValue(aDriver.getValue() + 1.2);
647 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
648
649
650
651 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
652 100., 100., 100., 1e-2, 1e-2, 1e-2
653 });
654
655
656 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
657 final double[][] dYdC = new double[6][6];
658 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
659 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
660
661
662 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
663
664
665 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
666
667
668 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
669 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
670 build();
671
672
673 final double expectedDeltaPos = 0.;
674 final double posEps = 1.77e-4;
675 final double expectedDeltaVel = 0.;
676 final double velEps = 7.93e-8;
677 final double[] expectedSigmasPos = {0.742488, 0.281914, 0.563213};
678 final double sigmaPosEps = 1e-6;
679 final double[] expectedSigmasVel = {2.206636e-4, 1.306656e-4, 1.293981e-4};
680 final double sigmaVelEps = 1e-10;
681 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
682 refOrbit, positionAngleType,
683 expectedDeltaPos, posEps,
684 expectedDeltaVel, velEps,
685 expectedSigmasPos, sigmaPosEps,
686 expectedSigmasVel, sigmaVelEps);
687 }
688
689
690
691
692
693 @Test
694 public void testKeplerianRangeWithOnBoardAntennaOffset() {
695
696
697 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
698
699
700 final OrbitType orbitType = OrbitType.KEPLERIAN;
701 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
702 final boolean perfectStart = true;
703 final double minStep = 1.e-6;
704 final double maxStep = 60.;
705 final double dP = 1.;
706 final NumericalPropagatorBuilder propagatorBuilder =
707 context.createBuilder(orbitType, positionAngleType, perfectStart,
708 minStep, maxStep, dP);
709 propagatorBuilder.setAttitudeProvider(new LofOffset(propagatorBuilder.getFrame(), LOFType.LVLH));
710
711
712 final Vector3D antennaPhaseCenter = new Vector3D(-1.2, 2.3, -0.7);
713
714
715 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
716 propagatorBuilder);
717 final List<ObservedMeasurement<?>> measurements =
718 EstimationTestUtils.createMeasurements(propagator,
719 new TwoWayRangeMeasurementCreator(context,
720 Vector3D.ZERO, null,
721 antennaPhaseCenter, null,
722 0),
723 1.0, 3.0, 300.0);
724
725
726 final PhaseCentersRangeModifier obaModifier = new PhaseCentersRangeModifier(FrequencyPattern.ZERO_CORRECTION,
727 new FrequencyPattern(antennaPhaseCenter,
728 null));
729 for (final ObservedMeasurement<?> range : measurements) {
730 ((Range) range).addModifier(obaModifier);
731 }
732
733
734 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
735
736
737 final Orbit refOrbit = referencePropagator.
738 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
739
740
741 ParameterDriver aDriver = propagatorBuilder.getOrbitalParametersDrivers().getDrivers().get(0);
742 aDriver.setValue(aDriver.getValue() + 1.2);
743 aDriver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
744
745
746
747 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
748 10., 10., 10., 1e-3, 1e-3, 1e-3
749 });
750
751
752 final Orbit initialOrbit = OrbitType.KEPLERIAN.convertType(context.initialOrbit);
753 final double[][] dYdC = new double[6][6];
754 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
755 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
756
757
758 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
759
760
761 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
762
763
764 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
765 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
766 build();
767
768
769 final double expectedDeltaPos = 0.;
770 final double posEps = 4.57e-3;
771 final double expectedDeltaVel = 0.;
772 final double velEps = 7.29e-6;
773 final double[] expectedSigmasPos = {1.105198, 0.930797, 1.254581};
774 final double sigmaPosEps = 1e-6;
775 final double[] expectedSigmasVel = {6.193757e-4, 4.088798e-4, 3.299140e-4};
776 final double sigmaVelEps = 1e-10;
777 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
778 refOrbit, positionAngleType,
779 expectedDeltaPos, posEps,
780 expectedDeltaVel, velEps,
781 expectedSigmasPos, sigmaPosEps,
782 expectedSigmasVel, sigmaVelEps);
783 }
784
785
786
787
788
789 @Test
790 public void testCartesianRangeRate() {
791
792
793 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
794
795
796 final OrbitType orbitType = OrbitType.CARTESIAN;
797 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
798 final boolean perfectStart = true;
799 final double minStep = 1.e-6;
800 final double maxStep = 60.;
801 final double dP = 1.;
802 final NumericalPropagatorBuilder propagatorBuilder =
803 context.createBuilder(orbitType, positionAngleType, perfectStart,
804 minStep, maxStep, dP);
805
806
807 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
808 propagatorBuilder);
809 final double satClkDrift = 3.2e-10;
810 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
811 final List<ObservedMeasurement<?>> measurements =
812 EstimationTestUtils.createMeasurements(propagator,
813 creator,
814 1.0, 3.0, 300.0);
815
816
817 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
818
819
820 final Orbit refOrbit = referencePropagator.
821 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
822
823
824
825 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
826 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
827 });
828
829
830 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
831 final double[][] dYdC = new double[6][6];
832 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
833 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
834
835
836 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
837
838
839 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
840 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
841 });
842 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
843
844
845 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
846 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
847 build();
848
849
850 final double expectedDeltaPos = 0.;
851 final double posEps = 1.5e-6;
852 final double expectedDeltaVel = 0.;
853 final double velEps = 5.1e-10;
854 final double[] expectedSigmasPos = {0.324407, 1.347014, 1.743326};
855 final double sigmaPosEps = 1e-6;
856 final double[] expectedSigmasVel = {2.85688e-4, 5.765933e-4, 5.056124e-4};
857 final double sigmaVelEps = 1e-10;
858 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
859 refOrbit, positionAngleType,
860 expectedDeltaPos, posEps,
861 expectedDeltaVel, velEps,
862 expectedSigmasPos, sigmaPosEps,
863 expectedSigmasVel, sigmaVelEps);
864 }
865
866
867
868
869
870 @Test
871 public void testCircularAzimuthElevation() {
872
873
874 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
875
876
877 final OrbitType orbitType = OrbitType.CIRCULAR;
878 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
879 final boolean perfectStart = true;
880 final double minStep = 1.e-6;
881 final double maxStep = 60.;
882 final double dP = 1.;
883 final NumericalPropagatorBuilder propagatorBuilder =
884 context.createBuilder(orbitType, positionAngleType, perfectStart,
885 minStep, maxStep, dP);
886
887
888 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
889 propagatorBuilder);
890 final List<ObservedMeasurement<?>> measurements =
891 EstimationTestUtils.createMeasurements(propagator,
892 new AngularAzElMeasurementCreator(context),
893 1.0, 4.0, 60.0);
894
895
896 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
897
898
899 final Orbit refOrbit = referencePropagator.
900 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
901
902
903 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
904 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
905 });
906
907
908 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
909 final double[][] dYdC = new double[6][6];
910 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
911 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
912
913
914 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
915
916
917 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
918 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
919 });
920 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
921
922
923 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
924 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
925 build();
926
927
928 final double expectedDeltaPos = 0.;
929 final double posEps = 4.78e-7;
930 final double expectedDeltaVel = 0.;
931 final double velEps = 1.54e-10;
932 final double[] expectedSigmasPos = {0.356902, 1.297507, 1.798551};
933 final double sigmaPosEps = 1e-6;
934 final double[] expectedSigmasVel = {2.468745e-4, 5.810027e-4, 3.887394e-4};
935 final double sigmaVelEps = 1e-10;
936 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
937 refOrbit, positionAngleType,
938 expectedDeltaPos, posEps,
939 expectedDeltaVel, velEps,
940 expectedSigmasPos, sigmaPosEps,
941 expectedSigmasVel, sigmaVelEps);
942 }
943
944
945
946
947
948 @Test
949 public void testEquinoctialRightAscensionDeclination() {
950
951
952 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
953
954
955 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
956 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
957 final boolean perfectStart = true;
958 final double minStep = 1.e-6;
959 final double maxStep = 60.;
960 final double dP = 1.;
961 final NumericalPropagatorBuilder propagatorBuilder =
962 context.createBuilder(orbitType, positionAngleType, perfectStart,
963 minStep, maxStep, dP);
964
965
966 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
967 propagatorBuilder);
968 final List<ObservedMeasurement<?>> measurements =
969 EstimationTestUtils.createMeasurements(propagator,
970 new AngularRaDecMeasurementCreator(context),
971 1.0, 4.0, 60.0);
972
973
974 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
975
976
977 final Orbit refOrbit = referencePropagator.
978 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
979
980
981 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
982 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
983 });
984
985
986 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
987 final double[][] dYdC = new double[6][6];
988 initialOrbit.getJacobianWrtCartesian(positionAngleType, dYdC);
989 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
990
991
992 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
993
994
995 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
996 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
997 });
998 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
999
1000
1001 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1002 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
1003 build();
1004
1005
1006 final double expectedDeltaPos = 0.;
1007 final double posEps = 4.8e-7;
1008 final double expectedDeltaVel = 0.;
1009 final double velEps = 1.6e-10;
1010 final double[] expectedSigmasPos = {0.356902, 1.297508, 1.798552};
1011 final double sigmaPosEps = 1e-6;
1012 final double[] expectedSigmasVel = {2.468746e-4, 5.810028e-4, 3.887394e-4};
1013 final double sigmaVelEps = 1e-10;
1014 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1015 refOrbit, positionAngleType,
1016 expectedDeltaPos, posEps,
1017 expectedDeltaVel, velEps,
1018 expectedSigmasPos, sigmaPosEps,
1019 expectedSigmasVel, sigmaVelEps);
1020 }
1021
1022
1023
1024
1025
1026 @Test
1027 public void testKeplerianRangeAzElAndRangeRate() {
1028
1029
1030 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1031
1032
1033 final OrbitType orbitType = OrbitType.KEPLERIAN;
1034 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
1035 final boolean perfectStart = true;
1036 final double minStep = 1.e-6;
1037 final double maxStep = 60.;
1038 final double dP = 1.;
1039 final NumericalPropagatorBuilder measPropagatorBuilder =
1040 context.createBuilder(orbitType, positionAngleType, perfectStart,
1041 minStep, maxStep, dP);
1042
1043
1044 final Propagator rangePropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1045 measPropagatorBuilder);
1046 final List<ObservedMeasurement<?>> rangeMeasurements =
1047 EstimationTestUtils.createMeasurements(rangePropagator,
1048 new TwoWayRangeMeasurementCreator(context),
1049 0.0, 4.0, 300.0);
1050
1051 final Propagator angularPropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1052 measPropagatorBuilder);
1053 final List<ObservedMeasurement<?>> angularMeasurements =
1054 EstimationTestUtils.createMeasurements(angularPropagator,
1055 new AngularAzElMeasurementCreator(context),
1056 0.0, 4.0, 500.0);
1057
1058 final Propagator rangeRatePropagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1059 measPropagatorBuilder);
1060 final List<ObservedMeasurement<?>> rangeRateMeasurements =
1061 EstimationTestUtils.createMeasurements(rangeRatePropagator,
1062 new RangeRateMeasurementCreator(context, false, 3.2e-10),
1063 0.0, 4.0, 700.0);
1064
1065
1066 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
1067 measurements.addAll(rangeMeasurements);
1068 measurements.addAll(angularMeasurements);
1069 measurements.addAll(rangeRateMeasurements);
1070 measurements.sort(Comparator.naturalOrder());
1071
1072
1073 final Propagator referencePropagator = measPropagatorBuilder.buildPropagator();
1074
1075
1076 final Orbit refOrbit = referencePropagator.
1077 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
1078
1079
1080 final NumericalPropagatorBuilder propagatorBuilder =
1081 context.createBuilder(orbitType, positionAngleType, false,
1082 minStep, maxStep, dP);
1083
1084
1085
1086 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
1087 1e6, 1e6, 1e6, 1e-4, 1e-4, 1e-4
1088 });
1089
1090
1091 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
1092 final double[][] dYdC = new double[6][6];
1093 initialOrbit.getJacobianWrtCartesian(positionAngleType, dYdC);
1094 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
1095
1096
1097 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
1098
1099
1100 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
1101 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
1102 });
1103 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
1104
1105
1106 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1107 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
1108 build();
1109
1110
1111 final double expectedDeltaPos = 0.;
1112 final double posEps = 2.94e-2;
1113 final double expectedDeltaVel = 0.;
1114 final double velEps = 5.8e-6;
1115 final double[] expectedSigmasPos = {1.747575, 0.666887, 1.696202};
1116 final double sigmaPosEps = 1e-6;
1117 final double[] expectedSigmasVel = {5.413689e-4, 4.088394e-4, 4.315366e-4};
1118 final double sigmaVelEps = 1e-10;
1119 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1120 refOrbit, positionAngleType,
1121 expectedDeltaPos, posEps,
1122 expectedDeltaVel, velEps,
1123 expectedSigmasPos, sigmaPosEps,
1124 expectedSigmasVel, sigmaVelEps);
1125 }
1126
1127
1128
1129
1130 @Test
1131 public void testKeplerianRangeAndRangeRate() {
1132
1133
1134 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1135
1136
1137 final OrbitType orbitType = OrbitType.KEPLERIAN;
1138 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
1139 final boolean perfectStart = true;
1140 final double minStep = 1.e-6;
1141 final double maxStep = 60.;
1142 final double dP = 1.;
1143 final NumericalPropagatorBuilder propagatorBuilder =
1144 context.createBuilder(orbitType, positionAngleType, perfectStart,
1145 minStep, maxStep, dP);
1146
1147
1148 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1149 propagatorBuilder);
1150 final List<ObservedMeasurement<?>> measurementsRange =
1151 EstimationTestUtils.createMeasurements(propagator,
1152 new TwoWayRangeMeasurementCreator(context),
1153 1.0, 3.0, 300.0);
1154
1155 final List<ObservedMeasurement<?>> measurementsRangeRate =
1156 EstimationTestUtils.createMeasurements(propagator,
1157 new RangeRateMeasurementCreator(context, false, 3.2e-10),
1158 1.0, 3.0, 300.0);
1159
1160
1161 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
1162 measurements.addAll(measurementsRange);
1163 measurements.addAll(measurementsRangeRate);
1164
1165
1166 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
1167
1168
1169 final Orbit refOrbit = referencePropagator.
1170 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
1171
1172
1173
1174 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
1175 1e-2, 1e-2, 1e-2, 1e-8, 1e-8, 1e-8
1176 });
1177
1178
1179 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
1180 final double[][] dYdC = new double[6][6];
1181 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
1182 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
1183
1184
1185 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
1186
1187
1188 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
1189 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
1190 });
1191 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
1192
1193
1194 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1195 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
1196 build();
1197
1198
1199 final double expectedDeltaPos = 0.;
1200 final double posEps = 1.7e-6;
1201 final double expectedDeltaVel = 0.;
1202 final double velEps = 5.8e-10;
1203 final double[] expectedSigmasPos = {0.341528, 8.175341, 4.634528};
1204 final double sigmaPosEps = 1e-6;
1205 final double[] expectedSigmasVel = {1.167859e-3, 1.036492e-3, 2.834413e-3};
1206 final double sigmaVelEps = 1e-9;
1207 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1208 refOrbit, positionAngleType,
1209 expectedDeltaPos, posEps,
1210 expectedDeltaVel, velEps,
1211 expectedSigmasPos, sigmaPosEps,
1212 expectedSigmasVel, sigmaVelEps);
1213 }
1214
1215 @Test
1216 public void testMultiSat() {
1217
1218 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1219
1220 final NumericalPropagatorBuilder propagatorBuilder1 =
1221 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1222 1.0e-6, 60.0, 1.0);
1223 final NumericalPropagatorBuilder propagatorBuilder2 =
1224 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1225 1.0e-6, 60.0, 1.0);
1226 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
1227
1228
1229 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
1230 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
1231 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
1232 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
1233 context.initialOrbit.getFrame(),
1234 context.initialOrbit.getMu());
1235 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
1236 propagatorBuilder2);
1237 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
1238 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
1239 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
1240 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1241 propagatorBuilder1);
1242 final double localClockOffset = 0.137e-6;
1243 final double remoteClockOffset = 469.0e-6;
1244 final List<ObservedMeasurement<?>> measurements =
1245 EstimationTestUtils.createMeasurements(propagator1,
1246 new InterSatellitesRangeMeasurementCreator(ephemeris,
1247 localClockOffset,
1248 remoteClockOffset),
1249 1.0, 3.0, 300.0);
1250
1251
1252 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1253 propagatorBuilder1);
1254 measurements.addAll(EstimationTestUtils.createMeasurements(propagator1,
1255 new TwoWayRangeMeasurementCreator(context),
1256 1.0, 3.0, 60.0));
1257 measurements.sort(Comparator.naturalOrder());
1258
1259
1260 final RealMatrix processNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
1261 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
1262 });
1263 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1264 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
1265 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
1266 build();
1267
1268 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
1269 ParameterDriver a0Driver = parameters.get(0);
1270 Assertions.assertEquals("a[0]", a0Driver.getName());
1271 a0Driver.setValue(a0Driver.getValue() + 1.2);
1272 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1273
1274 ParameterDriver a1Driver = parameters.get(6);
1275 Assertions.assertEquals("a[1]", a1Driver.getName());
1276 a1Driver.setValue(a1Driver.getValue() - 5.4);
1277 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1278
1279 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
1280 parameters.get( 7).getValue(),
1281 parameters.get( 8).getValue(),
1282 parameters.get( 9).getValue(),
1283 parameters.get(10).getValue(),
1284 parameters.get(11).getValue(),
1285 PositionAngleType.TRUE,
1286 closeOrbit.getFrame(),
1287 closeOrbit.getDate(),
1288 closeOrbit.getMu());
1289 Assertions.assertEquals(4.7246,
1290 Vector3D.distance(closeOrbit.getPosition(),
1291 before.getPosition()),
1292 1.0e-3);
1293 Assertions.assertEquals(0.0010514,
1294 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
1295 before.getPVCoordinates().getVelocity()),
1296 1.0e-6);
1297
1298 Orbit[] refOrbits = new Orbit[] {
1299 propagatorBuilder1.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
1300 propagatorBuilder2.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
1301 };
1302 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1303 refOrbits, new PositionAngleType[] { PositionAngleType.TRUE, PositionAngleType.TRUE },
1304 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
1305 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
1306 new double[][] {
1307 { 6.9e5, 6.0e5, 12.5e5 },
1308 { 6.8e5, 5.9e5, 12.7e5 }
1309 }, new double[] { 1e4, 1e4 },
1310 new double[][] {
1311 { 5.0e2, 5.3e2, 1.4e2 },
1312 { 5.0e2, 5.3e2, 1.4e2 }
1313 }, new double[] { 1.0e1, 1.0e1 });
1314
1315
1316
1317 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
1318 if (driver.getName().startsWith("a[")) {
1319
1320 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
1321 } else {
1322
1323 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
1324 }
1325 }
1326
1327 }
1328
1329
1330
1331
1332 @Test
1333 public void testWrappedException() {
1334
1335
1336 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1337
1338
1339 final OrbitType orbitType = OrbitType.KEPLERIAN;
1340 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
1341 final boolean perfectStart = true;
1342 final double minStep = 1.e-6;
1343 final double maxStep = 60.;
1344 final double dP = 1.;
1345 final NumericalPropagatorBuilder propagatorBuilder =
1346 context.createBuilder(orbitType, positionAngleType, perfectStart,
1347 minStep, maxStep, dP);
1348
1349
1350 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
1351 propagatorBuilder);
1352 final List<ObservedMeasurement<?>> measurements =
1353 EstimationTestUtils.createMeasurements(propagator,
1354 new TwoWayRangeMeasurementCreator(context),
1355 1.0, 3.0, 300.0);
1356
1357 final KalmanEstimatorBuilder kalmanBuilder = new KalmanEstimatorBuilder();
1358 kalmanBuilder.addPropagationConfiguration(propagatorBuilder,
1359 new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6)));
1360 final KalmanEstimator kalman = kalmanBuilder.build();
1361 kalman.setObserver(estimation -> {
1362 throw new DummyException();
1363 });
1364
1365
1366 try {
1367
1368 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1369 context.initialOrbit, positionAngleType,
1370 0., 0.,
1371 0., 0.,
1372 new double[3], 0.,
1373 new double[3], 0.);
1374 } catch (DummyException de) {
1375
1376 }
1377
1378 }
1379
1380 @Test
1381 public void testIssue695() {
1382
1383
1384 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1385
1386
1387 final Position position = new Position(context.initialOrbit.getDate(),
1388 new Vector3D(1.0, -1.0, 0.0),
1389 0.5, 1.0, new ObservableSatellite(0));
1390
1391
1392 MeasurementDecorator decorated = KalmanEstimatorUtil.decorate(position, context.initialOrbit.getDate());
1393
1394
1395 Assertions.assertEquals(0.0, decorated.getTime(), 1.0e-15);
1396
1397 final RealMatrix covariance = decorated.getCovariance();
1398 for (int i = 0; i < covariance.getRowDimension(); i++) {
1399 for (int j = 0; j < covariance.getColumnDimension(); j++) {
1400 if (i == j) {
1401 Assertions.assertEquals(1.0, covariance.getEntry(i, j), 1.0e-15);
1402 } else {
1403 Assertions.assertEquals(0.0, covariance.getEntry(i, j), 1.0e-15);
1404 }
1405 }
1406 }
1407
1408 }
1409
1410 @Test
1411 public void tesIssue696() {
1412
1413 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1414
1415 final NumericalPropagatorBuilder propagatorBuilder1 =
1416 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1417 1.0e-6, 60.0, 1.0);
1418 final NumericalPropagatorBuilder propagatorBuilder2 =
1419 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1420 1.0e-6, 60.0, 1.0);
1421 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
1422
1423
1424 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
1425 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
1426 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
1427 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
1428 context.initialOrbit.getFrame(),
1429 context.initialOrbit.getMu());
1430 final Propagator closePropagator = EstimationTestUtils.createPropagator(closeOrbit,
1431 propagatorBuilder2);
1432 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
1433 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
1434 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
1435 Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1436 propagatorBuilder1);
1437 final double localClockOffset = 0.137e-6;
1438 final double remoteClockOffset = 469.0e-6;
1439 final InterSatellitesRangeMeasurementCreator creator = new InterSatellitesRangeMeasurementCreator(ephemeris,
1440 localClockOffset,
1441 remoteClockOffset);
1442
1443 final List<ObservedMeasurement<?>> measurements =
1444 EstimationTestUtils.createMeasurements(propagator1, creator,
1445 1.0, 3.0, 300.0);
1446
1447
1448 propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1449 propagatorBuilder1);
1450 measurements.addAll(EstimationTestUtils.createMeasurements(propagator1,
1451 new TwoWayRangeMeasurementCreator(context),
1452 1.0, 3.0, 60.0));
1453 measurements.sort(Comparator.naturalOrder());
1454
1455
1456 creator.getLocalSatellite().getClockOffsetDriver().setSelected(true);
1457 creator.getRemoteSatellite().getClockOffsetDriver().setSelected(true);
1458
1459
1460 final ParameterDriversList estimatedMeasurementParameters = new ParameterDriversList();
1461 estimatedMeasurementParameters.add(creator.getLocalSatellite().getClockOffsetDriver());
1462 estimatedMeasurementParameters.add(creator.getRemoteSatellite().getClockOffsetDriver());
1463
1464
1465 final RealMatrix processNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
1466 1.e-4, 1.e-4, 1.e-4, 1.e-10, 1.e-10, 1.e-10
1467 });
1468 final RealMatrix measurementNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(new double[] {
1469 1.e-15, 1.e-15
1470 });
1471 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1472 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
1473 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
1474 estimatedMeasurementsParameters(estimatedMeasurementParameters, new ConstantProcessNoise(measurementNoiseMatrix)).
1475 build();
1476
1477 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
1478 ParameterDriver a0Driver = parameters.get(0);
1479 Assertions.assertEquals("a[0]", a0Driver.getName());
1480 a0Driver.setValue(a0Driver.getValue() + 1.2);
1481 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1482
1483 ParameterDriver a1Driver = parameters.get(6);
1484 Assertions.assertEquals("a[1]", a1Driver.getName());
1485 a1Driver.setValue(a1Driver.getValue() - 5.4);
1486 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
1487
1488 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
1489 parameters.get( 7).getValue(),
1490 parameters.get( 8).getValue(),
1491 parameters.get( 9).getValue(),
1492 parameters.get(10).getValue(),
1493 parameters.get(11).getValue(),
1494 PositionAngleType.TRUE,
1495 closeOrbit.getFrame(),
1496 closeOrbit.getDate(),
1497 closeOrbit.getMu());
1498 Assertions.assertEquals(4.7246,
1499 Vector3D.distance(closeOrbit.getPosition(),
1500 before.getPosition()),
1501 1.0e-3);
1502 Assertions.assertEquals(0.0010514,
1503 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
1504 before.getPVCoordinates().getVelocity()),
1505 1.0e-6);
1506
1507 Orbit[] refOrbits = new Orbit[] {
1508 propagatorBuilder1.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
1509 propagatorBuilder2.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
1510 };
1511 EstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1512 refOrbits, new PositionAngleType[] { PositionAngleType.TRUE, PositionAngleType.TRUE },
1513 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
1514 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
1515 new double[][] {
1516 { 6.9e5, 6.0e5, 12.5e5 },
1517 { 6.8e5, 5.9e5, 12.7e5 }
1518 }, new double[] { 1e4, 1e4 },
1519 new double[][] {
1520 { 5.0e2, 5.3e2, 1.4e2 },
1521 { 5.0e2, 5.3e2, 1.4e2 }
1522 }, new double[] { 1.0e1, 1.0e1 });
1523
1524
1525
1526 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
1527 if (driver.getName().startsWith("a[")) {
1528
1529 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
1530 } else {
1531
1532 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
1533 }
1534 }
1535
1536 }
1537
1538 @Test
1539 public void tesIssue850() {
1540
1541 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
1542
1543
1544 final NumericalPropagatorBuilder propagatorBuilder1 =
1545 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1546 1.0e-6, 60.0, 1.0);
1547 propagatorBuilder1.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
1548
1549 final NumericalPropagatorBuilder propagatorBuilder2 =
1550 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
1551 1.0e-6, 60.0, 1.0);
1552 propagatorBuilder2.getPropagationParametersDrivers().getDrivers().forEach(driver -> driver.setSelected(true));
1553
1554
1555 final Propagator propagator1 = EstimationTestUtils.createPropagator(context.initialOrbit,
1556 propagatorBuilder1);
1557
1558 final List<ObservedMeasurement<?>> measurements1 =
1559 EstimationTestUtils.createMeasurements(propagator1,
1560 new PositionMeasurementCreator(),
1561 0.0, 3.0, 300.0);
1562
1563 final Propagator propagator2 = EstimationTestUtils.createPropagator(context.initialOrbit,
1564 propagatorBuilder2);
1565
1566 final List<ObservedMeasurement<?>> measurements2 =
1567 EstimationTestUtils.createMeasurements(propagator2,
1568 new PositionMeasurementCreator(),
1569 0.0, 3.0, 300.0);
1570
1571
1572 final List<ObservedMeasurement<?>> measurements = new ArrayList<>();
1573 measurements.add(measurements1.get(0));
1574 measurements.add(measurements2.get(0));
1575 final ObservedMeasurement<?> multiplexed = new MultiplexedMeasurement(measurements);
1576
1577
1578 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
1579 1e-2, 1e-2, 1e-2, 1e-5, 1e-5, 1e-5, 1e-8
1580 });
1581
1582
1583 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
1584 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8, 1.e-8
1585 });
1586
1587
1588
1589 final KalmanEstimator kalman = new KalmanEstimatorBuilder().
1590 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(initialP, Q)).
1591 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(initialP, Q)).
1592 build();
1593
1594
1595 Propagator[] estimated = kalman.estimationStep(multiplexed);
1596 final Vector3D pos1 = estimated[0].getInitialState().getPosition();
1597 final Vector3D pos2 = estimated[1].getInitialState().getPosition();
1598
1599
1600 Assertions.assertEquals(0.0, pos1.distance(pos2), 1.0e-12);
1601 Assertions.assertEquals(0.0, pos1.distance(context.initialOrbit.getPosition()), 1.0e-12);
1602 Assertions.assertEquals(0.0, pos2.distance(context.initialOrbit.getPosition()), 1.0e-12);
1603
1604 }
1605
1606 private static class DummyException extends OrekitException {
1607 private static final long serialVersionUID = 1L;
1608 public DummyException() {
1609 super(OrekitMessages.INTERNAL_ERROR);
1610 }
1611 }
1612 }
1613
1614