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