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.Comparator;
20 import java.util.List;
21
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.util.MerweUnscentedTransform;
26 import org.junit.jupiter.api.Assertions;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.bodies.GeodeticPoint;
29 import org.orekit.bodies.OneAxisEllipsoid;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.errors.OrekitMessages;
32 import org.orekit.estimation.Context;
33 import org.orekit.estimation.UnscentedEstimationTestUtils;
34 import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
35 import org.orekit.estimation.measurements.GroundStation;
36 import org.orekit.estimation.measurements.InterSatellitesRangeMeasurementCreator;
37 import org.orekit.estimation.measurements.ObservableSatellite;
38 import org.orekit.estimation.measurements.ObservedMeasurement;
39 import org.orekit.estimation.measurements.PV;
40 import org.orekit.estimation.measurements.PVMeasurementCreator;
41 import org.orekit.estimation.measurements.Position;
42 import org.orekit.estimation.measurements.Range;
43 import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
44 import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
45 import org.orekit.estimation.measurements.modifiers.Bias;
46 import org.orekit.frames.FramesFactory;
47 import org.orekit.frames.TopocentricFrame;
48 import org.orekit.orbits.CartesianOrbit;
49 import org.orekit.orbits.KeplerianOrbit;
50 import org.orekit.orbits.Orbit;
51 import org.orekit.orbits.OrbitType;
52 import org.orekit.orbits.PositionAngleType;
53 import org.orekit.propagation.BoundedPropagator;
54 import org.orekit.propagation.EphemerisGenerator;
55 import org.orekit.propagation.Propagator;
56 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
57 import org.orekit.propagation.numerical.NumericalPropagator;
58 import org.orekit.time.AbsoluteDate;
59 import org.orekit.utils.Constants;
60 import org.orekit.utils.IERSConventions;
61 import org.orekit.utils.ParameterDriver;
62 import org.orekit.utils.ParameterDriversList;
63 import org.orekit.utils.ParameterDriversList.DelegatingDriver;
64 import org.orekit.utils.TimeStampedPVCoordinates;
65
66 public class UnscentedKalmanEstimatorTest {
67
68 @Test
69 public void testMissingPropagatorBuilder() {
70 try {
71 new UnscentedKalmanEstimatorBuilder().
72 build();
73 Assertions.fail("an exception should have been thrown");
74 } catch (OrekitException oe) {
75 Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
76 }
77 }
78
79 @Test
80 public void testMissingUnscentedTransform() {
81 try {
82 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
83 final OrbitType orbitType = OrbitType.CARTESIAN;
84 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
85 final boolean perfectStart = true;
86 final double minStep = 1.e-6;
87 final double maxStep = 60.;
88 final double dP = 1.;
89 final NumericalPropagatorBuilder propagatorBuilder =
90 context.createBuilder(orbitType, positionAngleType, perfectStart,
91 minStep, maxStep, dP);
92 new UnscentedKalmanEstimatorBuilder().
93 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6))).
94 build();
95 Assertions.fail("an exception should have been thrown");
96 } catch (OrekitException oe) {
97 Assertions.assertEquals(OrekitMessages.NO_UNSCENTED_TRANSFORM_CONFIGURED, oe.getSpecifier());
98 }
99 }
100
101
102
103
104 @Test
105 public void testPV() {
106
107
108 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
109
110
111 final OrbitType orbitType = OrbitType.CARTESIAN;
112 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
113 final boolean perfectStart = true;
114 final double minStep = 1.e-6;
115 final double maxStep = 60.;
116 final double dP = 1.;
117 final NumericalPropagatorBuilder propagatorBuilder =
118 context.createBuilder(orbitType, positionAngleType, perfectStart,
119 minStep, maxStep, dP);
120
121
122 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
123 propagatorBuilder);
124 final List<ObservedMeasurement<?>> measurements =
125 UnscentedEstimationTestUtils.createMeasurements(propagator,
126 new PVMeasurementCreator(),
127 0.0, 1.0, 300.0);
128
129 final NumericalPropagator referencePropagator = propagatorBuilder.
130 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
131
132
133 final Orbit refOrbit = referencePropagator.
134 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
135
136
137 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
138
139 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
140
141
142
143 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
144 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
145 unscentedTransformProvider(new MerweUnscentedTransform(6)).
146 build();
147
148
149 final double expectedDeltaPos = 0.;
150 final double posEps = 3.63e-6;
151 final double expectedDeltaVel = 0.;
152 final double velEps = 1.42e-9;
153 final double[] expectedsigmasPos = {1.762E-7, 1.899E-7, 7.398E-7};
154 final double sigmaPosEps = 1.0e-10;
155 final double[] expectedSigmasVel = {0.90962E-10, 2.61847E-10, 0.37545E-10};
156 final double sigmaVelEps = 1.0e-15;
157 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
158 refOrbit, positionAngleType,
159 expectedDeltaPos, posEps,
160 expectedDeltaVel, velEps,
161 expectedsigmasPos, sigmaPosEps,
162 expectedSigmasVel, sigmaVelEps);
163 }
164
165
166
167
168 @Test
169 public void testShiftedPV() {
170
171
172 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
173
174
175 final OrbitType orbitType = OrbitType.CARTESIAN;
176 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
177 final boolean perfectStart = true;
178 final double minStep = 1.e-6;
179 final double maxStep = 60.;
180 final double dP = 1.;
181 final double sigmaPos = 10.;
182 final double sigmaVel = 0.01;
183
184 final NumericalPropagatorBuilder propagatorBuilder =
185 context.createBuilder(orbitType, positionAngleType, perfectStart,
186 minStep, maxStep, dP);
187
188
189 final Vector3D initialPosShifted = context.initialOrbit.getPosition().add(new Vector3D(sigmaPos, sigmaPos, sigmaPos));
190 final Vector3D initialVelShifted = context.initialOrbit.getPVCoordinates().getVelocity().add(new Vector3D(sigmaVel, sigmaVel, sigmaVel));
191
192 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(context.initialOrbit.getDate(), initialPosShifted, initialVelShifted);
193
194 final CartesianOrbit shiftedOrbit = new CartesianOrbit(pv, context.initialOrbit.getFrame(), context.initialOrbit.getMu());
195
196
197 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
198
199 final List<ObservedMeasurement<?>> measurements =
200 UnscentedEstimationTestUtils.createMeasurements(propagator,
201 new PVMeasurementCreator(),
202 0.0, 1.0, 300.0);
203
204
205 final NumericalPropagator referencePropagator = propagatorBuilder.
206 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
207
208
209 final Orbit refOrbit = referencePropagator.
210 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
211
212
213 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double[] {sigmaPos*sigmaPos, sigmaPos*sigmaPos, sigmaPos*sigmaPos, sigmaVel*sigmaVel, sigmaVel*sigmaVel, sigmaVel*sigmaVel});
214
215
216 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
217
218 propagatorBuilder.resetOrbit(shiftedOrbit);
219
220 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
221 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
222 unscentedTransformProvider(new MerweUnscentedTransform(6)).
223 build();
224
225
226 final double expectedDeltaPos = 0.;
227 final double posEps = 3.58e-3;
228 final double expectedDeltaVel = 0.;
229 final double velEps = 1.30e-6;
230 final double[] expectedsigmasPos = {0.184985, 0.167475, 0.297570};
231 final double sigmaPosEps = 1.0e-6;
232 final double[] expectedSigmasVel = {6.93330E-5, 12.37128E-5, 4.11890E-5};
233 final double sigmaVelEps = 1.0e-10;
234 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
235 refOrbit, positionAngleType,
236 expectedDeltaPos, posEps,
237 expectedDeltaVel, velEps,
238 expectedsigmasPos, sigmaPosEps,
239 expectedSigmasVel, sigmaVelEps);
240
241 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
242 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
243 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
244 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
245 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
246 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
247 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
248
249 }
250
251
252
253
254 @Test
255 public void testCartesianRange() {
256
257
258 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
259
260
261 final OrbitType orbitType = OrbitType.CARTESIAN;
262 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
263 final boolean perfectStart = true;
264 final double minStep = 1.e-6;
265 final double maxStep = 60.;
266 final double dP = 1.;
267 final NumericalPropagatorBuilder propagatorBuilder =
268 context.createBuilder(orbitType, positionAngleType, perfectStart,
269 minStep, maxStep, dP);
270
271
272 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
273 propagatorBuilder);
274 final List<ObservedMeasurement<?>> measurements =
275 UnscentedEstimationTestUtils.createMeasurements(propagator,
276 new TwoWayRangeMeasurementCreator(context),
277 0.0, 1.0, 60.0);
278
279 final NumericalPropagator referencePropagator = propagatorBuilder.
280 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
281
282
283 final Orbit refOrbit = referencePropagator.
284 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
285
286
287 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
288
289
290 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
291
292
293
294 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
295 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
296 unscentedTransformProvider(new MerweUnscentedTransform(6)).
297 build();
298
299
300 final double expectedDeltaPos = 0.;
301 final double posEps = 8.35e-7;
302 final double expectedDeltaVel = 0.;
303 final double velEps = 3.39e-10;
304 final double[] expectedsigmasPos = {0.1938703E-8, 12.7585598E-8, 17.0372647E-8};
305 final double sigmaPosEps = 1.0e-15;
306 final double[] expectedSigmasVel = {3.32084E-11, 0.3787E-11, 8.0020E-11};
307 final double sigmaVelEps = 1.0e-15;
308 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
309 refOrbit, positionAngleType,
310 expectedDeltaPos, posEps,
311 expectedDeltaVel, velEps,
312 expectedsigmasPos, sigmaPosEps,
313 expectedSigmasVel, sigmaVelEps);
314
315 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
316 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
317 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
318 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
319 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
320 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
321 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
322 }
323
324
325
326
327 @Test
328 public void testKeplerianRange() {
329
330
331 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
332
333
334 final OrbitType orbitType = OrbitType.KEPLERIAN;
335 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
336 final boolean perfectStart = true;
337 final double minStep = 1.e-6;
338 final double maxStep = 60.;
339 final double dP = 1.;
340 final NumericalPropagatorBuilder propagatorBuilder =
341 context.createBuilder(orbitType, positionAngleType, perfectStart,
342 minStep, maxStep, dP);
343
344
345 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
346 propagatorBuilder);
347 final List<ObservedMeasurement<?>> measurements =
348 UnscentedEstimationTestUtils.createMeasurements(propagator,
349 new TwoWayRangeMeasurementCreator(context),
350 0.0, 1.0, 60.0);
351
352 final NumericalPropagator referencePropagator = propagatorBuilder.
353 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
354
355
356 final Orbit refOrbit = referencePropagator.
357 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
358
359
360 final RealMatrix initialP = MatrixUtils.createRealMatrix(6, 6);
361
362
363 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
364
365
366
367 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
368 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
369 unscentedTransformProvider(new MerweUnscentedTransform(6)).
370 build();
371
372
373 final double expectedDeltaPos = 0.;
374 final double posEps = 1.74e-6;
375 final double expectedDeltaVel = 0.;
376 final double velEps = 6.06e-10;
377 final double[] expectedsigmasPos = {8.869538E-9, 1.18524507E-7, 4.32132152E-8};
378 final double sigmaPosEps = 1.0e-15;
379 final double[] expectedSigmasVel = {1.5213E-11, 7.738E-12, 4.0380E-11};
380 final double sigmaVelEps = 1.0e-15;
381 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
382 refOrbit, positionAngleType,
383 expectedDeltaPos, posEps,
384 expectedDeltaVel, velEps,
385 expectedsigmasPos, sigmaPosEps,
386 expectedSigmasVel, sigmaVelEps);
387
388 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
389 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
390 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
391 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
392 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
393 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
394 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
395 }
396
397
398
399
400
401 @Test
402 public void testCartesianRangeRate() {
403
404
405 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
406
407
408 final OrbitType orbitType = OrbitType.CARTESIAN;
409 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
410 final boolean perfectStart = true;
411 final double minStep = 1.e-6;
412 final double maxStep = 60.;
413 final double dP = 1.;
414 final NumericalPropagatorBuilder propagatorBuilder =
415 context.createBuilder(orbitType, positionAngleType, perfectStart,
416 minStep, maxStep, dP);
417
418
419 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
420 propagatorBuilder);
421 final double satClkDrift = 3.2e-10;
422 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, satClkDrift);
423 final List<ObservedMeasurement<?>> measurements =
424 UnscentedEstimationTestUtils.createMeasurements(propagator,
425 creator,
426 1.0, 3.0, 300.0);
427
428
429 final NumericalPropagator referencePropagator = propagatorBuilder.
430 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
431
432
433 final Orbit refOrbit = referencePropagator.
434 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
435
436
437
438 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
439 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
440 });
441
442
443 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
444 final double[][] dYdC = new double[6][6];
445 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
446 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
447
448
449 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
450
451
452 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
453 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
454 });
455 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
456
457
458 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
459 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
460 unscentedTransformProvider(new MerweUnscentedTransform(6)).
461 build();
462
463
464 final double expectedDeltaPos = 0.;
465 final double posEps = 5.43e-6;
466 final double expectedDeltaVel = 0.;
467 final double velEps = 1.96e-9;
468 final double[] expectedSigmasPos = {0.324407, 1.347014, 1.743326};
469 final double sigmaPosEps = 1e-6;
470 final double[] expectedSigmasVel = {2.85688e-4, 5.765933e-4, 5.056124e-4};
471 final double sigmaVelEps = 1e-10;
472 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
473 refOrbit, positionAngleType,
474 expectedDeltaPos, posEps,
475 expectedDeltaVel, velEps,
476 expectedSigmasPos, sigmaPosEps,
477 expectedSigmasVel, sigmaVelEps);
478
479 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
480 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
481 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
482 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
483 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
484 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
485 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
486 }
487
488
489
490
491 @Test
492 public void testCartesianAzimuthElevation() {
493
494
495 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
496
497
498 final OrbitType orbitType = OrbitType.CARTESIAN;
499 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
500 final boolean perfectStart = true;
501 final double minStep = 1.e-6;
502 final double maxStep = 60.;
503 final double dP = 1.;
504 final NumericalPropagatorBuilder propagatorBuilder =
505 context.createBuilder(orbitType, positionAngleType, perfectStart,
506 minStep, maxStep, dP);
507
508
509 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
510 propagatorBuilder);
511 final List<ObservedMeasurement<?>> measurements =
512 UnscentedEstimationTestUtils.createMeasurements(propagator,
513 new AngularAzElMeasurementCreator(context),
514 0.0, 1.0, 60.0);
515
516
517 final NumericalPropagator referencePropagator = propagatorBuilder.
518 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
519
520
521 final Orbit refOrbit = referencePropagator.
522 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
523
524
525 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
526 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
527 });
528
529
530 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
531 final double[][] dYdC = new double[6][6];
532 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
533 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
534
535
536 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
537
538
539
540 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
541 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
542 });
543 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
544
545
546
547 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
548 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
549 unscentedTransformProvider(new MerweUnscentedTransform(6)).
550 build();
551
552
553 final double expectedDeltaPos = 0.;
554 final double posEps = 5.96e-7;
555 final double expectedDeltaVel = 0.;
556 final double velEps = 1.76e-10;
557 final double[] expectedSigmasPos = {0.043885, 0.600764, 0.279020};
558 final double sigmaPosEps = 1.0e-6;
559 final double[] expectedSigmasVel = {7.17260E-5, 3.037315E-5, 19.49047e-5};
560 final double sigmaVelEps = 1.0e-10;
561 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
562 refOrbit, positionAngleType,
563 expectedDeltaPos, posEps,
564 expectedDeltaVel, velEps,
565 expectedSigmasPos, sigmaPosEps,
566 expectedSigmasVel, sigmaVelEps);
567
568 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
569 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
570 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
571 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
572 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
573 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
574 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
575 }
576
577
578
579
580 @Test
581 public void testCircularAzimuthElevation() {
582
583
584 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
585
586
587 final OrbitType orbitType = OrbitType.CIRCULAR;
588 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
589 final boolean perfectStart = true;
590 final double minStep = 1.e-6;
591 final double maxStep = 60.;
592 final double dP = 1.;
593 final NumericalPropagatorBuilder propagatorBuilder =
594 context.createBuilder(orbitType, positionAngleType, perfectStart,
595 minStep, maxStep, dP);
596
597
598 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
599 propagatorBuilder);
600 final List<ObservedMeasurement<?>> measurements =
601 UnscentedEstimationTestUtils.createMeasurements(propagator,
602 new AngularAzElMeasurementCreator(context),
603 0.0, 1.0, 60.0);
604
605
606 final NumericalPropagator referencePropagator = propagatorBuilder.
607 buildPropagator(propagatorBuilder.getSelectedNormalizedParameters());
608
609
610 final Orbit refOrbit = referencePropagator.
611 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
612
613
614 final RealMatrix cartesianP = MatrixUtils.createRealDiagonalMatrix(new double [] {
615 1e-4, 1e-4, 1e-4, 1e-10, 1e-10, 1e-10
616 });
617
618
619 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
620 final double[][] dYdC = new double[6][6];
621 initialOrbit.getJacobianWrtCartesian(PositionAngleType.TRUE, dYdC);
622 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
623
624
625 final RealMatrix initialP = Jac.multiply(cartesianP.multiply(Jac.transpose()));
626
627
628
629 final RealMatrix cartesianQ = MatrixUtils.createRealDiagonalMatrix(new double [] {
630 1.e-6, 1.e-6, 1.e-6, 1.e-12, 1.e-12, 1.e-12
631 });
632 final RealMatrix Q = Jac.multiply(cartesianQ.multiply(Jac.transpose()));
633
634
635
636 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
637 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
638 unscentedTransformProvider(new MerweUnscentedTransform(6)).
639 build();
640
641
642 final double expectedDeltaPos = 0.;
643 final double posEps = 6.05e-7;
644 final double expectedDeltaVel = 0.;
645 final double velEps = 2.07e-10;
646 final double[] expectedSigmasPos = {0.012134, 0.511243, 0.264925};
647 final double sigmaPosEps = 1e-6;
648 final double[] expectedSigmasVel = {5.72891E-5, 1.58811E-5, 15.98658E-5};
649 final double sigmaVelEps = 1e-10;
650 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
651 refOrbit, positionAngleType,
652 expectedDeltaPos, posEps,
653 expectedDeltaVel, velEps,
654 expectedSigmasPos, sigmaPosEps,
655 expectedSigmasVel, sigmaVelEps);
656
657 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
658 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
659 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
660 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
661 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
662 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
663 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
664 }
665
666 @Test
667 public void testMultiSat() {
668
669 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
670
671 final NumericalPropagatorBuilder propagatorBuilder1 =
672 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
673 1.0e-6, 60.0, 1.0);
674 final NumericalPropagatorBuilder propagatorBuilder2 =
675 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
676 1.0e-6, 60.0, 1.0);
677 final AbsoluteDate referenceDate = propagatorBuilder1.getInitialOrbitDate();
678
679
680 final TimeStampedPVCoordinates original = context.initialOrbit.getPVCoordinates();
681 final Orbit closeOrbit = new CartesianOrbit(new TimeStampedPVCoordinates(context.initialOrbit.getDate(),
682 original.getPosition().add(new Vector3D(1000, 2000, 3000)),
683 original.getVelocity().add(new Vector3D(-0.03, 0.01, 0.02))),
684 context.initialOrbit.getFrame(),
685 context.initialOrbit.getMu());
686 final Propagator closePropagator = UnscentedEstimationTestUtils.createPropagator(closeOrbit,
687 propagatorBuilder2);
688 final EphemerisGenerator generator = closePropagator.getEphemerisGenerator();
689 closePropagator.propagate(context.initialOrbit.getDate().shiftedBy(3.5 * closeOrbit.getKeplerianPeriod()));
690 final BoundedPropagator ephemeris = generator.getGeneratedEphemeris();
691 Propagator propagator1 = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
692 propagatorBuilder1);
693 final double localClockOffset = 0.137e-6;
694 final double remoteClockOffset = 469.0e-6;
695 final List<ObservedMeasurement<?>> measurements =
696 UnscentedEstimationTestUtils.createMeasurements(propagator1,
697 new InterSatellitesRangeMeasurementCreator(ephemeris,
698 localClockOffset,
699 remoteClockOffset),
700 1.0, 3.0, 300.0);
701
702
703 propagator1 = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
704 propagatorBuilder1);
705 measurements.addAll(UnscentedEstimationTestUtils.createMeasurements(propagator1,
706 new TwoWayRangeMeasurementCreator(context),
707 1.0, 3.0, 60.0));
708 measurements.sort(Comparator.naturalOrder());
709
710
711 final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(6, 6);
712 final UnscentedKalmanEstimator kalman = new UnscentedKalmanEstimatorBuilder().
713 unscentedTransformProvider(new MerweUnscentedTransform(12)).
714 addPropagationConfiguration(propagatorBuilder1, new ConstantProcessNoise(processNoiseMatrix)).
715 addPropagationConfiguration(propagatorBuilder2, new ConstantProcessNoise(processNoiseMatrix)).
716 build();
717
718 List<DelegatingDriver> parameters = kalman.getOrbitalParametersDrivers(true).getDrivers();
719 ParameterDriver a0Driver = parameters.get(0);
720 Assertions.assertEquals("a[0]", a0Driver.getName());
721 a0Driver.setValue(a0Driver.getValue() + 1.2);
722 a0Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
723
724 ParameterDriver a1Driver = parameters.get(6);
725 Assertions.assertEquals("a[1]", a1Driver.getName());
726 a1Driver.setValue(a1Driver.getValue() - 5.4);
727 a1Driver.setReferenceDate(AbsoluteDate.GALILEO_EPOCH);
728
729 final Orbit before = new KeplerianOrbit(parameters.get( 6).getValue(),
730 parameters.get( 7).getValue(),
731 parameters.get( 8).getValue(),
732 parameters.get( 9).getValue(),
733 parameters.get(10).getValue(),
734 parameters.get(11).getValue(),
735 PositionAngleType.TRUE,
736 closeOrbit.getFrame(),
737 closeOrbit.getDate(),
738 closeOrbit.getMu());
739 Assertions.assertEquals(4.7246,
740 Vector3D.distance(closeOrbit.getPosition(),
741 before.getPosition()),
742 1.0e-3);
743 Assertions.assertEquals(0.0010514,
744 Vector3D.distance(closeOrbit.getPVCoordinates().getVelocity(),
745 before.getPVCoordinates().getVelocity()),
746 1.0e-6);
747
748 Orbit[] refOrbits = new Orbit[] {
749 propagatorBuilder1.
750 buildPropagator(propagatorBuilder1.getSelectedNormalizedParameters()).
751 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit(),
752 propagatorBuilder2.
753 buildPropagator(propagatorBuilder2.getSelectedNormalizedParameters()).
754 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit()
755 };
756 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
757 refOrbits, new PositionAngleType[] { PositionAngleType.TRUE, PositionAngleType.TRUE },
758 new double[] { 38.3, 172.3 }, new double[] { 0.1, 0.1 },
759 new double[] { 0.015, 0.068 }, new double[] { 1.0e-3, 1.0e-3 },
760 new double[][] {
761 { 1.5e-7, 0.6e-7, 4.2e-7 },
762 { 1.5e-7, 0.5e-7, 4.2e-7 }
763 }, new double[] { 1e-8, 1e-8 },
764 new double[][] {
765 { 1.9e-11, 17.5e-11, 3.1e-11 },
766 { 2.0e-11, 17.5e-11, 2.8e-11 }
767 }, new double[] { 1.0e-12, 1.0e-12 });
768
769
770
771 for (final ParameterDriver driver : kalman.getOrbitalParametersDrivers(true).getDrivers()) {
772 if (driver.getName().startsWith("a[")) {
773
774 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(AbsoluteDate.GALILEO_EPOCH), 1.0e-15);
775 } else {
776
777 Assertions.assertEquals(0, driver.getReferenceDate().durationFrom(referenceDate), 1.0e-15);
778 }
779 }
780
781 Assertions.assertEquals(12, kalman.getOrbitalParametersDrivers(false).getNbParams());
782 Assertions.assertEquals(12, kalman.getOrbitalParametersDrivers(true).getNbParams());
783 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
784 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
785 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
786 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
787 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
788
789 }
790
791
792
793
794 @Test
795 public void testWrappedException() {
796
797
798 Context context = UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
799
800
801 final OrbitType orbitType = OrbitType.KEPLERIAN;
802 final PositionAngleType positionAngleType = PositionAngleType.TRUE;
803 final boolean perfectStart = true;
804 final double minStep = 1.e-6;
805 final double maxStep = 60.;
806 final double dP = 1.;
807 final NumericalPropagatorBuilder propagatorBuilder =
808 context.createBuilder(orbitType, positionAngleType, perfectStart,
809 minStep, maxStep, dP);
810
811
812 final Bias<Range> rangeBias = new Bias<Range>(new String[] {"rangeBias"}, new double[] {0.0},
813 new double[] {1.0},
814 new double[] {0.0}, new double[] {10000.0});
815 rangeBias.getParametersDrivers().get(0).setSelected(true);
816
817
818
819 final ParameterDriversList drivers = new ParameterDriversList();
820 drivers.add(rangeBias.getParametersDrivers().get(0));
821
822
823 final Propagator propagator = UnscentedEstimationTestUtils.createPropagator(context.initialOrbit,
824 propagatorBuilder);
825 final List<ObservedMeasurement<?>> measurements =
826 UnscentedEstimationTestUtils.createMeasurements(propagator,
827 new TwoWayRangeMeasurementCreator(context,
828 Vector3D.ZERO, null,
829 Vector3D.ZERO, null,
830 2.0),
831 1.0, 3.0, 300.0);
832
833 measurements.forEach(measurement -> ((Range) measurement).addModifier(rangeBias));
834 propagatorBuilder.getAllForceModels().forEach(force -> force.getParametersDrivers().forEach(parameter -> parameter.setSelected(true)));
835
836 final UnscentedKalmanEstimatorBuilder kalmanBuilder = new UnscentedKalmanEstimatorBuilder();
837 kalmanBuilder.unscentedTransformProvider(new MerweUnscentedTransform(8));
838 kalmanBuilder.addPropagationConfiguration(propagatorBuilder,
839 new ConstantProcessNoise(MatrixUtils.createRealMatrix(7, 7)));
840 kalmanBuilder.estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(MatrixUtils.createRealIdentityMatrix(1)));
841 final UnscentedKalmanEstimator kalman = kalmanBuilder.build();
842 kalman.setObserver(estimation -> {
843 throw new DummyException();
844 });
845
846
847 try {
848
849 UnscentedEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
850 context.initialOrbit, positionAngleType,
851 0., 0.,
852 0., 0.,
853 new double[3], 0.,
854 new double[3], 0.);
855 } catch (DummyException de) {
856
857 }
858
859 }
860
861
862
863
864
865 @Test
866 public void testIssue1034() {
867
868 UnscentedEstimationTestUtils.eccentricContext("regular-data:potential:tides");
869
870
871 final AbsoluteDate reference = AbsoluteDate.J2000_EPOCH;
872
873
874 final OneAxisEllipsoid shape = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS, Constants.WGS84_EARTH_FLATTENING, FramesFactory.getITRF(IERSConventions.IERS_2010, false));
875 final GroundStation station = new GroundStation(new TopocentricFrame(shape, new GeodeticPoint(1.44, 0.2, 100.0), "topo"));
876
877
878 final double sigmaPos = 2.0;
879 final double sigmaVel = 2.0e-3;
880 final double sigmaRange = 2.0;
881 final Position pos = new Position(reference, Vector3D.PLUS_I, sigmaPos, 1.0, new ObservableSatellite(0));
882 final PV pv = new PV(reference, Vector3D.PLUS_I, Vector3D.PLUS_J, sigmaPos, sigmaVel, 1.0, new ObservableSatellite(0));
883 final Range range = new Range(station, false, reference, 100.0, sigmaRange, 1.0, new ObservableSatellite(0));
884
885
886 final MeasurementDecorator decoratedPos = KalmanEstimatorUtil.decorateUnscented(pos, reference);
887 final MeasurementDecorator decoratedPv = KalmanEstimatorUtil.decorateUnscented(pv, reference);
888 final MeasurementDecorator decoratedRange = KalmanEstimatorUtil.decorateUnscented(range, reference);
889
890
891 for (int row = 0; row < decoratedPos.getCovariance().getRowDimension(); row++) {
892 Assertions.assertEquals(sigmaPos * sigmaPos, decoratedPos.getCovariance().getEntry(row, row));
893 }
894
895
896 for (int row = 0; row < decoratedPv.getCovariance().getRowDimension(); row++) {
897 if (row < 3) {
898 Assertions.assertEquals(sigmaPos * sigmaPos, decoratedPv.getCovariance().getEntry(row, row));
899 } else {
900 Assertions.assertEquals(sigmaVel * sigmaVel, decoratedPv.getCovariance().getEntry(row, row));
901 }
902 }
903
904
905 Assertions.assertEquals(sigmaRange * sigmaRange, decoratedRange.getCovariance().getEntry(0, 0));
906
907 }
908
909 private static class DummyException extends OrekitException {
910 private static final long serialVersionUID = 1L;
911 public DummyException() {
912 super(OrekitMessages.INTERNAL_ERROR);
913 }
914 }
915
916 }