1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.sequential;
18
19 import java.util.Arrays;
20 import java.util.Comparator;
21 import java.util.List;
22
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.linear.MatrixUtils;
25 import org.hipparchus.linear.RealMatrix;
26 import org.hipparchus.util.MerweUnscentedTransform;
27 import org.junit.jupiter.api.Assertions;
28 import org.junit.jupiter.api.Test;
29 import org.orekit.bodies.GeodeticPoint;
30 import org.orekit.bodies.OneAxisEllipsoid;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.estimation.*;
34 import org.orekit.estimation.measurements.*;
35 import org.orekit.estimation.measurements.modifiers.Bias;
36 import org.orekit.frames.FramesFactory;
37 import org.orekit.frames.TopocentricFrame;
38 import org.orekit.orbits.CartesianOrbit;
39 import org.orekit.orbits.KeplerianOrbit;
40 import org.orekit.orbits.Orbit;
41 import org.orekit.orbits.OrbitType;
42 import org.orekit.orbits.PositionAngleType;
43 import org.orekit.propagation.BoundedPropagator;
44 import org.orekit.propagation.EphemerisGenerator;
45 import org.orekit.propagation.Propagator;
46 import org.orekit.propagation.SpacecraftState;
47 import org.orekit.propagation.analytical.tle.TLE;
48 import org.orekit.propagation.analytical.tle.TLEPropagator;
49 import org.orekit.propagation.analytical.tle.generation.FixedPointTleGenerationAlgorithm;
50 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
51 import org.orekit.propagation.conversion.TLEPropagatorBuilder;
52 import org.orekit.time.AbsoluteDate;
53 import org.orekit.utils.Constants;
54 import org.orekit.utils.IERSConventions;
55 import org.orekit.utils.ParameterDriver;
56 import org.orekit.utils.ParameterDriversList;
57 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
58 import org.orekit.utils.TimeStampedPVCoordinates;
59
60 public class UnscentedKalmanEstimatorTest {
61
62 @Test
63 void testEstimationStepWithBStarOnly() {
64
65 TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
66 String line1 = "1 07276U 74026A 00055.48318287 .00000000 00000-0 22970+3 0 9994";
67 String line2 = "2 07276 71.6273 78.7838 1248323 14.0598 3.8405 4.72707036231812";
68 final TLE tle = new TLE(line1, line2);
69 final TLEPropagatorBuilder propagatorBuilder = new TLEPropagatorBuilder(tle,
70 PositionAngleType.TRUE, 1., new FixedPointTleGenerationAlgorithm());
71 for (final ParameterDriver driver: propagatorBuilder.getOrbitalParametersDrivers().getDrivers()) {
72 driver.setSelected(false);
73 }
74 propagatorBuilder.getPropagationParametersDrivers().getDrivers().get(0).setSelected(true);
75 final UnscentedKalmanEstimatorBuilder builder = new UnscentedKalmanEstimatorBuilder();
76 builder.addPropagationConfiguration(propagatorBuilder,
77 new ConstantProcessNoise(MatrixUtils.createRealMatrix(1, 1)));
78 builder.unscentedTransformProvider(new MerweUnscentedTransform(1));
79 final UnscentedKalmanEstimator estimator = builder.build();
80 final AbsoluteDate measurementDate = tle.getDate().shiftedBy(1.0);
81 final TLEPropagator propagator = TLEPropagator.selectExtrapolator(tle);
82 final Position positionMeasurement = new Position(measurementDate, propagator.getPosition(measurementDate,
83 propagator.getFrame()), 1., 1., new ObservableSatellite(0));
84
85 Assertions.assertDoesNotThrow(() -> estimator.estimationStep(positionMeasurement));
86 }
87
88 @Test
89 public void testTwoOrbitalParameters() {
90
91
92 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
93
94
95 final OrbitType orbitType = OrbitType.KEPLERIAN;
96 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
97 final boolean perfectStart = true;
98 final double minStep = 1.e-6;
99 final double maxStep = 60.;
100 final double dP = 1.;
101 final NumericalPropagatorBuilder propagatorBuilder =
102 context.createBuilder(orbitType, positionAngleType, perfectStart,
103 minStep, maxStep, dP);
104
105
106 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
107 propagatorBuilder);
108 final AbsoluteDate measurementDate = context.initialOrbit.getDate().shiftedBy(600.0);
109 final SpacecraftState state = propagator.propagate(measurementDate);
110 final ObservedMeasurement<?> measurement = new PV(measurementDate,
111 state.getPosition().add(new Vector3D(10.0, -10.0, 5.0)),
112 state.getPVCoordinates().getVelocity().add(new Vector3D(-10.0, 5.0, -5.0)),
113 5.0, 5.0, 1.0, new ObservableSatellite(0));
114
115
116 propagatorBuilder.getOrbitalParametersDrivers().getDrivers()
117 .forEach(driver -> driver.setSelected(false));
118
119
120 propagatorBuilder.getOrbitalParametersDrivers().findByName("e").setSelected(true);
121 propagatorBuilder.getOrbitalParametersDrivers().findByName("v").setSelected(true);
122
123
124 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[]{
125 1e-2, 1e-5
126 });
127
128
129 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double[]{
130 1.e-8, 1.e-8
131 });
132
133
134 final UnscentedKalmanEstimator unscented = new UnscentedKalmanEstimatorBuilder()
135 .addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q))
136 .unscentedTransformProvider(new MerweUnscentedTransform(2))
137 .build();
138
139
140 unscented.estimationStep(measurement);
141
142
143 final KeplerianOrbit initialOrbit = (KeplerianOrbit) context.initialOrbit;
144 Assertions.assertEquals(initialOrbit.getA(),
145 propagatorBuilder.getOrbitalParametersDrivers().findByName("a").getValue(), 1e-8);
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(), 1e-15);
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 RealMatrix Q1 = MatrixUtils.createRealDiagonalMatrix(new double[]{
239 1e-8, 1e-8
240 });
241 RealMatrix Q2 = MatrixUtils.createRealDiagonalMatrix(new double[]{
242 1e-8, 1e-8, 1e-8
243 });
244
245
246 final UnscentedKalmanEstimator unscented = new UnscentedKalmanEstimatorBuilder()
247 .addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(initialP1, Q1))
248 .addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(initialP2, Q2))
249 .unscentedTransformProvider(new MerweUnscentedTransform(5))
250 .build();
251
252
253 unscented.estimationStep(combinedMeasurement);
254
255
256 Assertions.assertEquals(refOrbit1.getA(),
257 propagatorBuilder1.getOrbitalParametersDrivers().findByName("a[0]").getValue(), 1e-8);
258 Assertions.assertEquals(refOrbit1.getI(),
259 propagatorBuilder1.getOrbitalParametersDrivers().findByName("i[0]").getValue());
260 Assertions.assertEquals(refOrbit1.getRightAscensionOfAscendingNode(),
261 propagatorBuilder1.getOrbitalParametersDrivers().findByName("Ω[0]").getValue());
262 Assertions.assertEquals(refOrbit1.getPerigeeArgument(),
263 propagatorBuilder1.getOrbitalParametersDrivers().findByName("ω[0]").getValue(), 1e-15);
264
265 Assertions.assertEquals(refOrbit2.getA(),
266 propagatorBuilder2.getOrbitalParametersDrivers().findByName("a[1]").getValue(), 1e-8);
267 Assertions.assertEquals(refOrbit2.getI(),
268 propagatorBuilder2.getOrbitalParametersDrivers().findByName("i[1]").getValue());
269 Assertions.assertEquals(refOrbit2.getRightAscensionOfAscendingNode(),
270 propagatorBuilder2.getOrbitalParametersDrivers().findByName("Ω[1]").getValue());
271 Assertions.assertEquals(refOrbit2.getPerigeeArgument(),
272 propagatorBuilder2.getOrbitalParametersDrivers().findByName("ω[1]").getValue(), 1e-15);
273
274
275 Assertions.assertNotEquals(refOrbit1.getE(),
276 propagatorBuilder1.getOrbitalParametersDrivers().findByName("e[0]").getValue());
277 Assertions.assertNotEquals(refOrbit1.getTrueAnomaly(),
278 propagatorBuilder1.getOrbitalParametersDrivers().findByName("v[0]").getValue());
279
280 Assertions.assertNotEquals(refOrbit2.getE(),
281 propagatorBuilder2.getOrbitalParametersDrivers().findByName("e[1]").getValue());
282 Assertions.assertNotEquals(refOrbit2.getTrueAnomaly(),
283 propagatorBuilder2.getOrbitalParametersDrivers().findByName("v[1]").getValue());
284
285
286 final List<DelegatingDriver> drivers1 = propagatorBuilder1.getPropagationParametersDrivers().getDrivers();
287 for (int i = 0; i < parameterValues1.length; ++i) {
288 double postEstimation = drivers1.get(i).getValue();
289 Assertions.assertEquals(parameterValues1[i], postEstimation);
290 }
291
292 final List<DelegatingDriver> drivers2 = propagatorBuilder2.getPropagationParametersDrivers().getDrivers();
293 for (int i = 0; i < parameterValues2.length; ++i) {
294 double postEstimation = drivers2.get(i).getValue();
295 if (drivers2.get(i).getName().equals("reflection coefficient")) {
296 Assertions.assertNotEquals(parameterValues2[i], postEstimation);
297 } else {
298 Assertions.assertEquals(parameterValues2[i], postEstimation);
299 }
300 }
301 }
302
303 @Test
304 public void testMissingPropagatorBuilder() {
305 try {
306 new UnscentedKalmanEstimatorBuilder().
307 build();
308 Assertions.fail("an exception should have been thrown");
309 } catch (OrekitException oe) {
310 Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
311 }
312 }
313
314 @Test
315 public void testMissingUnscentedTransform() {
316 try {
317 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
318 final OrbitType orbitType = OrbitType.CARTESIAN;
319 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
320 final boolean perfectStart = true;
321 final double minStep = 1.e-6;
322 final double maxStep = 60.;
323 final double dP = 1.;
324 final NumericalPropagatorBuilder propagatorBuilder =
325 context.createBuilder(orbitType, positionAngleType, perfectStart,
326 minStep, maxStep, dP);
327 new UnscentedKalmanEstimatorBuilder().
328 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6))).
329 build();
330 Assertions.fail("an exception should have been thrown");
331 } catch (OrekitException oe) {
332 Assertions.assertEquals(OrekitMessages.NO_UNSCENTED_TRANSFORM_CONFIGURED, oe.getSpecifier());
333 }
334 }
335
336
337
338
339 @Test
340 public void testPV() {
341
342
343 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
344
345
346 final OrbitType orbitType = OrbitType.CARTESIAN;
347 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
348 final boolean perfectStart = true;
349 final double minStep = 1.e-6;
350 final double maxStep = 60.;
351 final double dP = 1.;
352 final NumericalPropagatorBuilder propagatorBuilder =
353 context.createBuilder(orbitType, positionAngleType, perfectStart,
354 minStep, maxStep, dP);
355
356
357 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
358 propagatorBuilder);
359 final List<ObservedMeasurement<?>> measurements =
360 UnscentedEstimationTestUtils.createMeasurements(propagator,
361 new PVMeasurementCreator(),
362 0.0, 1.0, 300.0);
363
364 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
365
366
367 final Orbit refOrbit = referencePropagator.
368 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
369
370
371 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
372
373 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
374
375
376
377 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
378 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
379 unscentedTransformProvider(new MerweUnscentedTransform(6)).
380 build();
381
382
383 final double expectedDeltaPos = 0.;
384 final double posEps = 2.7e-7;
385 final double expectedDeltaVel = 0.;
386 final double velEps = 9.7e-11;
387 final double[] expectedsigmasPos = {0.0, 0.0, 0.0};
388 final double sigmaPosEps = 1.0e-10;
389 final double[] expectedSigmasVel = {0.0, 0.0, 0.0};
390 final double sigmaVelEps = 1.0e-15;
391 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
392 refOrbit, positionAngleType,
393 expectedDeltaPos, posEps,
394 expectedDeltaVel, velEps,
395 expectedsigmasPos, sigmaPosEps,
396 expectedSigmasVel, sigmaVelEps);
397 }
398
399
400
401
402 @Test
403 public void testShiftedPV() {
404
405
406 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
407
408
409 final OrbitType orbitType = OrbitType.CARTESIAN;
410 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
411 final boolean perfectStart = true;
412 final double minStep = 1.e-6;
413 final double maxStep = 60.;
414 final double dP = 1.;
415 final double sigmaPos = 10.;
416 final double sigmaVel = 0.01;
417
418 final NumericalPropagatorBuilder propagatorBuilder =
419 context.createBuilder(orbitType, positionAngleType, perfectStart,
420 minStep, maxStep, dP);
421
422
423 final Vector3D initialPosShifted = context.initialOrbit.getPosition().add(new Vector3D(sigmaPos, sigmaPos, sigmaPos));
424 final Vector3D initialVelShifted = context.initialOrbit.getPVCoordinates().getVelocity().add(new Vector3D(sigmaVel, sigmaVel, sigmaVel));
425
426 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(context.initialOrbit.getDate(), initialPosShifted, initialVelShifted);
427
428 final CartesianOrbit shiftedOrbit = new CartesianOrbit(pv, context.initialOrbit.getFrame(), context.initialOrbit.getMu());
429
430
431 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
432
433 final List<ObservedMeasurement<?>> measurements =
434 UnscentedEstimationTestUtils.createMeasurements(propagator,
435 new PVMeasurementCreator(),
436 0.0, 1.0, 300.0);
437
438
439 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
440
441
442 final Orbit refOrbit = referencePropagator.
443 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
444
445
446 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[] {sigmaPos*sigmaPos, sigmaPos*sigmaPos, sigmaPos*sigmaPos, sigmaVel*sigmaVel, sigmaVel*sigmaVel, sigmaVel*sigmaVel});
447
448
449 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
450
451 propagatorBuilder.resetOrbit(shiftedOrbit);
452
453 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
454 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
455 unscentedTransformProvider(new MerweUnscentedTransform(6)).
456 build();
457
458
459 final double expectedDeltaPos = 0.;
460 final double posEps = 3.58e-3;
461 final double expectedDeltaVel = 0.;
462 final double velEps = 1.30e-6;
463 final double[] expectedsigmasPos = {0.184985, 0.167475, 0.297570};
464 final double sigmaPosEps = 1.0e-6;
465 final double[] expectedSigmasVel = {6.93330E-5, 12.37128E-5, 4.11890E-5};
466 final double sigmaVelEps = 1.0e-10;
467 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
468 refOrbit, positionAngleType,
469 expectedDeltaPos, posEps,
470 expectedDeltaVel, velEps,
471 expectedsigmasPos, sigmaPosEps,
472 expectedSigmasVel, sigmaVelEps);
473
474 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
475 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
476 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
477 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
478 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
479 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
480 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
481
482 }
483
484
485
486
487 @Test
488 public void testCartesianRange() {
489
490
491 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
492
493
494 final OrbitType orbitType = OrbitType.CARTESIAN;
495 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
496 final boolean perfectStart = true;
497 final double minStep = 1.e-6;
498 final double maxStep = 60.;
499 final double dP = 1.;
500 final NumericalPropagatorBuilder propagatorBuilder =
501 context.createBuilder(orbitType, positionAngleType, perfectStart,
502 minStep, maxStep, dP);
503
504
505 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
506 propagatorBuilder);
507 final List<ObservedMeasurement<?>> measurements =
508 UnscentedEstimationTestUtils.createMeasurements(propagator,
509 new TwoWayRangeMeasurementCreator(context),
510 0.0, 1.0, 60.0);
511
512 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
513
514
515 final Orbit refOrbit = referencePropagator.
516 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
517
518
519 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
520
521
522 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
523
524
525
526 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
527 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
528 unscentedTransformProvider(new MerweUnscentedTransform(6)).
529 build();
530
531
532 final double expectedDeltaPos = 0.;
533 final double posEps = 6.2e-8;
534 final double expectedDeltaVel = 0.;
535 final double velEps = 2.7e-11;
536 final double[] expectedsigmasPos = {0.0, 0.0, 0.0};
537 final double sigmaPosEps = 1.0e-15;
538 final double[] expectedSigmasVel = {0.0, 0.0, 0.0};
539 final double sigmaVelEps = 1.0e-15;
540 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
541 refOrbit, positionAngleType,
542 expectedDeltaPos, posEps,
543 expectedDeltaVel, velEps,
544 expectedsigmasPos, sigmaPosEps,
545 expectedSigmasVel, sigmaVelEps);
546
547 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
548 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
549 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
550 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
551 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
552 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
553 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
554 }
555
556
557
558
559 @Test
560 public void testKeplerianRange() {
561
562
563 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
564
565
566 final OrbitType orbitType = OrbitType.KEPLERIAN;
567 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
568 final boolean perfectStart = true;
569 final double minStep = 1.e-6;
570 final double maxStep = 60.;
571 final double dP = 1.;
572 final NumericalPropagatorBuilder propagatorBuilder =
573 context.createBuilder(orbitType, positionAngleType, perfectStart,
574 minStep, maxStep, dP);
575
576
577 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
578 propagatorBuilder);
579 final List<ObservedMeasurement<?>> measurements =
580 UnscentedEstimationTestUtils.createMeasurements(propagator,
581 new TwoWayRangeMeasurementCreator(context),
582 0.0, 1.0, 60.0);
583
584 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
585
586
587 final Orbit refOrbit = referencePropagator.
588 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
589
590
591 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
592
593
594 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
595
596
597
598 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
599 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
600 unscentedTransformProvider(new MerweUnscentedTransform(6)).
601 build();
602
603
604 final double expectedDeltaPos = 0.;
605 final double posEps = 4.0e-8;
606 final double expectedDeltaVel = 0.;
607 final double velEps = 1.4e-11;
608 final double[] expectedsigmasPos = {0.0, 0.0, 0.0};
609 final double sigmaPosEps = 1.0e-15;
610 final double[] expectedSigmasVel = {0.0, 0.0, 0.0};
611 final double sigmaVelEps = 1.0e-15;
612 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
613 refOrbit, positionAngleType,
614 expectedDeltaPos, posEps,
615 expectedDeltaVel, velEps,
616 expectedsigmasPos, sigmaPosEps,
617 expectedSigmasVel, sigmaVelEps);
618
619 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
620 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
621 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
622 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
623 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
624 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
625 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
626 }
627
628
629
630
631
632 @Test
633 public void testCartesianRangeRate() {
634
635
636 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
637
638
639 final OrbitType orbitType = OrbitType.CARTESIAN;
640 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
641 final boolean perfectStart = true;
642 final double minStep = 1.e-6;
643 final double maxStep = 60.;
644 final double dP = 1.;
645 final NumericalPropagatorBuilder propagatorBuilder =
646 context.createBuilder(orbitType, positionAngleType, perfectStart,
647 minStep, maxStep, dP);
648
649
650 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
651 propagatorBuilder);
652 final double satClkDrift = 3.2e-10;
653 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
654 final List<ObservedMeasurement<?>> measurements =
655 UnscentedEstimationTestUtils.createMeasurements(propagator,
656 creator,
657 1.0, 3.0, 300.0);
658
659
660 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
661
662
663 final Orbit refOrbit = referencePropagator.
664 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
665
666
667
668 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
669 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
670 });
671
672
673 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
674 final double[][] dYdC = new double[6][6];
675 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
676 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
677
678
679 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
680
681
682 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
683 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
684 });
685 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
686
687
688 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
689 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
690 unscentedTransformProvider(new MerweUnscentedTransform(6)).
691 build();
692
693
694 final double expectedDeltaPos = 0.;
695 final double posEps = 2.0e-6;
696 final double expectedDeltaVel = 0.;
697 final double velEps = 7.3e-10;
698 final double[] expectedSigmasPos = {0.324407, 1.347014, 1.743326};
699 final double sigmaPosEps = 1e-6;
700 final double[] expectedSigmasVel = {2.85688e-4, 5.765934e-4, 5.056124e-4};
701 final double sigmaVelEps = 1e-10;
702 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
703 refOrbit, positionAngleType,
704 expectedDeltaPos, posEps,
705 expectedDeltaVel, velEps,
706 expectedSigmasPos, sigmaPosEps,
707 expectedSigmasVel, sigmaVelEps);
708
709 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
710 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
711 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
712 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
713 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
714 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
715 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
716 }
717
718
719
720
721 @Test
722 public void testCartesianAzimuthElevation() {
723
724
725 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
726
727
728 final OrbitType orbitType = OrbitType.CARTESIAN;
729 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
730 final boolean perfectStart = true;
731 final double minStep = 1.e-6;
732 final double maxStep = 60.;
733 final double dP = 1.;
734 final NumericalPropagatorBuilder propagatorBuilder =
735 context.createBuilder(orbitType, positionAngleType, perfectStart,
736 minStep, maxStep, dP);
737
738
739 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
740 propagatorBuilder);
741 final List<ObservedMeasurement<?>> measurements =
742 UnscentedEstimationTestUtils.createMeasurements(propagator,
743 new AngularAzElMeasurementCreator(context),
744 0.0, 1.0, 60.0);
745
746
747 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
748
749
750 final Orbit refOrbit = referencePropagator.
751 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
752
753
754 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
755 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
756 });
757
758
759 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
760 final double[][] dYdC = new double[6][6];
761 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
762 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
763
764
765 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
766
767
768
769 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
770 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
771 });
772 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
773
774
775
776 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
777 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
778 unscentedTransformProvider(new MerweUnscentedTransform(6)).
779 build();
780
781
782 final double expectedDeltaPos = 0.;
783 final double posEps = 5.96e-7;
784 final double expectedDeltaVel = 0.;
785 final double velEps = 1.76e-10;
786 final double[] expectedSigmasPos = {0.043885, 0.600764, 0.279020};
787 final double sigmaPosEps = 1.0e-6;
788 final double[] expectedSigmasVel = {7.17260E-5, 3.037315E-5, 19.49047e-5};
789 final double sigmaVelEps = 1.0e-10;
790 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
791 refOrbit, positionAngleType,
792 expectedDeltaPos, posEps,
793 expectedDeltaVel, velEps,
794 expectedSigmasPos, sigmaPosEps,
795 expectedSigmasVel, sigmaVelEps);
796
797 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
798 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
799 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
800 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
801 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
802 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
803 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
804 }
805
806
807
808
809 @Test
810 public void testCircularAzimuthElevation() {
811
812
813 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
814
815
816 final OrbitType orbitType = OrbitType.CIRCULAR;
817 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
818 final boolean perfectStart = true;
819 final double minStep = 1.e-6;
820 final double maxStep = 60.;
821 final double dP = 1.;
822 final NumericalPropagatorBuilder propagatorBuilder =
823 context.createBuilder(orbitType, positionAngleType, perfectStart,
824 minStep, maxStep, dP);
825
826
827 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
828 propagatorBuilder);
829 final List<ObservedMeasurement<?>> measurements =
830 UnscentedEstimationTestUtils.createMeasurements(propagator,
831 new AngularAzElMeasurementCreator(context),
832 0.0, 1.0, 60.0);
833
834
835 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
836
837
838 final Orbit refOrbit = referencePropagator.
839 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
840
841
842 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
843 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
844 });
845
846
847 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
848 final double[][] dYdC = new double[6][6];
849 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
850 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
851
852
853 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
854
855
856
857 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
858 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
859 });
860 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
861
862
863
864 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
865 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
866 unscentedTransformProvider(new MerweUnscentedTransform(6)).
867 build();
868
869
870 final double expectedDeltaPos = 0.;
871 final double posEps = 1.5e-7;
872 final double expectedDeltaVel = 0.;
873 final double velEps = 5.2e-11;
874 final double[] expectedSigmasPos = {0.045182, 0.615288, 0.295163};
875 final double sigmaPosEps = 1e-6;
876 final double[] expectedSigmasVel = {7.25356E-5, 3.11525E-5, 19.81870E-5};
877 final double sigmaVelEps = 1e-10;
878 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
879 refOrbit, positionAngleType,
880 expectedDeltaPos, posEps,
881 expectedDeltaVel, velEps,
882 expectedSigmasPos, sigmaPosEps,
883 expectedSigmasVel, sigmaVelEps);
884
885 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
886 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
887 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
888 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
889 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
890 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
891 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
892 }
893
894 @Test
895 public void testMultiSat() {
896
897 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
898
899 final NumericalPropagatorBuilder propagatorBuilder1 =
900 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
901 1.0e-6, 60.0, 1.0);
902 final NumericalPropagatorBuilder propagatorBuilder2 =
903 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
904 1.0e-6, 60.0, 1.0);
905 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
906
907
908 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
909 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
910 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
911 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
912 context.initialOrbit.getFrame(),
913 context.initialOrbit.getMu());
914 final Propagator closePropagator = UnscentedEstimationTestUtils.createPropagator(closeOrbit,
915 propagatorBuilder2);
916 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
917 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
918 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
919 Propagator propagator1 = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
920 propagatorBuilder1);
921 final double localClockOffset = 0.137e-6;
922 final double remoteClockOffset = 469.0e-6;
923 final List<ObservedMeasurement<?>> measurements =
924 UnscentedEstimationTestUtils.createMeasurements(propagator1,
925 new InterSatellitesRangeMeasurementCreator(ephemeris,
926 localClockOffset,
927 remoteClockOffset),
928 1.0, 3.0, 300.0);
929
930
931 propagator1 = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
932 propagatorBuilder1);
933 measurements.addAll(UnscentedEstimationTestUtils.createMeasurements(propagator1,
934 new TwoWayRangeMeasurementCreator(context),
935 1.0, 3.0, 60.0));
936 measurements.sort(Comparator.naturalOrder());
937
938
939 final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(6, 6);
940 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
941 unscentedTransformProvider(new MerweUnscentedTransform(12)).
942 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
943 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
944 build();
945
946 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
947 ParameterDriver a0Driver = parameters.get(0);
948 Assertions.assertEquals("a[0]", a0Driver.getName());
949 a0Driver.setValue(a0Driver.getValue() + 1.2);
950 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
951
952 ParameterDriver a1Driver = parameters.get(6);
953 Assertions.assertEquals("a[1]", a1Driver.getName());
954 a1Driver.setValue(a1Driver.getValue() - 5.4);
955 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
956
957 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
958 parameters.get( 7).getValue(),
959 parameters.get( 8).getValue(),
960 parameters.get( 9).getValue(),
961 parameters.get(10).getValue(),
962 parameters.get(11).getValue(),
963 PositionAngleType.TRUE,
964 closeOrbit.getFrame(),
965 closeOrbit.getDate(),
966 closeOrbit.getMu());
967 Assertions.assertEquals(4.7246,
968 Vector3D.distance(closeOrbit.getPosition(),
969 before.getPosition()),
970 1.0e-3);
971 Assertions.assertEquals(0.0010514,
972 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
973 before.getPVCoordinates().getVelocity()),
974 1.0e-6);
975
976 Orbit[] refOrbits = new Orbit[] {
977 propagatorBuilder1.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
978 propagatorBuilder2.buildPropagator().propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
979 };
980 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
981 refOrbits, new PositionAngleType[] { PositionAngleType.TRUE, PositionAngleType.TRUE },
982 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
983 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
984 new double[][] {
985 { 0.0, 0.0, 0.0 },
986 { 0.0, 0.0, 0.0 }
987 }, new double[] { 1e-8, 1e-8 },
988 new double[][] {
989 { 0.0, 0.0, 0.0 },
990 { 0.0, 0.0, 0.0 }
991 }, new double[] { 1.0e-12, 1.0e-12 });
992
993
994
995 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
996 if (driver.getName().startsWith("a[")) {
997
998 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
999 } else {
1000
1001 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
1002 }
1003 }
1004
1005 Assertions.assertEquals(12, kalman.getOrbitalParametersDrivers(false).getNbParams());
1006 Assertions.assertEquals(12, kalman.getOrbitalParametersDrivers(true).getNbParams());
1007 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
1008 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
1009 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
1010 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
1011 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
1012
1013 }
1014
1015
1016
1017
1018 @Test
1019 public void testWrappedException() {
1020
1021
1022 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
1023
1024
1025 final OrbitType orbitType = OrbitType.KEPLERIAN;
1026 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
1027 final boolean perfectStart = true;
1028 final double minStep = 1.e-6;
1029 final double maxStep = 60.;
1030 final double dP = 1.;
1031 final NumericalPropagatorBuilder propagatorBuilder =
1032 context.createBuilder(orbitType, positionAngleType, perfectStart,
1033 minStep, maxStep, dP);
1034
1035
1036 final Bias<Range> rangeBias = new Bias<>(new String[] {"rangeBias"}, new double[] {0.0},
1037 new double[] {1.0},
1038 new double[] {0.0}, new double[] {10000.0});
1039 rangeBias.getParametersDrivers().get(0).setSelected(true);
1040
1041
1042
1043 final ParameterDriversList drivers = new ParameterDriversList();
1044 drivers.add(rangeBias.getParametersDrivers().get(0));
1045
1046
1047 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
1048 propagatorBuilder);
1049 final List<ObservedMeasurement<?>> measurements =
1050 UnscentedEstimationTestUtils.createMeasurements(propagator,
1051 new TwoWayRangeMeasurementCreator(context,
1052 Vector3D.ZERO, null,
1053 Vector3D.ZERO, null,
1054 2.0),
1055 1.0, 3.0, 300.0);
1056
1057 measurements.forEach(measurement -> ((Range) measurement).addModifier(rangeBias));
1058 propagatorBuilder.getAllForceModels().forEach(force -> force.getParametersDrivers().forEach(parameter -> parameter.setSelected(true)));
1059
1060 final UnscentedKalmanEstimatorBuilder kalmanBuilder = new UnscentedKalmanEstimatorBuilder();
1061 kalmanBuilder.unscentedTransformProvider(new MerweUnscentedTransform(8));
1062 kalmanBuilder.addPropagationConfiguration(propagatorBuilder,
1063 new ConstantProcessNoise(MatrixUtils.createRealMatrix(7, 7)));
1064 kalmanBuilder.estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(MatrixUtils.createRealIdentityMatrix(1)));
1065 final UnscentedKalmanEstimator kalman = kalmanBuilder.build();
1066 kalman.setObserver(estimation -> {
1067 throw new DummyException();
1068 });
1069
1070
1071 try {
1072
1073 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
1074 context.initialOrbit, positionAngleType,
1075 0., 0.,
1076 0., 0.,
1077 new double[3], 0.,
1078 new double[3], 0.);
1079 } catch (DummyException de) {
1080
1081 }
1082
1083 }
1084
1085
1086
1087
1088
1089 @Test
1090 public void testIssue1034() {
1091
1092 UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
1093
1094
1095 final AbsoluteDate reference = AbsoluteDate.J2000_EPOCH;
1096
1097
1098 final OneAxisEllipsoid shape = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, false));
1099 final GroundStation station = new GroundStation(new TopocentricFrame(shape, new GeodeticPoint(1.44, 0.2, 100.0), "topo"));
1100
1101
1102 final double sigmaPos = 2.0;
1103 final double sigmaVel = 2.0e-3;
1104 final double sigmaRange = 2.0;
1105 final Position pos = new Position(reference, Vector3D.PLUS_I, sigmaPos, 1.0, new ObservableSatellite(0));
1106 final PV pv = new PV(reference, Vector3D.PLUS_I, Vector3D.PLUS_J, sigmaPos, sigmaVel, 1.0, new ObservableSatellite(0));
1107 final Range range = new Range(station, false, reference, 100.0, sigmaRange, 1.0, new ObservableSatellite(0));
1108
1109
1110 final MeasurementDecorator decoratedPos = KalmanEstimatorUtil.decorateUnscented(pos, reference);
1111 final MeasurementDecorator decoratedPv = KalmanEstimatorUtil.decorateUnscented(pv, reference);
1112 final MeasurementDecorator decoratedRange = KalmanEstimatorUtil.decorateUnscented(range, reference);
1113
1114
1115 for (int row = 0; row < decoratedPos.getCovariance().getRowDimension(); row++) {
1116 Assertions.assertEquals(sigmaPos * sigmaPos, decoratedPos.getCovariance().getEntry(row, row));
1117 }
1118
1119
1120 for (int row = 0; row < decoratedPv.getCovariance().getRowDimension(); row++) {
1121 if (row < 3) {
1122 Assertions.assertEquals(sigmaPos * sigmaPos, decoratedPv.getCovariance().getEntry(row, row));
1123 } else {
1124 Assertions.assertEquals(sigmaVel * sigmaVel, decoratedPv.getCovariance().getEntry(row, row));
1125 }
1126 }
1127
1128
1129 Assertions.assertEquals(sigmaRange * sigmaRange, decoratedRange.getCovariance().getEntry(0, 0));
1130
1131 }
1132
1133
1134
1135
1136 @Test
1137 public void testProcessNoiseStates() {
1138
1139
1140 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
1141
1142
1143 final OrbitType orbitType = OrbitType.CARTESIAN;
1144 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
1145 final boolean perfectStart = true;
1146 final double minStep = 1.e-6;
1147 final double maxStep = 60.;
1148 final double dP = 1.;
1149 final NumericalPropagatorBuilder propagatorBuilder =
1150 context.createBuilder(orbitType, positionAngleType, perfectStart,
1151 minStep, maxStep, dP);
1152
1153
1154 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
1155 propagatorBuilder);
1156 final List<ObservedMeasurement<?>> measurements =
1157 UnscentedEstimationTestUtils.createMeasurements(propagator,
1158 new PVMeasurementCreator(),
1159 0.0, 1.0, 300.0);
1160
1161
1162 ProcessNoise processNoise = new ProcessNoise();
1163
1164
1165 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
1166 addPropagationConfiguration(propagatorBuilder, processNoise).
1167 unscentedTransformProvider(new MerweUnscentedTransform(6)).
1168 build();
1169
1170
1171 kalman.estimationStep(measurements.get(10));
1172
1173
1174 Assertions.assertEquals(measurements.get(0).getDate(), processNoise.getPrevious().getDate());
1175 Assertions.assertEquals(measurements.get(10).getDate(), processNoise.getCurrent().getDate());
1176 Assertions.assertNotEquals(processNoise.getPrevious().getPosition().getX(),
1177 processNoise.getCurrent().getPosition().getX());
1178 }
1179
1180 private static class DummyException extends OrekitException {
1181 private static final long serialVersionUID = 1L;
1182 public DummyException() {
1183 super(OrekitMessages.INTERNAL_ERROR);
1184 }
1185 }
1186
1187
1188 private static class ProcessNoise implements CovarianceMatrixProvider {
1189
1190 private SpacecraftState previous;
1191 private SpacecraftState current;
1192
1193 public SpacecraftState getPrevious() {
1194 return previous;
1195 }
1196
1197 public SpacecraftState getCurrent() {
1198 return current;
1199 }
1200
1201 @Override
1202 public RealMatrix getInitialCovarianceMatrix(SpacecraftState initial) {
1203 return MatrixUtils.createRealMatrix(6, 6);
1204 }
1205
1206 @Override
1207 public RealMatrix getProcessNoiseMatrix(SpacecraftState previous, SpacecraftState current) {
1208 this.previous = previous;
1209 this.current = current;
1210 return MatrixUtils.createRealMatrix(6, 6);
1211 }
1212 }
1213
1214 }