1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.orekit.estimation.sequential;
19
20 import java.util.List;
21
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.linear.RealMatrix;
24 import org.hipparchus.stat.descriptive.StreamingStatistics;
25 import org.hipparchus.util.FastMath;
26 import org.hipparchus.util.MerweUnscentedTransform;
27 import org.junit.jupiter.api.Assertions;
28 import org.junit.jupiter.api.Test;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.errors.OrekitMessages;
31 import org.orekit.estimation.DSSTContext;
32 import org.orekit.estimation.DSSTEstimationTestUtils;
33 import org.orekit.estimation.measurements.EstimatedMeasurement;
34 import org.orekit.estimation.measurements.ObservedMeasurement;
35 import org.orekit.estimation.measurements.Range;
36 import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
37 import org.orekit.forces.gravity.potential.GravityFieldFactory;
38 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
39 import org.orekit.orbits.Orbit;
40 import org.orekit.orbits.OrbitType;
41 import org.orekit.orbits.PositionAngleType;
42 import org.orekit.propagation.PropagationType;
43 import org.orekit.propagation.Propagator;
44 import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
45 import org.orekit.propagation.semianalytical.dsst.forces.DSSTTesseral;
46 import org.orekit.propagation.semianalytical.dsst.forces.DSSTZonal;
47 import org.orekit.time.AbsoluteDate;
48 import org.orekit.utils.Constants;
49
50 public class SemiAnalyticalUnscentedKalmanEstimatorTest {
51
52
53 public static class Observer implements KalmanObserver {
54
55
56 private StreamingStatistics stats;
57
58
59 public Observer() {
60 this.stats = new StreamingStatistics();
61 }
62
63
64 @Override
65 public void evaluationPerformed(final KalmanEstimation estimation) {
66
67
68 final EstimatedMeasurement<?> estimatedMeasurement = estimation.getPredictedMeasurement();
69
70
71 if (estimatedMeasurement.getObservedMeasurement() instanceof Range) {
72 final double[] estimated = estimatedMeasurement.getEstimatedValue();
73 final double[] observed = estimatedMeasurement.getObservedValue();
74
75 final double res = observed[0] - estimated[0];
76 stats.addValue(res);
77 }
78
79 }
80
81
82
83
84 public double getMeanResidual() {
85 return stats.getMean();
86 }
87
88 }
89
90 @Test
91 public void testMissingPropagatorBuilder() {
92 try {
93 new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
94 build();
95 Assertions.fail("an exception should have been thrown");
96 } catch (OrekitException oe) {
97 Assertions.assertEquals(OrekitMessages.NO_PROPAGATOR_CONFIGURED, oe.getSpecifier());
98 }
99 }
100
101 @Test
102 public void testMissingUnscentedTransform() {
103 try {
104 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
105 final boolean perfectStart = true;
106 final double minStep = 1.e-6;
107 final double maxStep = 60.;
108 final double dP = 1.;
109 final DSSTPropagatorBuilder propagatorBuilder =
110 context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart,
111 minStep, maxStep, dP);
112 new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
113 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(MatrixUtils.createRealMatrix(6, 6))).
114 build();
115 Assertions.fail("an exception should have been thrown");
116 } catch (OrekitException oe) {
117 Assertions.assertEquals(OrekitMessages.NO_UNSCENTED_TRANSFORM_CONFIGURED, oe.getSpecifier());
118 }
119 }
120
121 @Test
122 public void testKeplerianRange() {
123
124
125 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
126
127
128 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
129 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
130 final boolean perfectStart = true;
131 final double minStep = 120.0;
132 final double maxStep = 1200.0;
133 final double dP = 1.;
134
135
136 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
137
138
139 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
140 final List<ObservedMeasurement<?>> measurements =
141 DSSTEstimationTestUtils.createMeasurements(propagator,
142 new TwoWayRangeMeasurementCreator(context),
143 0.0, 6.0, 60.0);
144 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
145
146
147 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
148
149
150 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
151
152
153 final Orbit refOrbit = referencePropagator.
154 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
155
156
157 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
158 0., 0., 0., 0., 0., 0.
159 });
160
161
162 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
163 final double[][] dYdC = new double[6][6];
164 initialOrbit.getJacobianWrtCartesian(PositionAngleType.MEAN, dYdC);
165 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
166
167
168 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
169
170
171 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
172
173
174 final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
175 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
176 unscentedTransformProvider(new MerweUnscentedTransform(6)).
177 build();
178 final Observer observer = new Observer();
179 kalman.setObserver(observer);
180
181
182 final double expectedDeltaPos = 0.;
183 final double posEps = 1.0e-15;
184 final double expectedDeltaVel = 0.;
185 final double velEps = 1.0e-15;
186 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
187 refOrbit, positionAngleType,
188 expectedDeltaPos, posEps,
189 expectedDeltaVel, velEps);
190
191 Assertions.assertEquals(0.0, observer.getMeanResidual(), 5.08e-8);
192 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
193 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
194 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
195 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
196 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
197 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
198 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
199 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
200
201 }
202
203 @Test
204 public void testRangeWithZonal() {
205
206
207 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
208
209
210 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
211 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
212 final boolean perfectStart = true;
213 final double minStep = 120.0;
214 final double maxStep = 1200.0;
215 final double dP = 1.;
216
217
218 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
219 builder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
220
221
222 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
223 final List<ObservedMeasurement<?>> measurements =
224 DSSTEstimationTestUtils.createMeasurements(propagator,
225 new TwoWayRangeMeasurementCreator(context),
226 0.0, 6.0, 60.0);
227 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
228
229
230 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
231 propagatorBuilder.addForceModel(new DSSTZonal(GravityFieldFactory.getUnnormalizedProvider(2, 0)));
232
233
234 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
235
236
237 final Orbit refOrbit = referencePropagator.
238 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
239
240
241 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
242 0., 0., 0., 0., 0., 0.
243 });
244
245
246 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
247 final double[][] dYdC = new double[6][6];
248 initialOrbit.getJacobianWrtCartesian(PositionAngleType.MEAN, dYdC);
249 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
250
251
252 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
253
254
255 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
256
257
258 final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
259 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
260 unscentedTransformProvider(new MerweUnscentedTransform(6)).
261 build();
262 final Observer observer = new Observer();
263 kalman.setObserver(observer);
264
265
266 final double expectedDeltaPos = 0.;
267 final double posEps = 1.1e-7;
268 final double expectedDeltaVel = 0.;
269 final double velEps = 3.9e-11;
270 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
271 refOrbit, positionAngleType,
272 expectedDeltaPos, posEps,
273 expectedDeltaVel, velEps);
274
275 Assertions.assertEquals(0.0, observer.getMeanResidual(), 2.59e-3);
276 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
277 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
278 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
279 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
280 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
281 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
282 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
283 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
284
285 }
286
287 @Test
288 public void testRangeWithTesseral() {
289
290
291 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
292
293
294 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
295 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
296 final boolean perfectStart = true;
297 final double minStep = 120.0;
298 final double maxStep = 1200.0;
299 final double dP = 1.;
300
301
302 final UnnormalizedSphericalHarmonicsProvider gravityField = GravityFieldFactory.getUnnormalizedProvider(2, 2);
303 final DSSTPropagatorBuilder builder = context.createBuilder(PropagationType.OSCULATING, PropagationType.MEAN, perfectStart, minStep, maxStep, dP);
304 builder.addForceModel(new DSSTZonal(gravityField));
305 builder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
306 gravityField.getMaxDegree(),
307 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
308 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
309
310
311 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit, builder);
312 final List<ObservedMeasurement<?>> measurements =
313 DSSTEstimationTestUtils.createMeasurements(propagator,
314 new TwoWayRangeMeasurementCreator(context),
315 0.0, 6.0, 60.0);
316 final AbsoluteDate lastMeasurementEpoch = measurements.get(measurements.size() - 1).getDate();
317
318 final DSSTPropagatorBuilder propagatorBuilder = context.createBuilder(perfectStart, minStep, maxStep, dP);
319 propagatorBuilder.addForceModel(new DSSTZonal(gravityField));
320 propagatorBuilder.addForceModel(new DSSTTesseral(context.earth.getBodyFrame(), Constants.WGS84_EARTH_ANGULAR_VELOCITY, gravityField,
321 gravityField.getMaxDegree(),
322 gravityField.getMaxOrder(), 2, FastMath.min(12, gravityField.getMaxDegree() + 2),
323 gravityField.getMaxDegree(), gravityField.getMaxOrder(), FastMath.min(4, gravityField.getMaxDegree() - 2)));
324
325
326 final Propagator referencePropagator = propagatorBuilder.buildPropagator();
327
328
329 final Orbit refOrbit = referencePropagator.
330 propagate(measurements.get(measurements.size()-1).getDate()).getOrbit();
331
332
333 final RealMatrix equinoctialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
334 0., 0., 0., 0., 0., 0.
335 });
336
337
338 final Orbit initialOrbit = orbitType.convertType(context.initialOrbit);
339 final double[][] dYdC = new double[6][6];
340 initialOrbit.getJacobianWrtCartesian(PositionAngleType.MEAN, dYdC);
341 final RealMatrix Jac = MatrixUtils.createRealMatrix(dYdC);
342
343
344 final RealMatrix initialP = Jac.multiply(equinoctialP.multiply(Jac.transpose()));
345
346
347 RealMatrix Q = MatrixUtils.createRealMatrix(6, 6);
348
349 final MerweUnscentedTransform utProvider = new MerweUnscentedTransform(6, 0.5, 2., 0.);
350
351 final SemiAnalyticalUnscentedKalmanEstimator kalman = new SemiAnalyticalUnscentedKalmanEstimatorBuilder().
352 addPropagationConfiguration(propagatorBuilder, new ConstantProcessNoise(initialP, Q)).
353 unscentedTransformProvider(utProvider).
354 build();
355 final Observer observer = new Observer();
356 kalman.setObserver(observer);
357
358
359 final double expectedDeltaPos = 0.;
360 final double posEps = 4.2e-7;
361 final double expectedDeltaVel = 0.;
362 final double velEps = 3.8e-11;
363 DSSTEstimationTestUtils.checkKalmanFit(context, kalman, measurements,
364 refOrbit, positionAngleType,
365 expectedDeltaPos, posEps,
366 expectedDeltaVel, velEps);
367
368 Assertions.assertEquals(0.0, observer.getMeanResidual(), 2.55e-3);
369 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(false).getNbParams());
370 Assertions.assertEquals(6, kalman.getOrbitalParametersDrivers(true).getNbParams());
371 Assertions.assertEquals(1, kalman.getPropagationParametersDrivers(false).getNbParams());
372 Assertions.assertEquals(0, kalman.getPropagationParametersDrivers(true).getNbParams());
373 Assertions.assertEquals(0, kalman.getEstimatedMeasurementsParameters().getNbParams());
374 Assertions.assertEquals(measurements.size(), kalman.getCurrentMeasurementNumber());
375 Assertions.assertEquals(0.0, kalman.getCurrentDate().durationFrom(lastMeasurementEpoch), 1.0e-15);
376 Assertions.assertNotNull(kalman.getPhysicalEstimatedState());
377
378 }
379
380 }