1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements;
18
19 import org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.linear.MatrixUtils;
21 import org.hipparchus.stat.descriptive.rank.Median;
22 import org.hipparchus.util.FastMath;
23 import org.junit.jupiter.api.Assertions;
24 import org.junit.jupiter.api.Test;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.estimation.DSSTContext;
27 import org.orekit.estimation.DSSTEstimationTestUtils;
28 import org.orekit.orbits.OrbitType;
29 import org.orekit.orbits.PositionAngleType;
30 import org.orekit.propagation.Propagator;
31 import org.orekit.propagation.SpacecraftState;
32 import org.orekit.propagation.conversion.DSSTPropagatorBuilder;
33 import org.orekit.time.AbsoluteDate;
34 import org.orekit.utils.Differentiation;
35 import org.orekit.utils.StateFunction;
36
37 import java.util.List;
38
39 public class DSSTPVTest {
40
41 @Test
42 public void testStateDerivatives() {
43
44 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
45
46 final DSSTPropagatorBuilder propagatorBuilder =
47 context.createBuilder(true, 1.0e-6, 60.0, 0.001);
48
49
50 final Propagator propagator = DSSTEstimationTestUtils.createPropagator(context.initialOrbit,
51 propagatorBuilder);
52 final List<ObservedMeasurement<?>> measurements =
53 DSSTEstimationTestUtils.createMeasurements(propagator,
54 new PVMeasurementCreator(),
55 1.0, 3.0, 300.0);
56 propagator.clearStepHandlers();
57
58 double[] errorsP = new double[3 * 6 * measurements.size()];
59 double[] errorsV = new double[3 * 6 * measurements.size()];
60 int indexP = 0;
61 int indexV = 0;
62 for (final ObservedMeasurement<?> measurement : measurements) {
63
64 final AbsoluteDate date = measurement.getDate();
65 final SpacecraftState state = propagator.propagate(date);
66 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
67
68
69 final double[][] finiteDifferencesJacobian =
70 Differentiation.differentiate(new StateFunction() {
71 public double[] value(final SpacecraftState state) {
72 return measurement.
73 estimateWithoutDerivatives(new SpacecraftState[] { state }).
74 getEstimatedValue();
75 }
76 }, measurement.getDimension(),
77 propagator.getAttitudeProvider(), OrbitType.CARTESIAN,
78 PositionAngleType.TRUE, 1.0, 3).value(state);
79
80 Assertions.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
81 Assertions.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
82 for (int i = 0; i < jacobian.length; ++i) {
83 for (int j = 0; j < jacobian[i].length; ++j) {
84 final double relativeError = FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
85 finiteDifferencesJacobian[i][j]);
86 if (j < 3) {
87 errorsP[indexP++] = relativeError;
88 } else {
89 errorsV[indexV++] = relativeError;
90 }
91 }
92 }
93
94 }
95
96
97 Assertions.assertEquals(0.0, new Median().evaluate(errorsP), 0.0);
98 Assertions.assertEquals(0.0, new Median().evaluate(errorsV), 2.0e-10);
99
100 }
101
102
103
104 @Test
105 public void testPVWithSingleStandardDeviations() {
106
107
108 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
109
110
111 final Vector3D position = context.initialOrbit.getPosition();
112 final Vector3D velocity = context.initialOrbit.getPVCoordinates().getVelocity();
113 final AbsoluteDate date = context.initialOrbit.getDate();
114
115
116 final double sigmaP = 10.;
117 final double sigmaV = 0.1;
118 final double baseWeight = 0.5;
119
120
121 final double[][] Pref = new double[6][6];
122 for (int i = 0; i < 3; i++) {
123 Pref[i][i] = FastMath.pow(sigmaP, 2);
124 Pref[i+3][i+3] = FastMath.pow(sigmaV, 2);
125 }
126 final double[][] corrCoefRef = MatrixUtils.createRealIdentityMatrix(6).getData();
127
128
129 final ObservableSatellite[] sats = {
130 new ObservableSatellite(0),
131 new ObservableSatellite(2)
132 };
133
134
135 final PV[] pvs = new PV[2];
136 pvs[0] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[0]);
137 pvs[1] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[1]);
138
139
140 final double eps = 1e-25;
141
142
143 for (int k = 0; k < pvs.length; k++) {
144 final PV pv = pvs[k];
145
146
147 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
148
149
150 for (int i = 0; i < 6; i++) {
151 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
152 }
153
154 for (int i = 0; i < 3; i++) {
155 Assertions.assertEquals(sigmaP, pv.getTheoreticalStandardDeviation()[i] , eps);
156 Assertions.assertEquals(sigmaV, pv.getTheoreticalStandardDeviation()[i+3], eps);
157 }
158
159 final double[][] P = pv.getCovarianceMatrix();
160
161 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
162 Assertions.assertEquals(0., normP, eps);
163
164
165 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
166
167 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
168 Assertions.assertEquals(0., normCorrCoef, eps);
169 }
170 }
171
172
173
174 @Test
175 public void testPVWithVectorStandardDeviations() {
176
177
178 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
179
180
181 final Vector3D position = context.initialOrbit.getPosition();
182 final Vector3D velocity = context.initialOrbit.getPVCoordinates().getVelocity();
183 final AbsoluteDate date = context.initialOrbit.getDate();
184
185
186 final double[] sigmaP = {10., 20., 30.};
187 final double[] sigmaV = {0.1, 0.2, 0.3};
188 final double[] sigmaPV = {10., 20., 30., 0.1, 0.2, 0.3};
189 final double baseWeight = 0.5;
190
191
192 final double[][] Pref = new double[6][6];
193 for (int i = 0; i < 3; i++) {
194 Pref[i][i] = FastMath.pow(sigmaP[i], 2);
195 Pref[i+3][i+3] = FastMath.pow(sigmaV[i], 2);
196 }
197 final double[][] corrCoefRef = MatrixUtils.createRealIdentityMatrix(6).getData();
198
199
200 final ObservableSatellite[] sats = {
201 new ObservableSatellite(0),
202 new ObservableSatellite(2),
203 new ObservableSatellite(0),
204 new ObservableSatellite(10)
205 };
206
207
208 final PV[] pvs = new PV[4];
209 pvs[0] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[0]);
210 pvs[1] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[1]);
211 pvs[2] = new PV(date, position, velocity, sigmaPV, baseWeight, sats[2]);
212 pvs[3] = new PV(date, position, velocity, sigmaPV, baseWeight, sats[3]);
213
214
215 final double eps = 1e-25;
216
217
218 for (int k = 0; k < pvs.length; k++) {
219 final PV pv = pvs[k];
220
221
222 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
223
224
225 for (int i = 0; i < 6; i++) {
226 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
227 }
228
229 for (int i = 0; i < 3; i++) {
230 Assertions.assertEquals(sigmaP[i], pv.getTheoreticalStandardDeviation()[i] , eps);
231 Assertions.assertEquals(sigmaV[i], pv.getTheoreticalStandardDeviation()[i+3], eps);
232 }
233
234 final double[][] P = pv.getCovarianceMatrix();
235
236 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
237 Assertions.assertEquals(0., normP, eps);
238
239
240 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
241
242 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
243 Assertions.assertEquals(0., normCorrCoef, eps);
244 }
245 }
246
247
248
249 @Test
250 public void testPVWithTwoCovarianceMatrices() {
251
252 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
253
254
255 final Vector3D position = context.initialOrbit.getPosition();
256 final Vector3D velocity = context.initialOrbit.getPVCoordinates().getVelocity();
257 final AbsoluteDate date = context.initialOrbit.getDate();
258
259
260 final double[][] positionP = {{100., 400., 1200.}, {400., 400., 1800.}, {1200., 1800., 900.}};
261 final double[][] velocityP = {{0.01, 0.04, 0.12 }, {0.04, 0.04, 0.18 }, {0.12 , 0.18 , 0.09}};
262 final double baseWeight = 0.5;
263
264
265 final double[][] Pref = new double[6][6];
266 for (int i = 0; i < 3; i++) {
267 for (int j = i; j < 3; j++) {
268 Pref[i][j] = positionP[i][j];
269 Pref[j][i] = positionP[i][j];
270 Pref[i+3][j+3] = velocityP[i][j];
271 Pref[j+3][i+3] = velocityP[i][j];
272 }
273 }
274 final double[][] corrCoefRef3 = {{1., 2., 4.}, {2., 1., 3.}, {4., 3., 1.}};
275 final double[][] corrCoefRef = new double[6][6];
276 for (int i = 0; i < 3; i++) {
277 for (int j = i; j < 3; j++) {
278 corrCoefRef[i][j] = corrCoefRef3[i][j];
279 corrCoefRef[j][i] = corrCoefRef3[i][j];
280 corrCoefRef[i+3][j+3] = corrCoefRef3[i][j];
281 corrCoefRef[j+3][i+3] = corrCoefRef3[i][j];
282 }
283 }
284
285
286 final ObservableSatellite[] sats = {
287 new ObservableSatellite(0),
288 new ObservableSatellite(2)
289 };
290
291
292 final double[] sigmaP = {10., 20., 30.};
293 final double[] sigmaV = {0.1, 0.2, 0.3};
294
295
296 final PV[] pvs = new PV[2];
297 pvs[0] = new PV(date, position, velocity, positionP, velocityP, baseWeight, sats[0]);
298 pvs[1] = new PV(date, position, velocity, positionP, velocityP, baseWeight, sats[1]);
299
300
301 final double eps = 6.7e-16;
302
303
304 for (int k = 0; k < pvs.length; k++) {
305 final PV pv = pvs[k];
306
307
308 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
309
310
311 for (int i = 0; i < 6; i++) {
312 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
313 }
314
315 for (int i = 0; i < 3; i++) {
316 Assertions.assertEquals(sigmaP[i], pv.getTheoreticalStandardDeviation()[i] , eps);
317 Assertions.assertEquals(sigmaV[i], pv.getTheoreticalStandardDeviation()[i+3], eps);
318 }
319
320 final double[][] P = pv.getCovarianceMatrix();
321
322 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
323 Assertions.assertEquals(0., normP, eps);
324
325
326 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
327
328 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
329 Assertions.assertEquals(0., normCorrCoef, eps);
330 }
331
332 }
333
334
335
336 @Test
337 public void testPVWithCovarianceMatrix() {
338
339 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
340
341
342 final Vector3D position = context.initialOrbit.getPosition();
343 final Vector3D velocity = context.initialOrbit.getPVCoordinates().getVelocity();
344 final AbsoluteDate date = context.initialOrbit.getDate();
345
346
347 final double[] sigmaPV = {10., 20., 30., 0.1, 0.2, 0.3};
348 final double baseWeight = 0.5;
349 final double[][] corrCoefRef = new double[6][6];
350 for (int i = 0; i < 6; i++) {
351 for (int j = 0; j < 6; j++) {
352 if (i == j) {
353 corrCoefRef[i][i] = 1.;
354 } else {
355 corrCoefRef[i][j] = i + j + 1;
356 }
357 }
358 }
359
360
361 final double[][] Pref = new double[6][6];
362 for (int i = 0; i < 6; i++) {
363 for (int j = 0; j < 6; j++) {
364 Pref[i][j] = corrCoefRef[i][j]*sigmaPV[i]*sigmaPV[j];
365 }
366 }
367
368
369 final ObservableSatellite[] sats = {
370 new ObservableSatellite(0),
371 new ObservableSatellite(2)
372 };
373
374
375 final PV[] pvs = new PV[2];
376 pvs[0] = new PV(date, position, velocity, Pref, baseWeight, sats[0]);
377 pvs[1] = new PV(date, position, velocity, Pref, baseWeight, sats[1]);
378
379
380 final double eps = 1.8e-15;
381
382
383 for (int k = 0; k < pvs.length; k++) {
384 final PV pv = pvs[k];
385
386
387 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
388
389
390 for (int i = 0; i < 6; i++) {
391 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
392 }
393
394 for (int i = 0; i < 6; i++) {
395 Assertions.assertEquals(sigmaPV[i], pv.getTheoreticalStandardDeviation()[i] , eps);
396 }
397
398 final double[][] P = pv.getCovarianceMatrix();
399
400 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
401 Assertions.assertEquals(0., normP, eps);
402
403
404 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
405
406 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
407 Assertions.assertEquals(0., normCorrCoef, eps);
408 }
409
410 }
411
412
413 @Test
414 public void testExceptions() {
415
416 DSSTContext context = DSSTEstimationTestUtils.eccentricContext("regular-data:potential:tides");
417
418
419 final Vector3D position = context.initialOrbit.getPosition();
420 final Vector3D velocity = context.initialOrbit.getPVCoordinates().getVelocity();
421 final AbsoluteDate date = context.initialOrbit.getDate();
422 final double weight = 1.;
423 final ObservableSatellite sat = new ObservableSatellite(0);
424
425
426 try {
427 new PV(date, position, velocity, new double[] {0., 0., 0.}, new double[] {1.}, weight, sat);
428 Assertions.fail("An OrekitException should have been thrown");
429 } catch (OrekitException e) {
430
431 }
432
433
434 try {
435 new PV(date, position, velocity, new double[] {0., 0., 0.}, weight, sat);
436 Assertions.fail("An OrekitException should have been thrown");
437 } catch (OrekitException e) {
438
439 }
440
441
442 try {
443 new PV(date, position, velocity, new double[][] {{0., 0.}, {0., 0.}},
444 new double[][] {{0., 0.}, {0., 0.}}, weight, sat);
445 Assertions.fail("An OrekitException should have been thrown");
446 } catch (OrekitException e) {
447
448 }
449
450
451 try {
452 new PV(date, position, velocity, new double[][] {{0., 0.}, {0., 0.}}, weight, sat);
453 Assertions.fail("An OrekitException should have been thrown");
454 } catch (OrekitException e) {
455
456 }
457 }
458 }