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.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.FieldElement;
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.linear.*;
24 import org.hipparchus.util.Binary64;
25 import org.hipparchus.util.Binary64Field;
26 import org.hipparchus.util.FastMath;
27 import org.junit.jupiter.api.Assertions;
28 import org.junit.jupiter.api.DisplayName;
29 import org.junit.jupiter.api.Test;
30 import org.mockito.Mockito;
31 import org.orekit.Utils;
32 import org.orekit.errors.OrekitException;
33 import org.orekit.frames.Frame;
34 import org.orekit.frames.FramesFactory;
35 import org.orekit.frames.LOF;
36 import org.orekit.frames.LOFType;
37 import org.orekit.orbits.FieldCartesianOrbit;
38 import org.orekit.orbits.FieldKeplerianOrbit;
39 import org.orekit.orbits.FieldOrbit;
40 import org.orekit.orbits.OrbitType;
41 import org.orekit.orbits.PositionAngleType;
42 import org.orekit.time.AbsoluteDate;
43 import org.orekit.time.FieldAbsoluteDate;
44 import org.orekit.time.TimeScalesFactory;
45 import org.orekit.utils.Constants;
46 import org.orekit.utils.FieldPVCoordinates;
47 import org.orekit.utils.IERSConventions;
48 import org.orekit.utils.PVCoordinates;
49
50 import java.util.Arrays;
51
52 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
53 import static org.junit.jupiter.api.Assertions.assertEquals;
54
55 class FieldStateCovarianceTest {
56
57 private final Field<Binary64> field = Binary64Field.getInstance();
58 private final double DEFAULT_VALLADO_THRESHOLD = 1e-6;
59
60 @Test
61 @DisplayName("Test conversion from inertial frame to RTN local orbital frame")
62 public void should_return_same_covariance_matrix() {
63
64
65 final FieldAbsoluteDate<Binary64> initialDate = new FieldAbsoluteDate<>(field);
66 final Frame initialInertialFrame = FramesFactory.getGCRF();
67 final Binary64 mu = new Binary64(398600e9);
68
69 final FieldPVCoordinates<Binary64> initialPV =
70 new FieldPVCoordinates<>(field, new PVCoordinates(new Vector3D(6778000, 0, 0),
71 new Vector3D(0, 7668.63, 0)));
72
73 final FieldOrbit<Binary64> initialOrbit =
74 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, mu);
75
76 final BlockFieldMatrix<Binary64> initialCovarianceInInertialFrame = new BlockFieldMatrix<>(new Binary64[][] {
77 { new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-5),
78 new Binary64(1e-4) },
79 { new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-5) },
80 { new Binary64(0), new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0) },
81 { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3), new Binary64(0), new Binary64(0) },
82 { new Binary64(1e-5), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3),
83 new Binary64(0) },
84 { new Binary64(1e-4), new Binary64(1e-5), new Binary64(0), new Binary64(0), new Binary64(0),
85 new Binary64(1e-3) }
86 });
87
88 final FieldStateCovariance<Binary64> stateCovariance =
89 new FieldStateCovariance<>(initialCovarianceInInertialFrame, initialDate, initialInertialFrame,
90 OrbitType.CARTESIAN, PositionAngleType.MEAN);
91
92 final FieldMatrix<Binary64> covarianceMatrixInRTN =
93 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW_INERTIAL).getMatrix();
94
95
96 compareCovariance(initialCovarianceInInertialFrame, covarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
97
98 }
99
100
101
102
103
104
105
106
107
108 @Test
109 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to MOD")
110 public void should_return_Vallado_MOD_covariance_matrix_from_ECI() {
111
112
113 Utils.setDataRoot("regular-data");
114
115
116 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
117 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
118 final Frame initialInertialFrame = FramesFactory.getGCRF();
119 final FieldOrbit<Binary64> initialOrbit =
120 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
121
122 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
123
124 final Frame outputFrame = FramesFactory.getMOD(IERSConventions.IERS_2010);
125
126 final FieldStateCovariance<Binary64> stateCovariance =
127 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
128 OrbitType.CARTESIAN, PositionAngleType.MEAN);
129
130 final FieldMatrix<Binary64> convertedCovarianceMatrixInMOD =
131 stateCovariance.changeCovarianceFrame(initialOrbit, outputFrame).getMatrix();
132
133
134 final FieldMatrix<Binary64> expectedCovarianceMatrixInMOD = new BlockFieldMatrix<>(new Binary64[][] {
135 { new Binary64(9.999939e-001), new Binary64(9.999070e-003), new Binary64(9.997861e-003),
136 new Binary64(9.993866e-005), new Binary64(9.999070e-005), new Binary64(9.997861e-005) },
137 { new Binary64(9.999070e-003), new Binary64(1.000004e+000), new Binary64(1.000307e-002),
138 new Binary64(9.999070e-005), new Binary64(1.000428e-004), new Binary64(1.000307e-004) },
139 { new Binary64(9.997861e-003), new Binary64(1.000307e-002), new Binary64(1.000002e+000),
140 new Binary64(9.997861e-005), new Binary64(1.000307e-004), new Binary64(1.000186e-004) },
141 { new Binary64(9.993866e-005), new Binary64(9.999070e-005), new Binary64(9.997861e-005),
142 new Binary64(9.993866e-007), new Binary64(9.999070e-007), new Binary64(9.997861e-007) },
143 { new Binary64(9.999070e-005), new Binary64(1.000428e-004), new Binary64(1.000307e-004),
144 new Binary64(9.999070e-007), new Binary64(1.000428e-006), new Binary64(1.000307e-006) },
145 { new Binary64(9.997861e-005), new Binary64(1.000307e-004), new Binary64(1.000186e-004),
146 new Binary64(9.997861e-007), new Binary64(1.000307e-006), new Binary64(1.000186e-006) },
147 });
148
149 compareCovariance(expectedCovarianceMatrixInMOD, convertedCovarianceMatrixInMOD, DEFAULT_VALLADO_THRESHOLD);
150
151 }
152
153
154
155
156
157
158
159
160
161
162
163
164 @Test
165 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to NTW ( considered inertial)")
166 public void should_return_Vallado_NTW_covariance_matrix_from_ECI() {
167
168
169 Utils.setDataRoot("regular-data");
170
171
172 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
173 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
174 final Frame initialInertialFrame = FramesFactory.getGCRF();
175 final FieldOrbit<Binary64> initialOrbit =
176 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
177
178 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
179
180 final FieldStateCovariance<Binary64> stateCovariance =
181 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
182 OrbitType.CARTESIAN, PositionAngleType.MEAN);
183
184 final FieldMatrix<Binary64> convertedCovarianceMatrixInNTW =
185 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.NTW_INERTIAL).getMatrix();
186
187
188 final FieldMatrix<Binary64> expectedCovarianceMatrixInNTW = new BlockFieldMatrix<>(new Binary64[][] {
189 { new Binary64(9.918792e-001), new Binary64(6.679546e-003), new Binary64(-2.868345e-003),
190 new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005) },
191 { new Binary64(6.679546e-003), new Binary64(1.013743e+000), new Binary64(-1.019560e-002),
192 new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004) },
193 { new Binary64(-2.868345e-003), new Binary64(-1.019560e-002), new Binary64(9.943782e-001),
194 new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005) },
195 { new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005),
196 new Binary64(1.879167e-007), new Binary64(6.679546e-007), new Binary64(-2.868345e-007) },
197 { new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004),
198 new Binary64(6.679546e-007), new Binary64(2.374262e-006), new Binary64(-1.019560e-006) },
199 { new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005),
200 new Binary64(-2.868345e-007), new Binary64(-1.019560e-006), new Binary64(4.378217e-007) } });
201
202 compareCovariance(expectedCovarianceMatrixInNTW, convertedCovarianceMatrixInNTW, DEFAULT_VALLADO_THRESHOLD);
203
204 }
205
206
207
208
209
210
211
212
213
214
215
216
217 @Test
218 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to NTW (non inertial)")
219 public void should_return_Vallado_NTW_non_inertial_covariance_matrix_from_ECI() {
220
221
222 Utils.setDataRoot("regular-data");
223
224
225 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
226 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
227 final Frame initialInertialFrame = FramesFactory.getGCRF();
228 final FieldOrbit<Binary64> initialOrbit =
229 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
230
231 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
232
233 final FieldStateCovariance<Binary64> stateCovariance =
234 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
235 OrbitType.CARTESIAN, PositionAngleType.MEAN);
236
237
238 final FieldMatrix<Binary64> convertedCovarianceMatrixInNTW =
239 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.NTW).getMatrix();
240
241
242 final RealMatrix expectedCovarianceMatrixInNTWdouble = new BlockRealMatrix(new double[][] {
243 { 9.918792e-01, 6.679546e-03, -2.868345e-03, 2.6215894e-05, -1.035665e-03, -2.868345e-05 },
244 { 6.679546e-03, 1.013743e+00, -1.019560e-02, 1.193556e-03, 2.300019e-04, -1.019560e-04 },
245 {-2.868345e-03, -1.019560e-02, 9.943782e-01, -4.0015724e-05, -9.8767909e-05, 4.378217e-05 },
246 { 2.6215894e-05, 1.193556e-03, -4.0015724e-05, 1.58878e-06, 9.0271200e-07, -4.0015724e-07 },
247 {-1.035665e-03, 2.300019e-04, -9.8767909e-05, 9.0271200e-07, 3.4511471e-06, -9.8767909e-07 },
248 {-2.868345e-05, -1.019560e-04, 4.378217e-05, -4.0015724e-07, -9.8767909e-07, 4.378217e-07 },
249 });
250 final FieldMatrix<Binary64> expectedCovarianceMatrixInNTW = MatrixUtils.createFieldMatrix(field, 6, 6);
251 for (int i = 0; i < 6; i++) {
252 for (int j = 0; j < 6; j++) {
253 expectedCovarianceMatrixInNTW.setEntry(i, j, new Binary64(expectedCovarianceMatrixInNTWdouble.getEntry(i, j)));
254 }
255 }
256
257 compareCovariance(expectedCovarianceMatrixInNTW, convertedCovarianceMatrixInNTW, DEFAULT_VALLADO_THRESHOLD);
258
259 }
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275 @Test
276 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to PEF")
277 public void should_return_Vallado_PEF_covariance_matrix_from_ECI() {
278
279
280 Utils.setDataRoot("regular-data");
281
282
283
284 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
285 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
286 final Frame initialInertialFrame = FramesFactory.getGCRF();
287
288 final FieldOrbit<Binary64> initialOrbit =
289 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
290
291
292 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
293
294
295 final Frame outputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
296
297 final FieldStateCovariance<Binary64> stateCovariance =
298 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
299 OrbitType.CARTESIAN,
300 PositionAngleType.MEAN);
301
302 final FieldMatrix<Binary64> convertedCovarianceMatrixInITRF =
303 stateCovariance.changeCovarianceFrame(initialOrbit, outputFrame).getMatrix();
304
305
306 final FieldMatrix<Binary64> expectedCovarianceMatrixInITRF = new BlockFieldMatrix<>(new Binary64[][] {
307 { new Binary64(9.9340005761276870e-01), new Binary64(7.5124999798868530e-03),
308 new Binary64(5.8312675007359050e-03), new Binary64(3.4548396261054936e-05),
309 new Binary64(2.68512370468592e-06), new Binary64(5.8312677693153940e-05) },
310 { new Binary64(7.5124999798868025e-03), new Binary64(1.0065990293034541e+00),
311 new Binary64(1.2884310200351924e-02), new Binary64(1.4852736004690684e-04),
312 new Binary64(1.6544247282904867e-04), new Binary64(1.2884310644320954e-04) },
313 { new Binary64(5.8312675007359040e-03), new Binary64(1.2884310200351924e-02),
314 new Binary64(1.0000009130837746e+00), new Binary64(5.9252211072590390e-05),
315 new Binary64(1.2841787487219444e-04), new Binary64(1.0000913090989617e-04) },
316 { new Binary64(3.4548396261054936e-05), new Binary64(1.4852736004690686e-04),
317 new Binary64(5.9252211072590403e-05), new Binary64(3.5631474857130520e-07),
318 new Binary64(7.6083489184819870e-07), new Binary64(5.9252213790760030e-07) },
319 { new Binary64(2.685123704685915e-06), new Binary64(1.6544247282904864e-04),
320 new Binary64(1.2841787487219447e-04), new Binary64(7.6083489184819880e-07),
321 new Binary64(1.6542289254142709e-06), new Binary64(1.2841787929229964e-06) },
322 { new Binary64(5.8312677693153934e-05), new Binary64(1.2884310644320950e-04),
323 new Binary64(1.0000913090989616e-04), new Binary64(5.9252213790760020e-07),
324 new Binary64(1.2841787929229960e-06), new Binary64(1.0000913098203875e-06) }
325 });
326
327 compareCovariance(expectedCovarianceMatrixInITRF, convertedCovarianceMatrixInITRF, 1e-20);
328
329 }
330
331
332
333
334
335
336
337
338
339
340
341
342
343 @Test
344 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to RTN")
345 public void should_return_Vallado_RSW_covariance_matrix_from_ECI() {
346
347
348 Utils.setDataRoot("regular-data");
349
350
351 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
352 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
353 final Frame initialInertialFrame = FramesFactory.getGCRF();
354 final FieldOrbit<Binary64> initialOrbit =
355 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
356
357 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
358
359 final FieldStateCovariance<Binary64> stateCovariance =
360 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
361 OrbitType.CARTESIAN,
362 PositionAngleType.MEAN);
363
364 final FieldMatrix<Binary64> convertedCovarianceMatrixInRTN =
365 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW_INERTIAL).getMatrix();
366
367
368 final FieldMatrix<Binary64> expectedCovarianceMatrixInRTN = new BlockFieldMatrix<>(new Binary64[][] {
369 { new Binary64(9.918921e-001), new Binary64(6.700644e-003), new Binary64(-2.878187e-003),
370 new Binary64(1.892086e-005), new Binary64(6.700644e-005), new Binary64(-2.878187e-005) },
371 { new Binary64(6.700644e-003), new Binary64(1.013730e+000), new Binary64(-1.019283e-002),
372 new Binary64(6.700644e-005), new Binary64(2.372970e-004), new Binary64(-1.019283e-004) },
373 { new Binary64(-2.878187e-003), new Binary64(-1.019283e-002), new Binary64(9.943782e-001),
374 new Binary64(-2.878187e-005), new Binary64(-1.019283e-004), new Binary64(4.378217e-005) },
375 { new Binary64(1.892086e-005), new Binary64(6.700644e-005), new Binary64(-2.878187e-005),
376 new Binary64(1.892086e-007), new Binary64(6.700644e-007), new Binary64(-2.878187e-007) },
377 { new Binary64(6.700644e-005), new Binary64(2.372970e-004), new Binary64(-1.019283e-004),
378 new Binary64(6.700644e-007), new Binary64(2.372970e-006), new Binary64(-1.019283e-006) },
379 { new Binary64(-2.878187e-005), new Binary64(-1.019283e-004), new Binary64(4.378217e-005),
380 new Binary64(-2.878187e-007), new Binary64(-1.019283e-006), new Binary64(4.378217e-007) } });
381
382 compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
383
384 }
385
386
387
388
389
390
391
392
393
394
395
396
397
398 @Test
399 @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to RTN")
400 public void should_return_Vallado_RSW_non_inertial_covariance_matrix_from_ECI() {
401
402
403 Utils.setDataRoot("regular-data");
404
405
406 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
407 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
408 final Frame initialInertialFrame = FramesFactory.getGCRF();
409 final FieldOrbit<Binary64> initialOrbit =
410 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
411
412 final FieldMatrix<Binary64> initialCovarianceMatrix = getValladoInitialCovarianceMatrix();
413
414 final FieldStateCovariance<Binary64> stateCovariance =
415 new FieldStateCovariance<>(initialCovarianceMatrix, initialDate, initialInertialFrame,
416 OrbitType.CARTESIAN,
417 PositionAngleType.MEAN);
418
419 final FieldMatrix<Binary64> convertedCovarianceMatrixInRTN =
420 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
421
422
423 final FieldMatrix<Binary64> expectedCovarianceMatrixInRTN = new BlockFieldMatrix<>(new Binary64[][] {
424 { new Binary64(9.918921e-01), new Binary64(6.700644e-03), new Binary64(-2.878187e-03),
425 new Binary64(2.637186e-05), new Binary64(-1.035961e-03), new Binary64(-2.878187e-05) },
426 { new Binary64(6.700644e-03), new Binary64(1.013730e+00), new Binary64(-1.019283e-02),
427 new Binary64(1.194257e-03), new Binary64(2.298460e-04), new Binary64(-1.019283e-04) },
428 { new Binary64(-2.878187e-03), new Binary64(-1.019283e-02), new Binary64(9.943782e-01),
429 new Binary64(-4.011613e-05), new Binary64(-9.872780e-05), new Binary64(4.378217e-05) },
430 { new Binary64(2.637186e-05), new Binary64(1.194257e-03), new Binary64(-4.011613e-05),
431 new Binary64(1.591713e-06), new Binary64(9.046096e-07), new Binary64(-4.011613e-07) },
432 { new Binary64(-1.035961e-03), new Binary64(2.298460e-04), new Binary64(-9.872780e-05),
433 new Binary64(9.046096e-07), new Binary64(3.450431e-06), new Binary64(-9.872780e-07) },
434 { new Binary64(-2.878187e-05), new Binary64(-1.019283e-04), new Binary64(4.378217e-05),
435 new Binary64(-4.011613e-07), new Binary64(-9.872780e-07), new Binary64(4.378217e-07) }
436 });
437
438 compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
439
440 }
441
442
443
444
445
446
447
448
449 @Test
450 @DisplayName("Test covariance conversion Vallado test case : PEF cartesian to ECI")
451 public void should_return_Vallado_ECI_covariance_matrix_from_PEF() {
452
453
454 Utils.setDataRoot("regular-data");
455
456
457 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
458 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
459 final Frame initialInertialFrame = FramesFactory.getGCRF();
460 final FieldOrbit<Binary64> initialOrbit =
461 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
462
463 final FieldMatrix<Binary64> initialCovarianceMatrixInPEF = new BlockFieldMatrix<>(new Binary64[][] {
464 { new Binary64(9.9340005761276870e-01), new Binary64(7.5124999798868530e-03),
465 new Binary64(5.8312675007359050e-03), new Binary64(3.4548396261054936e-05),
466 new Binary64(2.6851237046859200e-06), new Binary64(5.8312677693153940e-05) },
467 { new Binary64(7.5124999798868025e-03), new Binary64(1.0065990293034541e+00),
468 new Binary64(1.2884310200351924e-02), new Binary64(1.4852736004690684e-04),
469 new Binary64(1.6544247282904867e-04), new Binary64(1.2884310644320954e-04) },
470 { new Binary64(5.8312675007359040e-03), new Binary64(1.2884310200351924e-02),
471 new Binary64(1.0000009130837746e+00), new Binary64(5.9252211072590390e-05),
472 new Binary64(1.2841787487219444e-04), new Binary64(1.0000913090989617e-04) },
473 { new Binary64(3.4548396261054936e-05), new Binary64(1.4852736004690686e-04),
474 new Binary64(5.9252211072590403e-05), new Binary64(3.5631474857130520e-07),
475 new Binary64(7.6083489184819870e-07), new Binary64(5.9252213790760030e-07) },
476 { new Binary64(2.6851237046859150e-06), new Binary64(1.6544247282904864e-04),
477 new Binary64(1.2841787487219447e-04), new Binary64(7.6083489184819880e-07),
478 new Binary64(1.6542289254142709e-06), new Binary64(1.2841787929229964e-06) },
479 { new Binary64(5.8312677693153934e-05), new Binary64(1.2884310644320950e-04),
480 new Binary64(1.0000913090989616e-04), new Binary64(5.9252213790760020e-07),
481 new Binary64(1.2841787929229960e-06), new Binary64(1.0000913098203875e-06) }
482 });
483
484 final Frame inputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, false);
485
486
487 final FieldStateCovariance<Binary64> stateCovariance =
488 new FieldStateCovariance<>(initialCovarianceMatrixInPEF, initialDate, inputFrame, OrbitType.CARTESIAN,
489 PositionAngleType.MEAN);
490
491
492 final FieldMatrix<Binary64> convertedCovarianceMatrixInECI =
493 stateCovariance.changeCovarianceFrame(initialOrbit, initialInertialFrame).getMatrix();
494
495
496 final FieldMatrix<Binary64> expectedCovarianceMatrixInECI = getValladoInitialCovarianceMatrix();
497
498 compareCovariance(expectedCovarianceMatrixInECI, convertedCovarianceMatrixInECI, 1e-7);
499
500 }
501
502 @Test
503 @DisplayName("Test covariance conversion from RTN local orbital frame to inertial frame")
504 public void should_rotate_covariance_matrix_by_minus_ninety_degrees() {
505
506
507 final FieldAbsoluteDate<Binary64> initialDate = new FieldAbsoluteDate<>(field);
508 final Frame initialInertialFrame = FramesFactory.getGCRF();
509 final Binary64 mu = field.getOne().multiply(398600e9);
510
511 final FieldPVCoordinates<Binary64> initialPV = new FieldPVCoordinates<>(field, new PVCoordinates(
512 new Vector3D(0, 6778000, 0),
513 new Vector3D(-7668.63, 0, 0)));
514
515 final FieldOrbit<Binary64> initialOrbit =
516 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, mu);
517
518 final FieldMatrix<Binary64> initialCovarianceInRTN = new BlockFieldMatrix<>(new Binary64[][] {
519 { new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0) },
520 { new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(-1e-5) },
521 { new Binary64(0), new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0) },
522 { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3), new Binary64(0), new Binary64(0) },
523 { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3), new Binary64(0) },
524 { new Binary64(0), new Binary64(-1e-5), new Binary64(0), new Binary64(0), new Binary64(0),
525 new Binary64(1e-3) }
526 });
527
528 final FieldStateCovariance<Binary64> stateCovariance =
529 new FieldStateCovariance<>(initialCovarianceInRTN, initialDate, LOFType.QSW);
530
531
532 final FieldMatrix<Binary64> convertedCovarianceMatrixInInertialFrame =
533 stateCovariance.changeCovarianceFrame(initialOrbit, initialInertialFrame).getMatrix();
534
535
536
537
538 final FieldMatrix<Binary64> expectedMatrixInInertialFrame = new BlockFieldMatrix<>(new Binary64[][] {
539 { new Binary64(1.000000e+00), new Binary64(0.000000e+00), new Binary64(0.000000e+00),
540 new Binary64(0.000000e+00), new Binary64(1.131400e-03), new Binary64(1.000000e-05) },
541 { new Binary64(0.000000e+00), new Binary64(1.000000e+00), new Binary64(0.000000e+00),
542 new Binary64(-1.131400e-03), new Binary64(0.000000e+00), new Binary64(0.000000e+00) },
543 { new Binary64(0.000000e+00), new Binary64(0.000000e+00), new Binary64(1.000000e+00),
544 new Binary64(0.000000e+00), new Binary64(0.000000e+00), new Binary64(0.000000e+00) },
545 { new Binary64(0.000000e+00), new Binary64(-1.131400e-03), new Binary64(0.000000e+00),
546 new Binary64(1.001280e-03), new Binary64(0.000000e+00), new Binary64(0.000000e+00) },
547 { new Binary64(1.131400e-03), new Binary64(0.000000e+00), new Binary64(0.000000e+00),
548 new Binary64(0.000000e+00), new Binary64(1.001280e-03), new Binary64(1.131400e-08) },
549 { new Binary64(1.000000e-05), new Binary64(0.000000e+00), new Binary64(0.000000e+00),
550 new Binary64(0.000000e+00), new Binary64(1.131400e-08), new Binary64(1.000000e-03) },
551 });
552
553 compareCovariance(expectedMatrixInInertialFrame, convertedCovarianceMatrixInInertialFrame,
554 DEFAULT_VALLADO_THRESHOLD);
555
556 }
557
558
559
560
561
562
563
564
565
566 @Test
567 @DisplayName("Test covariance conversion from Vallado test case NTW to RSW")
568 public void should_convert_Vallado_NTW_to_RSW() {
569
570
571 Utils.setDataRoot("regular-data");
572
573
574 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
575 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
576 final Frame initialInertialFrame = FramesFactory.getGCRF();
577 final FieldOrbit<Binary64> initialOrbit =
578 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
579
580 final FieldMatrix<Binary64> initialCovarianceMatrixInNTW = new BlockFieldMatrix<>(new Binary64[][] {
581 { new Binary64(9.918792e-001), new Binary64(6.679546e-003), new Binary64(-2.868345e-003),
582 new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005) },
583 { new Binary64(6.679546e-003), new Binary64(1.013743e+000), new Binary64(-1.019560e-002),
584 new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004) },
585 { new Binary64(-2.868345e-003), new Binary64(-1.019560e-002), new Binary64(9.943782e-001),
586 new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005) },
587 { new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005),
588 new Binary64(1.879167e-007), new Binary64(6.679546e-007), new Binary64(-2.868345e-007) },
589 { new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004),
590 new Binary64(6.679546e-007), new Binary64(2.374262e-006), new Binary64(-1.019560e-006) },
591 { new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005),
592 new Binary64(-2.868345e-007), new Binary64(-1.019560e-006), new Binary64(4.378217e-007) }
593 });
594
595 final FieldStateCovariance<Binary64> stateCovariance =
596 new FieldStateCovariance<>(initialCovarianceMatrixInNTW, initialDate, LOFType.NTW);
597
598
599 final FieldMatrix<Binary64> convertedCovarianceMatrixInRTN =
600 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
601
602
603 final RealMatrix expectedCovarianceMatrixInRTNDouble = new BlockRealMatrix(new double[][] {
604 { 9.918921e-001, 6.700644e-003, -2.878187e-003, 1.8924189e-005, 6.651362e-005, -2.878187e-005 },
605 { 6.700644e-003, 1.013730e+000, -1.019283e-002, 6.7510094e-005, 2.3729368e-004, -1.01928257e-004 },
606 { -2.878187e-003, -1.019283e-002, 9.943782e-001, -2.8786942e-005, -1.01926827e-004, 4.378217e-005 },
607 { 1.8924189e-005, 6.7510094e-005, -2.8786942e-005, 1.89275434e-007, 6.7017283e-007, -2.8786942e-007 },
608 { 6.651362e-005, 2.3729368e-004, -1.01926827e-004, 6.7017283e-007, 2.372903e-006, -1.0192682e-006 },
609 { -2.878187e-005, -1.01928257e-004, 4.378217e-005, -2.87869424e-007, -1.0192682e-006, 4.378217e-007 }
610 });
611 final FieldMatrix<Binary64> expectedCovarianceMatrixInRTN = MatrixUtils.createFieldMatrix(field, 6, 6);
612 for (int i = 0; i < 6; i++) {
613 for (int j = 0; j < 6; j++) {
614 expectedCovarianceMatrixInRTN.setEntry(i, j, new Binary64(expectedCovarianceMatrixInRTNDouble.getEntry(i, j)));
615 }
616 }
617
618 compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
619
620 }
621
622 @Test
623 @DisplayName("Test covariance conversion from inertial frame to RTN local orbital frame")
624 public void should_rotate_covariance_matrix_by_ninety_degrees() {
625
626
627 final FieldAbsoluteDate<Binary64> initialDate = new FieldAbsoluteDate<>(field);
628 final Frame initialInertialFrame = FramesFactory.getGCRF();
629 final Binary64 mu = field.getOne().multiply(398600e9);
630
631 final FieldPVCoordinates<Binary64> initialPV = new FieldPVCoordinates<>(field, new PVCoordinates(
632 new Vector3D(0, 6778000, 0),
633 new Vector3D(-7668.63, 0, 0)));
634
635 final FieldOrbit<Binary64> initialOrbit =
636 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, mu);
637
638 final FieldMatrix<Binary64> initialCovarianceInInertialFrame = new BlockFieldMatrix<>(new Binary64[][] {
639 { new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-5) },
640 { new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0) },
641 { new Binary64(0), new Binary64(0), new Binary64(1), new Binary64(0), new Binary64(0), new Binary64(0) },
642 { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3), new Binary64(0), new Binary64(0) },
643 { new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(1e-3), new Binary64(0) },
644 { new Binary64(1e-5), new Binary64(0), new Binary64(0), new Binary64(0), new Binary64(0),
645 new Binary64(1e-3) }
646 });
647
648 final FieldStateCovariance<Binary64> stateCovariance =
649 new FieldStateCovariance<>(initialCovarianceInInertialFrame, initialDate, initialInertialFrame,
650 OrbitType.CARTESIAN, PositionAngleType.MEAN);
651
652
653 final FieldMatrix<Binary64> convertedCovarianceMatrixInRTN =
654 stateCovariance.changeCovarianceFrame(initialOrbit, LOFType.QSW).getMatrix();
655
656
657
658 final FieldMatrix<Binary64> expectedMatrixInRTN = new BlockFieldMatrix<>(new Binary64[][] {
659 { new Binary64(1.000000e+00), new Binary64(0.000000e+00), new Binary64(0.000000e+00),
660 new Binary64(0.000000e+00), new Binary64(-1.131400e-03), new Binary64(0.000000e+00) },
661 { new Binary64(0.000000e+00), new Binary64(1.000000e+00), new Binary64(0.000000e+00),
662 new Binary64(1.131400e-03), new Binary64(0.000000e+00), new Binary64(-1.000000e-05) },
663 { new Binary64(0.000000e+00), new Binary64(0.000000e+00), new Binary64(1.000000e+00),
664 new Binary64(0.000000e+00), new Binary64(0.000000e+00), new Binary64(0.000000e+00) },
665 { new Binary64(0.000000e+00), new Binary64(1.131400e-03), new Binary64(0.000000e+00),
666 new Binary64(1.001280e-03), new Binary64(0.000000e+00), new Binary64(-1.131400e-08) },
667 { new Binary64(-1.131400e-03), new Binary64(0.000000e+00), new Binary64(0.000000e+00),
668 new Binary64(0.000000e+00), new Binary64(1.001280e-03), new Binary64(0.000000e+00) },
669 { new Binary64(0.000000e+00), new Binary64(-1.000000e-05), new Binary64(0.000000e+00),
670 new Binary64(-1.131400e-08), new Binary64(0.000000e+00), new Binary64(1.000000e-03) },
671 });
672
673 compareCovariance(expectedMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
674 }
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690 @Test
691 @DisplayName("Test custom covariance conversion Vallado test case : GCRF -> TEME -> IRTF -> NTW -> RTN -> ITRF -> GCRF")
692 public void should_return_initial_covariance_after_multiple_conversion() {
693
694
695 Utils.setDataRoot("regular-data");
696
697
698 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
699 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
700 final Frame initialInertialFrame = FramesFactory.getGCRF();
701 final FieldOrbit<Binary64> initialOrbit =
702 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
703
704 final FieldMatrix<Binary64> initialCovarianceMatrixInGCRF = getValladoInitialCovarianceMatrix();
705
706 final Frame teme = FramesFactory.getTEME();
707
708 final Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false);
709
710 final FieldStateCovariance<Binary64> stateCovariance =
711 new FieldStateCovariance<>(initialCovarianceMatrixInGCRF, initialDate, initialInertialFrame,
712 OrbitType.CARTESIAN, PositionAngleType.MEAN);
713
714
715
716 final FieldStateCovariance<Binary64> convertedCovarianceMatrixInTEME =
717 stateCovariance.changeCovarianceFrame(initialOrbit, teme);
718
719
720 final FieldStateCovariance<Binary64> convertedCovarianceMatrixInITRF =
721 convertedCovarianceMatrixInTEME.changeCovarianceFrame(initialOrbit, itrf);
722
723
724 final FieldStateCovariance<Binary64> convertedCovarianceMatrixInNTW =
725 convertedCovarianceMatrixInITRF.changeCovarianceFrame(initialOrbit, LOFType.NTW);
726
727
728 final FieldStateCovariance<Binary64> convertedCovarianceMatrixInRTN =
729 convertedCovarianceMatrixInNTW.changeCovarianceFrame(initialOrbit, LOFType.QSW);
730
731
732 final FieldStateCovariance<Binary64> convertedCovarianceMatrixBackInITRF =
733 convertedCovarianceMatrixInRTN.changeCovarianceFrame(initialOrbit, itrf);
734
735
736 final FieldStateCovariance<Binary64> convertedCovarianceMatrixBackInTEME =
737 convertedCovarianceMatrixBackInITRF.changeCovarianceFrame(initialOrbit, teme);
738
739
740 final FieldStateCovariance<Binary64> convertedCovarianceMatrixInGCRF =
741 convertedCovarianceMatrixBackInTEME.changeCovarianceFrame(initialOrbit, initialInertialFrame);
742
743
744 compareCovariance(initialCovarianceMatrixInGCRF, convertedCovarianceMatrixInGCRF.getMatrix(), 1e-12);
745
746 }
747
748
749
750
751
752
753
754 @Test
755 @DisplayName("Test shiftedBy method of StateCovariance")
756 public void should_return_expected_shifted_state_covariance() {
757
758
759 Utils.setDataRoot("regular-data");
760
761
762 final FieldAbsoluteDate<Binary64> initialDate = new FieldAbsoluteDate<>(field);
763 final Frame inertialFrame = FramesFactory.getGCRF();
764 final Frame nonInertialFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
765 final FieldPVCoordinates<Binary64> pv = getValladoInitialPV(field);
766 final Binary64 mu = field.getOne().multiply(398600e9);
767 final FieldOrbit<Binary64> initialOrbit =
768 new FieldCartesianOrbit<>(pv, inertialFrame, initialDate, mu);
769
770 final Binary64 timeShift = field.getOne().multiply(300);
771
772
773 final FieldStateCovariance<Binary64> initialCovarianceInCartesian =
774 new FieldStateCovariance<>(getValladoInitialCovarianceMatrix(), initialDate, inertialFrame,
775 OrbitType.CARTESIAN, PositionAngleType.MEAN);
776
777 final FieldStateCovariance<Binary64> covarianceInEquinoctial =
778 initialCovarianceInCartesian.changeCovarianceType(initialOrbit, OrbitType.EQUINOCTIAL,
779 PositionAngleType.MEAN);
780
781 final FieldStateCovariance<Binary64> covarianceInCartesianInLOF =
782 initialCovarianceInCartesian.changeCovarianceFrame(initialOrbit, LOFType.QSW);
783
784 final FieldStateCovariance<Binary64> covarianceInCartesianInNonInertial =
785 initialCovarianceInCartesian.changeCovarianceFrame(initialOrbit, nonInertialFrame);
786
787
788 final FieldStateCovariance<Binary64> shiftedCovarianceInEquinoctial =
789 covarianceInEquinoctial.shiftedBy(field, initialOrbit, timeShift);
790 final FieldMatrix<Binary64> shiftedCovarianceInEquinoctialBackToInitial =
791 shiftedCovarianceInEquinoctial.changeCovarianceType(initialOrbit.shiftedBy(timeShift),
792 OrbitType.CARTESIAN, PositionAngleType.MEAN)
793 .getMatrix();
794
795 final FieldStateCovariance<Binary64> shiftedCovarianceInCartesianInLOF =
796 covarianceInCartesianInLOF.shiftedBy(field, initialOrbit, timeShift);
797 final FieldMatrix<Binary64> shiftedCovarianceInCartesianInLOFBackToInitial =
798 shiftedCovarianceInCartesianInLOF.changeCovarianceFrame(initialOrbit.shiftedBy(timeShift),
799 inertialFrame)
800 .getMatrix();
801
802 final FieldStateCovariance<Binary64> shiftedCovarianceInCartesianInNonInertial =
803 covarianceInCartesianInNonInertial.shiftedBy(field, initialOrbit, timeShift);
804 final FieldMatrix<Binary64> shiftedCovarianceInCartesianInNonInertialBackToInitial =
805 shiftedCovarianceInCartesianInNonInertial.changeCovarianceFrame(initialOrbit.shiftedBy(timeShift),
806 inertialFrame)
807 .getMatrix();
808
809
810
811 final FieldMatrix<Binary64> stm = MatrixUtils.createFieldIdentityMatrix(field, 6);
812 final Binary64 sma = initialOrbit.getA();
813 final Binary64 contribution = mu.divide(sma.pow(5)).sqrt().multiply(timeShift).multiply(-1.5);
814 stm.setEntry(5, 0, contribution);
815
816 final FieldStateCovariance<Binary64> initialCovarianceInKeplerian =
817 initialCovarianceInCartesian.changeCovarianceType(initialOrbit, OrbitType.KEPLERIAN,
818 PositionAngleType.MEAN);
819 final FieldMatrix<Binary64> referenceCovarianceMatrixInKeplerian =
820 stm.multiply(initialCovarianceInKeplerian.getMatrix().multiplyTransposed(stm));
821
822 final FieldMatrix<Binary64> referenceCovarianceMatrixInCartesian =
823 new FieldStateCovariance<>(referenceCovarianceMatrixInKeplerian, initialDate.shiftedBy(timeShift),
824 inertialFrame, OrbitType.KEPLERIAN, PositionAngleType.MEAN).changeCovarianceType(
825 initialOrbit.shiftedBy(timeShift), OrbitType.CARTESIAN, PositionAngleType.MEAN).getMatrix();
826
827
828 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInEquinoctialBackToInitial, 1e-7);
829 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInCartesianInLOFBackToInitial, 1e-7);
830 compareCovariance(referenceCovarianceMatrixInCartesian, shiftedCovarianceInCartesianInNonInertialBackToInitial,
831 1e-7);
832
833 }
834
835
836
837
838
839
840
841
842
843 @Test
844 @DisplayName("Test thrown error if input frame is not pseudo-inertial and "
845 + "the covariance matrix is not expressed in cartesian elements")
846 public void should_return_orekit_exception() {
847
848
849 Utils.setDataRoot("regular-data");
850
851
852 final FieldAbsoluteDate<Binary64> initialDate = getValladoInitialDate(field);
853 final FieldPVCoordinates<Binary64> initialPV = getValladoInitialPV(field);
854 final Frame initialInertialFrame = FramesFactory.getGCRF();
855 final FieldOrbit<Binary64> initialOrbit =
856 new FieldCartesianOrbit<>(initialPV, initialInertialFrame, initialDate, getValladoMu(field));
857
858 final FieldMatrix<Binary64> randomCovarianceMatrix = new BlockFieldMatrix<>(new Binary64[][] {
859 { new Binary64(9.918792e-001), new Binary64(6.679546e-003), new Binary64(-2.868345e-003),
860 new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005) },
861 { new Binary64(6.679546e-003), new Binary64(1.013743e+000), new Binary64(-1.019560e-002),
862 new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004) },
863 { new Binary64(-2.868345e-003), new Binary64(-1.019560e-002), new Binary64(9.943782e-001),
864 new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005) },
865 { new Binary64(1.879167e-005), new Binary64(6.679546e-005), new Binary64(-2.868345e-005),
866 new Binary64(1.879167e-007), new Binary64(6.679546e-007), new Binary64(-2.868345e-007) },
867 { new Binary64(6.679546e-005), new Binary64(2.374262e-004), new Binary64(-1.019560e-004),
868 new Binary64(6.679546e-007), new Binary64(2.374262e-006), new Binary64(-1.019560e-006) },
869 { new Binary64(-2.868345e-005), new Binary64(-1.019560e-004), new Binary64(4.378217e-005),
870 new Binary64(-2.868345e-007), new Binary64(-1.019560e-006), new Binary64(4.378217e-007) } });
871
872 final Frame nonInertialFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
873
874 final Frame inertialFrame = FramesFactory.getGCRF();
875
876
877 Assertions.assertThrows(OrekitException.class,
878 () -> new FieldStateCovariance<>(randomCovarianceMatrix, initialDate, nonInertialFrame,
879 OrbitType.CIRCULAR,
880 PositionAngleType.MEAN).changeCovarianceFrame(initialOrbit,
881 inertialFrame));
882
883 Assertions.assertThrows(OrekitException.class,
884 () -> new FieldStateCovariance<>(randomCovarianceMatrix, initialDate, nonInertialFrame,
885 OrbitType.EQUINOCTIAL,
886 PositionAngleType.MEAN).changeCovarianceFrame(initialOrbit,
887 LOFType.QSW));
888
889 Assertions.assertThrows(OrekitException.class,
890 () -> new FieldStateCovariance<>(randomCovarianceMatrix, initialDate, nonInertialFrame,
891 OrbitType.EQUINOCTIAL,
892 PositionAngleType.MEAN).changeCovarianceType(initialOrbit,
893 OrbitType.KEPLERIAN,
894 PositionAngleType.MEAN));
895
896 Assertions.assertThrows(OrekitException.class,
897 () -> new FieldStateCovariance<>(randomCovarianceMatrix, initialDate,
898 LOFType.QSW).changeCovarianceType(
899 initialOrbit, OrbitType.KEPLERIAN, PositionAngleType.MEAN));
900
901 Assertions.assertThrows(OrekitException.class,
902 () -> new FieldStateCovariance<>(randomCovarianceMatrix, initialDate, nonInertialFrame,
903 OrbitType.CARTESIAN,
904 PositionAngleType.MEAN).changeCovarianceType(initialOrbit,
905 OrbitType.KEPLERIAN,
906 PositionAngleType.MEAN));
907
908 }
909
910 private FieldMatrix<Binary64> getValladoInitialCovarianceMatrix() {
911 return new BlockFieldMatrix<>(new Binary64[][] {
912 { new Binary64(1), new Binary64(1e-2), new Binary64(1e-2), new Binary64(1e-4), new Binary64(1e-4),
913 new Binary64(1e-4) },
914 { new Binary64(1e-2), new Binary64(1), new Binary64(1e-2), new Binary64(1e-4), new Binary64(1e-4),
915 new Binary64(1e-4) },
916 { new Binary64(1e-2), new Binary64(1e-2), new Binary64(1), new Binary64(1e-4), new Binary64(1e-4),
917 new Binary64(1e-4) },
918 { new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-6), new Binary64(1e-6),
919 new Binary64(1e-6) },
920 { new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-6), new Binary64(1e-6),
921 new Binary64(1e-6) },
922 { new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-4), new Binary64(1e-6), new Binary64(1e-6),
923 new Binary64(1e-6) }
924 });
925 }
926
927
928
929
930
931
932
933
934 private <T extends CalculusFieldElement<T>> void compareCovariance(final FieldMatrix<T> reference,
935 final FieldMatrix<T> computed,
936 final double threshold) {
937 for (int row = 0; row < reference.getRowDimension(); row++) {
938 for (int column = 0; column < reference.getColumnDimension(); column++) {
939 if (reference.getEntry(row, column).getReal() == 0) {
940 Assertions.assertEquals(reference.getEntry(row, column).getReal(),
941 computed.getEntry(row, column).getReal(),
942 threshold);
943 }
944 else {
945 Assertions.assertEquals(reference.getEntry(row, column).getReal(),
946 computed.getEntry(row, column).getReal(),
947 FastMath.abs(threshold * reference.getEntry(row, column).getReal()));
948 }
949 }
950 }
951 }
952
953 private <T extends CalculusFieldElement<T>> FieldAbsoluteDate<T> getValladoInitialDate(final Field<T> field) {
954 return new FieldAbsoluteDate<>(field, 2000, 12, 15, 16, 58, 50.208, TimeScalesFactory.getUTC());
955 }
956
957 private <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> getValladoInitialPV(final Field<T> field) {
958 return new FieldPVCoordinates<>(field,
959 new PVCoordinates(new Vector3D(-605792.21660, -5870229.51108, 3493053.19896),
960 new Vector3D(-1568.25429, -3702.34891, -6479.48395)));
961 }
962
963 private <T extends CalculusFieldElement<T>> T getValladoMu(final Field<T> field) {
964 return field.getOne().multiply(Constants.IERS2010_EARTH_MU);
965 }
966
967 @Test
968 @DisplayName("Test getters")
969 void should_return_mocks() {
970
971
972 Utils.setDataRoot("regular-data");
973
974
975 @SuppressWarnings("unchecked")
976 final FieldAbsoluteDate<Binary64> initialDate = Mockito.mock(FieldAbsoluteDate.class);
977 final Frame initialFrame = Mockito.mock(Frame.class);
978 @SuppressWarnings("unchecked")
979 final FieldMatrix<Binary64> initialCovarianceMatrixInGCRF = Mockito.mock(BlockFieldMatrix.class);
980
981 final FieldStateCovariance<Binary64> stateCovariance =
982 new FieldStateCovariance<>(initialCovarianceMatrixInGCRF, initialDate, initialFrame,
983 OrbitType.CARTESIAN, PositionAngleType.MEAN);
984
985
986 final FieldAbsoluteDate<Binary64> gottenDate = stateCovariance.getDate();
987 final Frame gottenFrame = stateCovariance.getFrame();
988 final LOF gottenLOF = stateCovariance.getLOF();
989 final OrbitType gottenOrbitType = stateCovariance.getOrbitType();
990 final PositionAngleType gottenPositionAngleType = stateCovariance.getPositionAngleType();
991
992
993 Assertions.assertEquals(initialDate, gottenDate);
994 Assertions.assertEquals(initialFrame, gottenFrame);
995 Assertions.assertNull(gottenLOF);
996 Assertions.assertEquals(OrbitType.CARTESIAN, gottenOrbitType);
997 Assertions.assertEquals(PositionAngleType.MEAN, gottenPositionAngleType);
998
999 }
1000
1001 @Test
1002 void testConversionToNonFieldEquivalentLOF() {
1003
1004
1005 final int dim = 6;
1006 final Field<Binary64> field = Binary64Field.getInstance();
1007
1008 final FieldMatrix<Binary64> matrix = MatrixUtils.createFieldMatrix(field, dim, dim);
1009 final FieldAbsoluteDate<Binary64> fieldDate = new FieldAbsoluteDate<>(field);
1010 final LOF lofMock = Mockito.mock(LOF.class);
1011
1012 final FieldStateCovariance<Binary64> fieldStateCovariance = new FieldStateCovariance<>(matrix, fieldDate, lofMock);
1013
1014
1015 final StateCovariance stateCovariance = fieldStateCovariance.toStateCovariance();
1016
1017
1018
1019 final RealMatrix covarianceMatrix = stateCovariance.getMatrix();
1020 for (int i = 0; i < dim; i++) {
1021 for (int j = 0; j < dim; j++) {
1022 Assertions.assertEquals(matrix.getEntry(i, j).getReal(), covarianceMatrix.getEntry(i, j));
1023 }
1024 }
1025
1026
1027 Assertions.assertEquals(fieldDate.getDate().toAbsoluteDate(), stateCovariance.getDate());
1028
1029
1030 Assertions.assertEquals(lofMock, stateCovariance.getLOF());
1031
1032 }
1033
1034 @Test
1035 void testConversionToNonFieldEquivalentFrame() {
1036
1037
1038 final int dim = 6;
1039 final Field<Binary64> field = Binary64Field.getInstance();
1040
1041 final FieldMatrix<Binary64> matrix = MatrixUtils.createFieldMatrix(field, dim, dim);
1042 final FieldAbsoluteDate<Binary64> fieldDate = new FieldAbsoluteDate<>(field);
1043 final Frame frameMock = Mockito.mock(Frame.class);
1044 final OrbitType orbitType = OrbitType.CARTESIAN;
1045 final PositionAngleType positionAngleType = PositionAngleType.MEAN;
1046
1047 final FieldStateCovariance<Binary64> fieldStateCovariance =
1048 new FieldStateCovariance<>(matrix, fieldDate, frameMock, orbitType, positionAngleType);
1049
1050
1051 final StateCovariance stateCovariance = fieldStateCovariance.toStateCovariance();
1052
1053
1054
1055 final RealMatrix covarianceMatrix = stateCovariance.getMatrix();
1056 for (int i = 0; i < dim; i++) {
1057 for (int j = 0; j < dim; j++) {
1058 Assertions.assertEquals(matrix.getEntry(i, j).getReal(), covarianceMatrix.getEntry(i, j));
1059 }
1060 }
1061
1062
1063 Assertions.assertEquals(fieldDate.getDate().toAbsoluteDate(), stateCovariance.getDate());
1064
1065
1066 Assertions.assertEquals(frameMock, stateCovariance.getFrame());
1067
1068
1069 Assertions.assertEquals(orbitType, stateCovariance.getOrbitType());
1070
1071
1072 Assertions.assertEquals(positionAngleType, stateCovariance.getPositionAngleType());
1073
1074 }
1075
1076 @Test
1077 public void testIssue1485CartesianOrbitTypeAndNullAngleType() {
1078
1079 final Field<Binary64> field = Binary64Field.getInstance();
1080
1081
1082 doTestIssue1485CartesianOrbitTypeAndNullAngleType(field);
1083 }
1084
1085 <T extends CalculusFieldElement<T>> void doTestIssue1485CartesianOrbitTypeAndNullAngleType(final Field<T> field) {
1086
1087 final T one = field.getOne();
1088 final FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, AbsoluteDate.ARBITRARY_EPOCH);
1089 final FieldMatrix<T> expectedMatrix = MatrixUtils.createFieldIdentityMatrix(field, 6);
1090 final PositionAngleType anomalyType = PositionAngleType.MEAN;
1091 final Frame inFrame = FramesFactory.getEME2000();
1092 final Frame outFrame = FramesFactory.getGCRF();
1093 final double mu = Constants.EGM96_EARTH_MU;
1094
1095 final FieldKeplerianOrbit<T> orbit =
1096 new FieldKeplerianOrbit<>(one.multiply(1e7), one.multiply(0.01), one.multiply(1.), one.multiply(2.),
1097 one.multiply(3.), one.multiply(4.), anomalyType, inFrame, date, one.multiply(mu));
1098
1099 final FieldStateCovariance<T> originalCovariance =
1100 new FieldStateCovariance<>(expectedMatrix, date, inFrame, OrbitType.CARTESIAN, null);
1101
1102
1103 assertDoesNotThrow(() -> originalCovariance.changeCovarianceFrame(orbit, outFrame));
1104
1105
1106 final FieldStateCovariance<T> convertedCovariance = originalCovariance.changeCovarianceFrame(orbit, outFrame);
1107 assertEquals(getNorm1(originalCovariance.getMatrix()), getNorm1(convertedCovariance.getMatrix()));
1108 }
1109
1110 @Test
1111 void testIssue1485SameLOF() {
1112
1113 final Field<Binary64> field = Binary64Field.getInstance();
1114 final LOF lof = LOFType.TNW;
1115
1116
1117 doTestIssue1485SameFrameDefinition(field, lof);
1118 }
1119
1120 @Test
1121 void testIssue1485SameFrame() {
1122
1123 final Field<Binary64> field = Binary64Field.getInstance();
1124 final Frame eme2000 = FramesFactory.getEME2000();
1125
1126
1127 doTestIssue1485SameFrameDefinition(field, eme2000);
1128 }
1129
1130 <T, I extends CalculusFieldElement<I>> void doTestIssue1485SameFrameDefinition(Field<I> field, T frameOrLOF) {
1131
1132 final I one = field.getOne();
1133 final FieldAbsoluteDate<I> date = new FieldAbsoluteDate<>(field, AbsoluteDate.ARBITRARY_EPOCH);
1134 final FieldMatrix<I> expectedMatrix = MatrixUtils.createFieldIdentityMatrix(field, 6);
1135 final PositionAngleType anomalyType = PositionAngleType.MEAN;
1136 final Frame frame = FramesFactory.getEME2000();
1137 final double mu = Constants.EGM96_EARTH_MU;
1138 final FieldKeplerianOrbit<I> orbit =
1139 new FieldKeplerianOrbit<>(one.multiply(1e7), one.multiply(0.01), one.multiply(1.), one.multiply(2.),
1140 one.multiply(3.), one.multiply(4.), anomalyType, frame, date, one.multiply(mu));
1141
1142 final FieldStateCovariance<I> originalCovariance;
1143 final FieldStateCovariance<I> covariance;
1144 if (frameOrLOF instanceof LOF) {
1145 originalCovariance = new FieldStateCovariance<>(expectedMatrix, date, (LOF) frameOrLOF);
1146 covariance = originalCovariance.changeCovarianceFrame(orbit, (LOF) frameOrLOF);
1147 } else {
1148 originalCovariance =
1149 new FieldStateCovariance<>(expectedMatrix, date, (Frame) frameOrLOF, orbit.getType(), anomalyType);
1150 covariance = originalCovariance.changeCovarianceFrame(orbit, (Frame) frameOrLOF);
1151 }
1152
1153
1154 final FieldMatrix<I> actualMatrix = covariance.getMatrix();
1155 Assertions.assertEquals(0, getNorm1(actualMatrix.subtract(expectedMatrix)));
1156 }
1157
1158
1159
1160
1161
1162
1163
1164
1165 private <I extends CalculusFieldElement<I>> double getNorm1(final FieldMatrix<I> fieldMatrix) {
1166
1167 return Arrays.stream(
1168 Arrays.stream(fieldMatrix.getData())
1169 .flatMap(Arrays::stream)
1170 .mapToDouble(FieldElement::getReal)
1171 .toArray())
1172 .sum();
1173 }
1174
1175 }