1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation;
18
19 import org.hipparchus.geometry.euclidean.threed.Vector3D;
20 import org.hipparchus.linear.BlockRealMatrix;
21 import org.hipparchus.linear.MatrixUtils;
22 import org.hipparchus.linear.RealMatrix;
23 import org.hipparchus.util.FastMath;
24 import org.junit.jupiter.api.Assertions;
25 import org.junit.jupiter.api.DisplayName;
26 import org.junit.jupiter.api.Test;
27 import org.orekit.Utils;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.forces.gravity.potential.GravityFieldFactory;
30 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
31 import org.orekit.frames.Frame;
32 import org.orekit.frames.FramesFactory;
33 import org.orekit.frames.LOFType;
34 import org.orekit.orbits.CartesianOrbit;
35 import org.orekit.orbits.Orbit;
36 import org.orekit.orbits.OrbitType;
37 import org.orekit.orbits.PositionAngle;
38 import org.orekit.time.AbsoluteDate;
39 import org.orekit.time.TimeScalesFactory;
40 import org.orekit.utils.Constants;
41 import org.orekit.utils.IERSConventions;
42 import org.orekit.utils.PVCoordinates;
43
44 public class StateCovarianceTest {
45
46 private SpacecraftState initialState;
47 private double[][] initCov;
48 final private double DEFAULT_VALLADO_THRESHOLD = 1e-6;
49
50
51
52
53 @Test
54 public void testFrameConversion() {
55
56
57 setUp();
58
59
60 final Frame frameA = FramesFactory.getEME2000();
61 final Frame frameB = FramesFactory.getTEME();
62
63
64 final StateCovariance reference = new StateCovariance(MatrixUtils.createRealMatrix(initCov), initialState.getDate(), frameA, OrbitType.CARTESIAN, PositionAngle.MEAN);
65
66
67 StateCovariance transformed = reference.changeCovarianceFrame(initialState.getOrbit(), frameB);
68
69
70 transformed = transformed.changeCovarianceFrame(initialState.getOrbit(), frameA);
71
72
73 compareCovariance(reference.getMatrix(), transformed.getMatrix(), 5.2e-15);
74
75 }
76
77 @Test
78 public void testConstructor() {
79 final AbsoluteDate initialDate = new AbsoluteDate();
80 final Frame initialInertialFrame = FramesFactory.getGCRF();
81 final StateCovariance stateCovariance = new StateCovariance(getValladoInitialCovarianceMatrix(), initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
82 Assertions.assertEquals(OrbitType.CARTESIAN, stateCovariance.getOrbitType());
83 Assertions.assertEquals(PositionAngle.MEAN, stateCovariance.getPositionAngle());
84 Assertions.assertEquals(initialInertialFrame, stateCovariance.getFrame());
85 Assertions.assertNull(stateCovariance.getLOFType());
86 Assertions.assertEquals(initialDate, stateCovariance.getDate());
87 }
88
89 public void setUp() {
90 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
91 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
92 Orbit initialOrbit = new CartesianOrbit(
93 new PVCoordinates(new Vector3D(7526993.581890527, -9646310.10026971, 1464110.4928112086),
94 new Vector3D(3033.79456099698, 1715.265069098717, -4447.658745923895)),
95 FramesFactory.getEME2000(),
96 new AbsoluteDate("2016-02-13T16:00:00.000", TimeScalesFactory.getUTC()),
97 Constants.WGS84_EARTH_MU);
98 initialState = new SpacecraftState(initialOrbit);
99 initCov = new double[][] {
100 { 8.651816029e+01, 5.689987127e+01, -2.763870764e+01, -2.435617201e-02, 2.058274137e-02,
101 -5.872883051e-03 },
102 { 5.689987127e+01, 7.070624321e+01, 1.367120909e+01, -6.112622013e-03, 7.623626008e-03,
103 -1.239413190e-02 },
104 { -2.763870764e+01, 1.367120909e+01, 1.811858898e+02, 3.143798992e-02, -4.963106559e-02,
105 -7.420114385e-04 },
106 { -2.435617201e-02, -6.112622013e-03, 3.143798992e-02, 4.657077389e-05, 1.469943634e-05,
107 3.328475593e-05 },
108 { 2.058274137e-02, 7.623626008e-03, -4.963106559e-02, 1.469943634e-05, 3.950715934e-05,
109 2.516044258e-05 },
110 { -5.872883051e-03, -1.239413190e-02, -7.420114385e-04, 3.328475593e-05, 2.516044258e-05,
111 3.547466120e-05 }
112 };
113 }
114
115
116
117
118
119
120
121
122 private void compareCovariance(final RealMatrix reference, final RealMatrix computed, final double threshold) {
123 for (int row = 0; row < reference.getRowDimension(); row++) {
124 for (int column = 0; column < reference.getColumnDimension(); column++) {
125 if (reference.getEntry(row, column) == 0) {
126 Assertions.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column),
127 threshold);
128 }
129 else {
130 Assertions.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column),
131 FastMath.abs(threshold * reference.getEntry(row, column)));
132 }
133 }
134 }
135 }
136
137 @Test
138 @DisplayName("Test conversion from inertial frame to RTN local orbital frame")
139 public void should_return_same_covariance_matrix() {
140
141
142 final AbsoluteDate initialDate = new AbsoluteDate();
143 final Frame initialInertialFrame = FramesFactory.getGCRF();
144 final double mu = 398600e9;
145
146 final PVCoordinates initialPV = new PVCoordinates(
147 new Vector3D(6778000, 0, 0),
148 new Vector3D(0, 7668.63, 0));
149
150 final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu);
151
152 final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] {
153 { 1, 0, 0, 0, 1e-5, 1e-4 },
154 { 0, 1, 0, 0, 0, 1e-5 },
155 { 0, 0, 1, 0, 0, 0 },
156 { 0, 0, 0, 1e-3, 0, 0 },
157 { 1e-5, 0, 0, 0, 1e-3, 0 },
158 { 1e-4, 1e-5, 0, 0, 0, 1e-3 }
159 });
160
161 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceInInertialFrame, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
162
163 final RealMatrix covarianceMatrixInRTN = stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
164
165
166 final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] {
167 { 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, -1.121400e-03, 1.000000e-04 },
168 { 0.000000e+00, 1.000000e+00, 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.000000e-05 },
169 { 0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
170 { 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, 1.131400e-08 },
171 {-1.121400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001257e-03, -1.131400e-07 },
172 { 1.000000e-04, 1.000000e-05, 0.000000e+00, 1.131400e-08, -1.131400e-07, 1.000000e-03 },
173 });
174
175 compareCovariance(covarianceMatrixInRTN, expectedMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
176
177 }
178
179
180
181
182 @Test
183 public void testTypeConversion() {
184
185
186 setUp();
187
188
189 final OrbitType cart = OrbitType.CARTESIAN;
190 final OrbitType kep = OrbitType.KEPLERIAN;
191
192
193 final StateCovariance reference = new StateCovariance(MatrixUtils.createRealMatrix(initCov), initialState.getDate(), initialState.getFrame(), cart, PositionAngle.MEAN);
194
195
196 StateCovariance transformed = reference.changeCovarianceType(initialState.getOrbit(), kep, PositionAngle.MEAN);
197
198
199 transformed = transformed.changeCovarianceType(initialState.getOrbit(), cart, PositionAngle.MEAN);
200
201
202 compareCovariance(reference.getMatrix(), transformed.getMatrix(), 3.5e-12);
203
204 }
205
206 @Test
207 @DisplayName("Test covariance conversion from inertial frame to RTN local orbital frame")
208 public void should_rotate_covariance_matrix_by_ninety_degrees() {
209
210
211 final AbsoluteDate initialDate = new AbsoluteDate();
212 final Frame initialInertialFrame = FramesFactory.getGCRF();
213 final double mu = 398600e9;
214
215 final PVCoordinates initialPV = new PVCoordinates(
216 new Vector3D(0, 6778000, 0),
217 new Vector3D(-7668.63, 0, 0));
218
219 final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu);
220
221 final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] {
222 { 1, 0, 0, 0, 0, 1e-5 },
223 { 0, 1, 0, 0, 0, 0 },
224 { 0, 0, 1, 0, 0, 0 },
225 { 0, 0, 0, 1e-3, 0, 0 },
226 { 0, 0, 0, 0, 1e-3, 0 },
227 { 1e-5, 0, 0, 0, 0, 1e-3 }
228 });
229
230 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceInInertialFrame, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
231
232
233 final RealMatrix convertedCovarianceMatrixInRTN = stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
234
235
236
237 final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] {
238 { 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, -1.131400e-03, 0.000000e+00 },
239 { 0.000000e+00, 1.000000e+00, 0.000000e+00, 1.131400e-03, 0.000000e+00, -1.000000e-05 },
240 { 0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
241 { 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, -1.131400e-08 },
242 {-1.131400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001280e-03, 0.000000e+00 },
243 { 0.000000e+00, -1.000000e-05, 0.000000e+00, -1.131400e-08, 0.000000e+00, 1.000000e-03 },
244 });
245
246 compareCovariance(expectedMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
247 }
248
249 @Test
250 @DisplayName("Test covariance conversion from RTN local orbital frame to inertial frame")
251 public void should_rotate_covariance_matrix_by_minus_ninety_degrees() {
252
253
254 final AbsoluteDate initialDate = new AbsoluteDate();
255 final Frame initialInertialFrame = FramesFactory.getGCRF();
256 final double mu = 398600e9;
257
258 final PVCoordinates initialPV = new PVCoordinates(
259 new Vector3D(0, 6778000, 0),
260 new Vector3D(-7668.63, 0, 0));
261
262 final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu);
263
264 final RealMatrix initialCovarianceInRTN = new BlockRealMatrix(new double[][] {
265 { 1, 0, 0, 0, 0, 0 },
266 { 0, 1, 0, 0, 0, -1e-5 },
267 { 0, 0, 1, 0, 0, 0 },
268 { 0, 0, 0, 1e-3, 0, 0 },
269 { 0, 0, 0, 0, 1e-3, 0 },
270 { 0, -1e-5, 0, 0, 0, 1e-3 }
271 });
272
273 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceInRTN, initialDate, LOFType.QSW);
274
275
276 final RealMatrix convertedCovarianceMatrixInInertialFrame = stateCovariance.changeCovarianceFrame(initialOrbit, initialInertialFrame).getMatrix();
277
278
279
280
281 final RealMatrix expectedMatrixInInertialFrame = new BlockRealMatrix(new double[][] {
282 {1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.131400e-03, 1.000000e-05 },
283 {0.000000e+00, 1.000000e+00, 0.000000e+00, -1.131400e-03, 0.000000e+00, 0.000000e+00 },
284 {0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
285 {0.000000e+00, -1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, 0.000000e+00 },
286 {1.131400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001280e-03, 1.131400e-08 },
287 {1.000000e-05, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.131400e-08, 1.000000e-03 },
288 });
289
290 compareCovariance(expectedMatrixInInertialFrame, convertedCovarianceMatrixInInertialFrame, DEFAULT_VALLADO_THRESHOLD);
291
292 }
293
294
295
296
297
298
299
300
301
302
303
304
305
306 @Test
307 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to RTN")
308 public void should_return_Vallado_RSW_covariance_matrix_from_ECI() {
309
310
311 Utils.setDataRoot("regular-data");
312
313
314 final AbsoluteDate initialDate = getValladoInitialDate();
315 final PVCoordinates initialPV = getValladoInitialPV();
316 final Frame initialInertialFrame = FramesFactory.getGCRF();
317 final Orbit initialOrbit =
318 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
319
320 final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
321
322 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrix, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
323
324 final RealMatrix convertedCovarianceMatrixInRTN = stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
325
326
327 final RealMatrix expectedCovarianceMatrixInRTN = new BlockRealMatrix(new double[][] {
328 { 9.918921e-01, 6.700644e-03, -2.878187e-03, 2.637186e-05, -1.035961e-03, -2.878187e-05 },
329 { 6.700644e-03, 1.013730e+00, -1.019283e-02, 1.194257e-03, 2.298460e-04, -1.019283e-04 },
330 {-2.878187e-03, -1.019283e-02, 9.943782e-01, -4.011613e-05, -9.872780e-05, 4.378217e-05 },
331 { 2.637186e-05, 1.194257e-03, -4.011613e-05, 1.591713e-06, 9.046096e-07, -4.011613e-07 },
332 {-1.035961e-03, 2.298460e-04, -9.872780e-05, 9.046096e-07, 3.450431e-06, -9.872780e-07 },
333 {-2.878187e-05, -1.019283e-04, 4.378217e-05, -4.011613e-07, -9.872780e-07, 4.378217e-07 }
334 });
335
336 compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
337
338 }
339
340 private AbsoluteDate getValladoInitialDate() {
341 return new AbsoluteDate(2000, 12, 15, 16, 58, 50.208, TimeScalesFactory.getUTC());
342 }
343
344 private PVCoordinates getValladoInitialPV() {
345 return new PVCoordinates(
346 new Vector3D(-605792.21660, -5870229.51108, 3493053.19896),
347 new Vector3D(-1568.25429, -3702.34891, -6479.48395));
348 }
349
350 private double getValladoMu() {
351 return Constants.IERS2010_EARTH_MU;
352 }
353
354 private RealMatrix getValladoInitialCovarianceMatrix() {
355 return new BlockRealMatrix(new double[][] {
356 { 1, 1e-2, 1e-2, 1e-4, 1e-4, 1e-4 },
357 { 1e-2, 1, 1e-2, 1e-4, 1e-4, 1e-4 },
358 { 1e-2, 1e-2, 1, 1e-4, 1e-4, 1e-4 },
359 { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 },
360 { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 },
361 { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 }
362 });
363 }
364
365
366
367
368
369
370
371
372
373
374
375
376 @Test
377 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to NTW")
378 public void should_return_Vallado_NTW_covariance_matrix_from_ECI() {
379
380
381 Utils.setDataRoot("regular-data");
382
383
384 final AbsoluteDate initialDate = getValladoInitialDate();
385 final PVCoordinates initialPV = getValladoInitialPV();
386 final Frame initialInertialFrame = FramesFactory.getGCRF();
387 final Orbit initialOrbit =
388 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
389
390 final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
391
392 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrix, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
393
394 final RealMatrix convertedCovarianceMatrixInNTW =
395 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.NTW).getMatrix();
396
397
398 final RealMatrix expectedCovarianceMatrixInNTW = new BlockRealMatrix(new double[][] {
399 { 9.918792e-01, 6.679546e-03, -2.868345e-03, 2.621921e-05, -1.036158e-03, -2.868345e-05 },
400 { 6.679546e-03, 1.013743e+00, -1.019560e-02, 1.194061e-03, 2.299986e-04, -1.019560e-04 },
401 {-2.868345e-03, -1.019560e-02, 9.943782e-01, -4.002079e-05, -9.876648e-05, 4.378217e-05 },
402 { 2.621921e-05, 1.194061e-03, -4.002079e-05, 1.589968e-06, 9.028133e-07, -4.002079e-07 },
403 {-1.036158e-03, 2.299986e-04, -9.876648e-05, 9.028133e-07, 3.452177e-06, -9.876648e-07 },
404 {-2.868345e-05, -1.019560e-04, 4.378217e-05, -4.002079e-07, -9.876648e-07, 4.378217e-07 },
405 });
406
407 compareCovariance(expectedCovarianceMatrixInNTW, convertedCovarianceMatrixInNTW, DEFAULT_VALLADO_THRESHOLD);
408
409 }
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426 @Test
427 @DisplayName("Test custom covariance conversion Vallado test case : GCRF -> TEME -> IRTF -> NTW -> RTN -> ITRF -> GCRF")
428 public void should_return_initial_covariance_after_multiple_conversion() {
429
430
431 Utils.setDataRoot("regular-data");
432
433
434 final AbsoluteDate initialDate = getValladoInitialDate();
435 final PVCoordinates initialPV = getValladoInitialPV();
436 final Frame initialInertialFrame = FramesFactory.getGCRF();
437 final Orbit initialOrbit =
438 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
439
440 final RealMatrix initialCovarianceMatrixInGCRF = getValladoInitialCovarianceMatrix();
441
442 final Frame teme = FramesFactory.getTEME();
443
444 final Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false);
445
446 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrixInGCRF, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
447
448
449
450 final StateCovariance convertedCovarianceMatrixInTEME =
451 stateCovariance.changeCovarianceFrame(initialOrbit, teme);
452
453
454 final StateCovariance convertedCovarianceMatrixInITRF =
455 convertedCovarianceMatrixInTEME.changeCovarianceFrame(initialOrbit, itrf);
456
457
458 final StateCovariance convertedCovarianceMatrixInNTW =
459 convertedCovarianceMatrixInITRF.changeCovarianceFrame(initialOrbit, LOFType.NTW);
460
461
462 final StateCovariance convertedCovarianceMatrixInRTN =
463 convertedCovarianceMatrixInNTW.changeCovarianceFrame(initialOrbit, LOFType.QSW);
464
465
466 final StateCovariance convertedCovarianceMatrixBackInITRF =
467 convertedCovarianceMatrixInRTN.changeCovarianceFrame(initialOrbit, itrf);
468
469
470 final StateCovariance convertedCovarianceMatrixBackInTEME =
471 convertedCovarianceMatrixBackInITRF.changeCovarianceFrame(initialOrbit, teme);
472
473
474 final StateCovariance convertedCovarianceMatrixInGCRF =
475 convertedCovarianceMatrixBackInTEME.changeCovarianceFrame(initialOrbit, initialInertialFrame);
476
477
478 final RealMatrix expectedCovarianceMatrixInGCRF = getValladoInitialCovarianceMatrix();
479
480 compareCovariance(expectedCovarianceMatrixInGCRF, convertedCovarianceMatrixInGCRF.getMatrix(), 1e-12);
481
482 }
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499 @Test
500 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to PEF")
501 public void should_return_Vallado_PEF_covariance_matrix_from_ECI() {
502
503
504 Utils.setDataRoot("regular-data");
505
506
507
508 final AbsoluteDate initialDate = getValladoInitialDate();
509 final PVCoordinates initialPV = getValladoInitialPV();
510 final Frame initialInertialFrame = FramesFactory.getGCRF();
511
512 final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
513
514
515 final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
516
517
518 final Frame outputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
519
520 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrix, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
521
522 final RealMatrix convertedCovarianceMatrixInITRF =
523 stateCovariance.changeCovarianceFrame(initialOrbit, outputFrame).getMatrix();
524
525
526 final RealMatrix expectedCovarianceMatrixInITRF = new BlockRealMatrix(new double[][] {
527 { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, 3.4548396261054936e-05,
528 2.6851237046859200e-06, 5.8312677693153940e-05 },
529 { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, 1.4852736004690684e-04,
530 1.6544247282904867e-04, 1.2884310644320954e-04 },
531 { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, 5.9252211072590390e-05,
532 1.2841787487219444e-04, 1.0000913090989617e-04 },
533 { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, 3.5631474857130520e-07,
534 7.6083489184819870e-07, 5.9252213790760030e-07 },
535 { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, 7.6083489184819880e-07,
536 1.6542289254142709e-06, 1.2841787929229964e-06 },
537 { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, 5.9252213790760020e-07,
538 1.2841787929229960e-06, 1.0000913098203875e-06 }
539 });
540
541 compareCovariance(expectedCovarianceMatrixInITRF, convertedCovarianceMatrixInITRF, 1e-20);
542
543 }
544
545
546
547
548
549
550
551
552 @Test
553 @DisplayName("Test covariance conversion Vallado test case : PEF cartesian to ECI")
554 public void should_return_Vallado_ECI_covariance_matrix_from_PEF() {
555
556
557 Utils.setDataRoot("regular-data");
558
559
560 final AbsoluteDate initialDate = getValladoInitialDate();
561 final PVCoordinates initialPV = getValladoInitialPV();
562 final Frame initialInertialFrame = FramesFactory.getGCRF();
563 final Orbit initialOrbit =
564 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
565
566 final RealMatrix initialCovarianceMatrixInPEF = new BlockRealMatrix(new double[][] {
567 { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, 3.4548396261054936e-05,
568 2.6851237046859200e-06, 5.8312677693153940e-05 },
569 { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, 1.4852736004690684e-04,
570 1.6544247282904867e-04, 1.2884310644320954e-04 },
571 { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, 5.9252211072590390e-05,
572 1.2841787487219444e-04, 1.0000913090989617e-04 },
573 { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, 3.5631474857130520e-07,
574 7.6083489184819870e-07, 5.9252213790760030e-07 },
575 { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, 7.6083489184819880e-07,
576 1.6542289254142709e-06, 1.2841787929229964e-06 },
577 { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, 5.9252213790760020e-07,
578 1.2841787929229960e-06, 1.0000913098203875e-06 }
579 });
580
581 final Frame inputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, false);
582
583
584 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrixInPEF, initialDate, inputFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
585
586
587 final RealMatrix convertedCovarianceMatrixInECI = stateCovariance.changeCovarianceFrame(initialOrbit, initialInertialFrame).getMatrix();
588
589
590 final RealMatrix expectedCovarianceMatrixInECI = getValladoInitialCovarianceMatrix();
591
592 compareCovariance(expectedCovarianceMatrixInECI, convertedCovarianceMatrixInECI, 1e-7);
593
594 }
595
596
597
598
599
600
601
602
603
604 @Test
605 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to MOD")
606 public void should_return_Vallado_MOD_covariance_matrix_from_ECI() {
607
608
609 Utils.setDataRoot("regular-data");
610
611
612 final AbsoluteDate initialDate = getValladoInitialDate();
613 final PVCoordinates initialPV = getValladoInitialPV();
614 final Frame initialInertialFrame = FramesFactory.getGCRF();
615 final Orbit initialOrbit =
616 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
617
618 final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
619
620 final Frame outputFrame = FramesFactory.getMOD(IERSConventions.IERS_2010);
621
622 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrix, initialDate, initialInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN);
623
624 final RealMatrix convertedCovarianceMatrixInMOD =
625 stateCovariance.changeCovarianceFrame(initialOrbit, outputFrame).getMatrix();
626
627
628 final RealMatrix expectedCovarianceMatrixInMOD = new BlockRealMatrix(new double[][] {
629 { 9.999939e-001, 9.999070e-003, 9.997861e-003, 9.993866e-005, 9.999070e-005, 9.997861e-005 },
630 { 9.999070e-003, 1.000004e+000, 1.000307e-002, 9.999070e-005, 1.000428e-004, 1.000307e-004 },
631 { 9.997861e-003, 1.000307e-002, 1.000002e+000, 9.997861e-005, 1.000307e-004, 1.000186e-004 },
632 { 9.993866e-005, 9.999070e-005, 9.997861e-005, 9.993866e-007, 9.999070e-007, 9.997861e-007 },
633 { 9.999070e-005, 1.000428e-004, 1.000307e-004, 9.999070e-007, 1.000428e-006, 1.000307e-006 },
634 { 9.997861e-005, 1.000307e-004, 1.000186e-004, 9.997861e-007, 1.000307e-006, 1.000186e-006 },
635 });
636
637 compareCovariance(expectedCovarianceMatrixInMOD, convertedCovarianceMatrixInMOD, DEFAULT_VALLADO_THRESHOLD);
638
639 }
640
641
642
643
644
645
646
647
648
649 @Test
650 @DisplayName("Test covariance conversion from Vallado test case NTW to RSW")
651 public void should_convert_Vallado_NTW_to_RSW() {
652
653
654 Utils.setDataRoot("regular-data");
655
656
657 final AbsoluteDate initialDate = getValladoInitialDate();
658 final PVCoordinates initialPV = getValladoInitialPV();
659 final Frame initialInertialFrame = FramesFactory.getGCRF();
660 final Orbit initialOrbit =
661 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
662
663 final RealMatrix initialCovarianceMatrixInNTW = new BlockRealMatrix(new double[][] {
664 { 9.918792e-001, 6.679546e-003, -2.868345e-003, 1.879167e-005, 6.679546e-005, -2.868345e-005 },
665 { 6.679546e-003, 1.013743e+000, -1.019560e-002, 6.679546e-005, 2.374262e-004, -1.019560e-004 },
666 { -2.868345e-003, -1.019560e-002, 9.943782e-001, -2.868345e-005, -1.019560e-004, 4.378217e-005 },
667 { 1.879167e-005, 6.679546e-005, -2.868345e-005, 1.879167e-007, 6.679546e-007, -2.868345e-007 },
668 { 6.679546e-005, 2.374262e-004, -1.019560e-004, 6.679546e-007, 2.374262e-006, -1.019560e-006 },
669 { -2.868345e-005, -1.019560e-004, 4.378217e-005, -2.868345e-007, -1.019560e-006, 4.378217e-007 }
670 });
671
672 final StateCovariance stateCovariance = new StateCovariance(initialCovarianceMatrixInNTW, initialDate, LOFType.NTW);
673
674
675 final RealMatrix convertedCovarianceMatrixInRTN =
676 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
677
678
679 final RealMatrix expectedCovarianceMatrixInRTN = new BlockRealMatrix(new double[][] {
680 { 9.918921e-001, 6.700644e-003, -2.878187e-003, 1.892086e-005, 6.700644e-005, -2.878187e-005 },
681 { 6.700644e-003, 1.013730e+000, -1.019283e-002, 6.700644e-005, 2.372970e-004, -1.019283e-004 },
682 { -2.878187e-003, -1.019283e-002, 9.943782e-001, -2.878187e-005, -1.019283e-004, 4.378217e-005 },
683 { 1.892086e-005, 6.700644e-005, -2.878187e-005, 1.892086e-007, 6.700644e-007, -2.878187e-007 },
684 { 6.700644e-005, 2.372970e-004, -1.019283e-004, 6.700644e-007, 2.372970e-006, -1.019283e-006 },
685 { -2.878187e-005, -1.019283e-004, 4.378217e-005, -2.878187e-007, -1.019283e-006, 4.378217e-007 }
686 });
687
688 compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
689
690 }
691
692
693
694
695
696
697
698 @Test
699 @DisplayName("Test shiftedBy method of StateCovariance")
700 public void should_return_expected_shifted_state_covariance() {
701
702
703 Utils.setDataRoot("regular-data");
704
705
706 final AbsoluteDate initialDate = new AbsoluteDate();
707 final Frame inertialFrame = FramesFactory.getGCRF();
708 final Frame nonInertialFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
709 final PVCoordinates pv = getValladoInitialPV();
710 final double mu = Constants.IERS2010_EARTH_MU;
711 final Orbit initialOrbit = new CartesianOrbit(pv, inertialFrame, initialDate, mu);
712
713 final double timeShift = 300;
714
715
716 final StateCovariance initialCovarianceInCartesian =
717 new StateCovariance(getValladoInitialCovarianceMatrix(), initialDate, inertialFrame,
718 OrbitType.CARTESIAN, PositionAngle.MEAN);
719
720 final StateCovariance covarianceInEquinoctial =
721 initialCovarianceInCartesian.changeCovarianceType(initialOrbit, OrbitType.EQUINOCTIAL, PositionAngle.MEAN);
722
723 final StateCovariance covarianceInCartesianInLOF =
724 initialCovarianceInCartesian.changeCovarianceFrame(initialOrbit, LOFType.QSW);
725
726 final StateCovariance covarianceInCartesianInNonInertial =
727 initialCovarianceInCartesian.changeCovarianceFrame(initialOrbit, nonInertialFrame);
728
729
730 final StateCovariance shiftedCovarianceInEquinoctial =
731 covarianceInEquinoctial.shiftedBy(initialOrbit, timeShift);
732 final RealMatrix shiftedCovarianceInEquinoctialBackToInitial =
733 shiftedCovarianceInEquinoctial.changeCovarianceType(initialOrbit.shiftedBy(timeShift),
734 OrbitType.CARTESIAN, PositionAngle.MEAN)
735 .getMatrix();
736
737 final StateCovariance shiftedCovarianceInCartesianInLOF =
738 covarianceInCartesianInLOF.shiftedBy(initialOrbit, timeShift);
739 final RealMatrix shiftedCovarianceInCartesianInLOFBackToInitial =
740 shiftedCovarianceInCartesianInLOF.changeCovarianceFrame(initialOrbit.shiftedBy(timeShift),
741 inertialFrame)
742 .getMatrix();
743
744 final StateCovariance shiftedCovarianceInCartesianInNonInertial =
745 covarianceInCartesianInNonInertial.shiftedBy(initialOrbit, timeShift);
746 final RealMatrix shiftedCovarianceInCartesianInNonInertialBackToInitial =
747 shiftedCovarianceInCartesianInNonInertial.changeCovarianceFrame(initialOrbit.shiftedBy(timeShift),
748 inertialFrame)
749 .getMatrix();
750
751
752
753 final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(6);
754 final double sma = initialOrbit.getA();
755 final double contribution = -1.5 * timeShift * FastMath.sqrt(mu / FastMath.pow(sma, 5));
756 stm.setEntry(5, 0, contribution);
757
758 final StateCovariance initialCovarianceInKeplerian =
759 initialCovarianceInCartesian.changeCovarianceType(initialOrbit, OrbitType.KEPLERIAN, PositionAngle.MEAN);
760
761 final RealMatrix referenceCovarianceMatrixInKeplerian =
762 stm.multiply(initialCovarianceInKeplerian.getMatrix().multiplyTransposed(stm));
763
764 final RealMatrix referenceCovarianceMatrixInCartesian =
765 new StateCovariance(referenceCovarianceMatrixInKeplerian, initialDate.shiftedBy(timeShift),
766 inertialFrame, OrbitType.KEPLERIAN, PositionAngle.MEAN).changeCovarianceType(
767 initialOrbit.shiftedBy(timeShift), OrbitType.CARTESIAN, PositionAngle.MEAN).getMatrix();
768
769
770 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInEquinoctialBackToInitial, 1e-7);
771 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInCartesianInLOFBackToInitial, 1e-7);
772 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInCartesianInNonInertialBackToInitial, 1e-7);
773
774 }
775
776
777
778
779
780
781
782
783
784 @Test
785 @DisplayName("Test thrown error if input frame is not pseudo-inertial and the covariance matrix is not expressed in cartesian elements")
786 public void should_return_orekit_exception() {
787
788
789 Utils.setDataRoot("regular-data");
790
791
792 final AbsoluteDate initialDate = getValladoInitialDate();
793 final PVCoordinates initialPV = getValladoInitialPV();
794 final Frame initialInertialFrame = FramesFactory.getGCRF();
795 final Orbit initialOrbit =
796 new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu());
797
798 final RealMatrix randomCovarianceMatrix = new BlockRealMatrix(new double[][] {
799 { 9.918792e-001, 6.679546e-003, -2.868345e-003, 1.879167e-005, 6.679546e-005, -2.868345e-005 },
800 { 6.679546e-003, 1.013743e+000, -1.019560e-002, 6.679546e-005, 2.374262e-004, -1.019560e-004 },
801 { -2.868345e-003, -1.019560e-002, 9.943782e-001, -2.868345e-005, -1.019560e-004, 4.378217e-005 },
802 { 1.879167e-005, 6.679546e-005, -2.868345e-005, 1.879167e-007, 6.679546e-007, -2.868345e-007 },
803 { 6.679546e-005, 2.374262e-004, -1.019560e-004, 6.679546e-007, 2.374262e-006, -1.019560e-006 },
804 { -2.868345e-005, -1.019560e-004, 4.378217e-005, -2.868345e-007, -1.019560e-006, 4.378217e-007 }
805 });
806
807 final Frame nonInertialFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
808
809 final Frame inertialFrame = FramesFactory.getGCRF();
810
811
812 Assertions.assertThrows(OrekitException.class,
813 () -> {
814 new StateCovariance(randomCovarianceMatrix, initialDate, nonInertialFrame, OrbitType.CIRCULAR, PositionAngle.MEAN).changeCovarianceFrame(initialOrbit, inertialFrame);
815 });
816
817 Assertions.assertThrows(OrekitException.class,
818 () -> {
819 new StateCovariance(randomCovarianceMatrix, initialDate, nonInertialFrame, OrbitType.EQUINOCTIAL, PositionAngle.MEAN).changeCovarianceFrame(initialOrbit, LOFType.QSW);
820 });
821
822 Assertions.assertThrows(OrekitException.class,
823 () -> {
824 new StateCovariance(randomCovarianceMatrix, initialDate, nonInertialFrame, OrbitType.EQUINOCTIAL, PositionAngle.MEAN).changeCovarianceType(initialOrbit, OrbitType.KEPLERIAN, PositionAngle.MEAN);
825 });
826
827 Assertions.assertThrows(OrekitException.class,
828 () -> {
829 new StateCovariance(randomCovarianceMatrix, initialDate, LOFType.QSW).changeCovarianceType(initialOrbit, OrbitType.KEPLERIAN, PositionAngle.MEAN);
830 });
831
832 Assertions.assertThrows(OrekitException.class,
833 () -> {
834 new StateCovariance(randomCovarianceMatrix, initialDate, nonInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN).changeCovarianceType(initialOrbit, OrbitType.KEPLERIAN, PositionAngle.MEAN);
835 });
836
837 }
838
839
840 }