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.TLEContext;
27 import org.orekit.estimation.TLEEstimationTestUtils;
28 import org.orekit.orbits.Orbit;
29 import org.orekit.orbits.OrbitType;
30 import org.orekit.orbits.PositionAngleType;
31 import org.orekit.propagation.Propagator;
32 import org.orekit.propagation.SpacecraftState;
33 import org.orekit.propagation.analytical.tle.TLEPropagator;
34 import org.orekit.propagation.conversion.TLEPropagatorBuilder;
35 import org.orekit.time.AbsoluteDate;
36 import org.orekit.utils.Differentiation;
37
38 import java.util.List;
39
40 public class TLEPVTest {
41
42 @Test
43 public void testStateDerivatives() {
44
45 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
46
47 final TLEPropagatorBuilder propagatorBuilder =
48 context.createBuilder(0.001);
49
50
51 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
52 final Propagator propagator = TLEEstimationTestUtils.createPropagator(initialOrbit,
53 propagatorBuilder);
54 final List<ObservedMeasurement<?>> measurements =
55 TLEEstimationTestUtils.createMeasurements(propagator,
56 new PVMeasurementCreator(),
57 1.0, 3.0, 300.0);
58 propagator.clearStepHandlers();
59
60 double[] errorsP = new double[3 * 6 * measurements.size()];
61 double[] errorsV = new double[3 * 6 * measurements.size()];
62 int indexP = 0;
63 int indexV = 0;
64 for (final ObservedMeasurement<?> measurement : measurements) {
65
66 final AbsoluteDate date = measurement.getDate();
67 final SpacecraftState state = propagator.propagate(date);
68 final double[][] jacobian = measurement.estimate(0, 0, new SpacecraftState[] { state }).getStateDerivatives(0);
69
70
71 final double[][] finiteDifferencesJacobian =
72 Differentiation.differentiate(state1 -> measurement.
73 estimateWithoutDerivatives(new SpacecraftState[] { state1 }).
74 getEstimatedValue(), measurement.getDimension(),
75 propagator.getAttitudeProvider(), OrbitType.CARTESIAN,
76 PositionAngleType.TRUE, 1.0, 3).value(state);
77
78 Assertions.assertEquals(finiteDifferencesJacobian.length, jacobian.length);
79 Assertions.assertEquals(finiteDifferencesJacobian[0].length, jacobian[0].length);
80 for (int i = 0; i < jacobian.length; ++i) {
81 for (int j = 0; j < jacobian[i].length; ++j) {
82 final double relativeError = FastMath.abs((finiteDifferencesJacobian[i][j] - jacobian[i][j]) /
83 finiteDifferencesJacobian[i][j]);
84 if (j < 3) {
85 errorsP[indexP++] = relativeError;
86 } else {
87 errorsV[indexV++] = relativeError;
88 }
89 }
90 }
91
92 }
93
94
95 Assertions.assertEquals(0.0, new Median().evaluate(errorsP), 0.0);
96 Assertions.assertEquals(0.0, new Median().evaluate(errorsV), 3.8e-10);
97
98 }
99
100
101 @Test
102 public void testPVWithSingleStandardDeviations() {
103
104
105 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
106 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
107
108
109 final Vector3D position = initialOrbit.getPosition();
110 final Vector3D velocity = initialOrbit.getPVCoordinates().getVelocity();
111 final AbsoluteDate date = initialOrbit.getDate();
112
113
114 final double sigmaP = 10.;
115 final double sigmaV = 0.1;
116 final double baseWeight = 0.5;
117
118
119 final double[][] Pref = new double[6][6];
120 for (int i = 0; i < 3; i++) {
121 Pref[i][i] = FastMath.pow(sigmaP, 2);
122 Pref[i+3][i+3] = FastMath.pow(sigmaV, 2);
123 }
124 final double[][] corrCoefRef = MatrixUtils.createRealIdentityMatrix(6).getData();
125
126
127 final ObservableSatellite[] sats = {
128 new ObservableSatellite(0),
129 new ObservableSatellite(2)
130 };
131
132
133 final PV[] pvs = new PV[2];
134 pvs[0] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[0]);
135 pvs[1] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[1]);
136
137
138 final double eps = 1e-25;
139
140
141 for (int k = 0; k < pvs.length; k++) {
142 final PV pv = pvs[k];
143
144
145 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
146
147
148 for (int i = 0; i < 6; i++) {
149 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
150 }
151
152 for (int i = 0; i < 3; i++) {
153 Assertions.assertEquals(sigmaP, pv.getTheoreticalStandardDeviation()[i] , eps);
154 Assertions.assertEquals(sigmaV, pv.getTheoreticalStandardDeviation()[i+3], eps);
155 }
156
157 final double[][] P = pv.getCovarianceMatrix();
158
159 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
160 Assertions.assertEquals(0., normP, eps);
161
162
163 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
164
165 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
166 Assertions.assertEquals(0., normCorrCoef, eps);
167 }
168 }
169
170
171
172 @Test
173 public void testPVWithVectorStandardDeviations() {
174
175
176 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
177 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
178
179
180 final Vector3D position = initialOrbit.getPosition();
181 final Vector3D velocity = initialOrbit.getPVCoordinates().getVelocity();
182 final AbsoluteDate date = initialOrbit.getDate();
183
184
185 final double[] sigmaP = {10., 20., 30.};
186 final double[] sigmaV = {0.1, 0.2, 0.3};
187 final double[] sigmaPV = {10., 20., 30., 0.1, 0.2, 0.3};
188 final double baseWeight = 0.5;
189
190
191 final double[][] Pref = new double[6][6];
192 for (int i = 0; i < 3; i++) {
193 Pref[i][i] = FastMath.pow(sigmaP[i], 2);
194 Pref[i+3][i+3] = FastMath.pow(sigmaV[i], 2);
195 }
196 final double[][] corrCoefRef = MatrixUtils.createRealIdentityMatrix(6).getData();
197
198
199 final ObservableSatellite[] sats = {
200 new ObservableSatellite(0),
201 new ObservableSatellite(2),
202 new ObservableSatellite(0),
203 new ObservableSatellite(10)
204 };
205
206
207 final PV[] pvs = new PV[4];
208 pvs[0] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[0]);
209 pvs[1] = new PV(date, position, velocity, sigmaP, sigmaV, baseWeight, sats[1]);
210 pvs[2] = new PV(date, position, velocity, sigmaPV, baseWeight, sats[2]);
211 pvs[3] = new PV(date, position, velocity, sigmaPV, baseWeight, sats[3]);
212
213
214 final double eps = 1e-25;
215
216
217 for (int k = 0; k < pvs.length; k++) {
218 final PV pv = pvs[k];
219
220
221 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
222
223
224 for (int i = 0; i < 6; i++) {
225 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
226 }
227
228 for (int i = 0; i < 3; i++) {
229 Assertions.assertEquals(sigmaP[i], pv.getTheoreticalStandardDeviation()[i] , eps);
230 Assertions.assertEquals(sigmaV[i], pv.getTheoreticalStandardDeviation()[i+3], eps);
231 }
232
233 final double[][] P = pv.getCovarianceMatrix();
234
235 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
236 Assertions.assertEquals(0., normP, eps);
237
238
239 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
240
241 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
242 Assertions.assertEquals(0., normCorrCoef, eps);
243 }
244 }
245
246
247
248 @Test
249 public void testPVWithTwoCovarianceMatrices() {
250
251 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
252 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
253
254
255 final Vector3D position = initialOrbit.getPosition();
256 final Vector3D velocity = initialOrbit.getPVCoordinates().getVelocity();
257 final AbsoluteDate date = 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 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
340 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
341
342
343 final Vector3D position = initialOrbit.getPosition();
344 final Vector3D velocity = initialOrbit.getPVCoordinates().getVelocity();
345 final AbsoluteDate date = initialOrbit.getDate();
346
347
348 final double[] sigmaPV = {10., 20., 30., 0.1, 0.2, 0.3};
349 final double baseWeight = 0.5;
350 final double[][] corrCoefRef = new double[6][6];
351 for (int i = 0; i < 6; i++) {
352 for (int j = 0; j < 6; j++) {
353 if (i == j) {
354 corrCoefRef[i][i] = 1.;
355 } else {
356 corrCoefRef[i][j] = i + j + 1;
357 }
358 }
359 }
360
361
362 final double[][] Pref = new double[6][6];
363 for (int i = 0; i < 6; i++) {
364 for (int j = 0; j < 6; j++) {
365 Pref[i][j] = corrCoefRef[i][j]*sigmaPV[i]*sigmaPV[j];
366 }
367 }
368
369
370 final ObservableSatellite[] sats = {
371 new ObservableSatellite(0),
372 new ObservableSatellite(2)
373 };
374
375
376 final PV[] pvs = new PV[2];
377 pvs[0] = new PV(date, position, velocity, Pref, baseWeight, sats[0]);
378 pvs[1] = new PV(date, position, velocity, Pref, baseWeight, sats[1]);
379
380
381 final double eps = 1.8e-15;
382
383
384 for (int k = 0; k < pvs.length; k++) {
385 final PV pv = pvs[k];
386
387
388 Assertions.assertEquals(sats[k].getPropagatorIndex(), pv.getSatellites().get(0).getPropagatorIndex(), eps);
389
390
391 for (int i = 0; i < 6; i++) {
392 Assertions.assertEquals(baseWeight, pv.getBaseWeight()[i], eps);
393 }
394
395 for (int i = 0; i < 6; i++) {
396 Assertions.assertEquals(sigmaPV[i], pv.getTheoreticalStandardDeviation()[i] , eps);
397 }
398
399 final double[][] P = pv.getCovarianceMatrix();
400
401 final double normP = MatrixUtils.createRealMatrix(P).subtract(MatrixUtils.createRealMatrix(Pref)).getNorm1();
402 Assertions.assertEquals(0., normP, eps);
403
404
405 final double[][] corrCoef = pv.getCorrelationCoefficientsMatrix();
406
407 final double normCorrCoef = MatrixUtils.createRealMatrix(corrCoef).subtract(MatrixUtils.createRealMatrix(corrCoefRef)).getNorm1();
408 Assertions.assertEquals(0., normCorrCoef, eps);
409 }
410
411 }
412
413
414 @Test
415 public void testExceptions() {
416
417 TLEContext context = TLEEstimationTestUtils.eccentricContext("regular-data:potential:tides");
418 final Orbit initialOrbit = TLEPropagator.selectExtrapolator(context.initialTLE).getInitialState().getOrbit();
419
420
421 final Vector3D position = initialOrbit.getPosition();
422 final Vector3D velocity = initialOrbit.getPVCoordinates().getVelocity();
423 final AbsoluteDate date = initialOrbit.getDate();
424 final double weight = 1.;
425 final ObservableSatellite sat = new ObservableSatellite(0);
426
427
428 try {
429 new PV(date, position, velocity, new double[] {0., 0., 0.}, new double[] {1.}, weight, sat);
430 Assertions.fail("An OrekitException should have been thrown");
431 } catch (OrekitException e) {
432
433 }
434
435
436 try {
437 new PV(date, position, velocity, new double[] {0., 0., 0.}, weight, sat);
438 Assertions.fail("An OrekitException should have been thrown");
439 } catch (OrekitException e) {
440
441 }
442
443
444 try {
445 new PV(date, position, velocity, new double[][] {{0., 0.}, {0., 0.}},
446 new double[][] {{0., 0.}, {0., 0.}}, weight, sat);
447 Assertions.fail("An OrekitException should have been thrown");
448 } catch (OrekitException e) {
449
450 }
451
452
453 try {
454 new PV(date, position, velocity, new double[][] {{0., 0.}, {0., 0.}}, weight, sat);
455 Assertions.fail("An OrekitException should have been thrown");
456 } catch (OrekitException e) {
457
458 }
459 }
460 }