1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.sequential;
18
19 import static org.junit.Assert.assertArrayEquals;
20
21 import java.util.Arrays;
22 import java.util.Locale;
23
24 import org.hipparchus.analysis.UnivariateFunction;
25 import org.hipparchus.analysis.polynomials.PolynomialFunction;
26 import org.hipparchus.linear.Array2DRowRealMatrix;
27 import org.hipparchus.linear.ArrayRealVector;
28 import org.hipparchus.linear.MatrixUtils;
29 import org.hipparchus.linear.RealMatrix;
30 import org.hipparchus.linear.RealVector;
31 import org.hipparchus.random.CorrelatedRandomVectorGenerator;
32 import org.hipparchus.random.GaussianRandomGenerator;
33 import org.hipparchus.random.Well1024a;
34 import org.hipparchus.stat.descriptive.StreamingStatistics;
35 import org.hipparchus.util.FastMath;
36 import org.junit.Assert;
37 import org.junit.Test;
38 import org.orekit.errors.OrekitIllegalArgumentException;
39 import org.orekit.estimation.Context;
40 import org.orekit.estimation.EstimationTestUtils;
41 import org.orekit.estimation.Force;
42 import org.orekit.frames.LOFType;
43 import org.orekit.frames.Transform;
44 import org.orekit.orbits.OrbitType;
45 import org.orekit.orbits.PositionAngle;
46 import org.orekit.propagation.Propagator;
47 import org.orekit.propagation.SpacecraftState;
48 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
49 import org.orekit.utils.CartesianDerivativesFilter;
50 import org.orekit.utils.PVCoordinates;
51 import org.orekit.utils.TimeStampedPVCoordinates;
52
53 public class UnivariateprocessNoiseTest {
54
55
56 @Test
57 public void testUnivariateProcessNoiseGetters() {
58
59 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
60
61
62 final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
63
64 lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
65 lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
66 lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
67
68 lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
69 lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
70 lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
71
72
73 final UnivariateFunction[] propagationParametersEvolution =
74 new UnivariateFunction[] {new PolynomialFunction(new double[] {10., 1., 1e-4}),
75 new PolynomialFunction(new double[] {1000., 0., 0.})};
76
77
78 final UnivariateFunction[] measurementsParametersEvolution =
79 new UnivariateFunction[] {new PolynomialFunction(new double[] {100., 1., 1e-3})};
80
81
82 final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(9);
83
84
85
86 final LOFType lofType = LOFType.TNW;
87 final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
88 lofType,
89 PositionAngle.TRUE,
90 lofCartesianOrbitalParametersEvolution,
91 propagationParametersEvolution,
92 measurementsParametersEvolution);
93
94 Assert.assertEquals(LOFType.TNW, processNoise.getLofType());
95 Assert.assertEquals(PositionAngle.TRUE, processNoise.getPositionAngle());
96 Assert.assertEquals(initialCovarianceMatrix,
97 processNoise.getInitialCovarianceMatrix(new SpacecraftState(context.initialOrbit)));
98 Assert.assertArrayEquals(lofCartesianOrbitalParametersEvolution, processNoise.getLofCartesianOrbitalParametersEvolution());
99 Assert.assertArrayEquals(propagationParametersEvolution, processNoise.getPropagationParametersEvolution());
100 Assert.assertArrayEquals(measurementsParametersEvolution, processNoise.getMeasurementsParametersEvolution());
101
102 final UnivariateProcessNoise processNoiseOld = new UnivariateProcessNoise(initialCovarianceMatrix,
103 lofType,
104 PositionAngle.TRUE,
105 lofCartesianOrbitalParametersEvolution,
106 propagationParametersEvolution);
107
108 Assert.assertEquals(LOFType.TNW, processNoiseOld.getLofType());
109 Assert.assertEquals(PositionAngle.TRUE, processNoiseOld.getPositionAngle());
110 Assert.assertEquals(initialCovarianceMatrix,
111 processNoiseOld.getInitialCovarianceMatrix(new SpacecraftState(context.initialOrbit)));
112 Assert.assertArrayEquals(lofCartesianOrbitalParametersEvolution, processNoiseOld.getLofCartesianOrbitalParametersEvolution());
113 Assert.assertArrayEquals(propagationParametersEvolution, processNoiseOld.getPropagationParametersEvolution());
114 }
115
116
117
118
119
120
121 @Test
122 public void testProcessNoiseMatrixCartesian() {
123
124
125 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
126
127
128 final boolean print = false;
129
130
131 final OrbitType orbitType = OrbitType.CARTESIAN;
132 final PositionAngle positionAngle = PositionAngle.TRUE;
133 final boolean perfectStart = true;
134 final double minStep = 1.e-6;
135 final double maxStep = 60.;
136 final double dP = 1.;
137 final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
138 minStep, maxStep, dP,
139 Force.POTENTIAL, Force.THIRD_BODY_MOON,
140 Force.THIRD_BODY_SUN,
141 Force.SOLAR_RADIATION_PRESSURE);
142
143 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
144 propagatorBuilder);
145
146
147 final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
148
149 lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
150 lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
151 lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
152
153 lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
154 lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
155 lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
156
157
158 final UnivariateFunction[] propagationParametersEvolution =
159 new UnivariateFunction[] {new PolynomialFunction(new double[] {10., 1., 1e-4}),
160 new PolynomialFunction(new double[] {1000., 0., 0.})};
161
162
163
164 final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(8);
165
166
167
168 final LOFType lofType = LOFType.TNW;
169 final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
170 lofType,
171 positionAngle,
172 lofCartesianOrbitalParametersEvolution,
173 propagationParametersEvolution);
174
175 final SpacecraftState state0 = propagator.getInitialState();
176 final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(3600.));
177 final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate()
178 .shiftedBy(2*context.initialOrbit.getKeplerianPeriod()));
179
180
181 final int sampleNumber = 10000;
182
183
184 final double relativeTolerance = 1.5e-2;
185
186 if (print) {
187 System.out.println("Orbit Type : " + orbitType);
188 System.out.println("Position Angle: " + positionAngle + "\n");
189 }
190 checkCovarianceValue(print, state0, state0, processNoise, 2, sampleNumber, relativeTolerance);
191 checkCovarianceValue(print, state0, state1, processNoise, 2, sampleNumber, relativeTolerance);
192 checkCovarianceValue(print, state0, state2, processNoise, 2, sampleNumber, relativeTolerance);
193 }
194
195
196
197
198
199
200 @Test
201 public void testProcessNoiseMatrixCartesianNew() {
202
203
204 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
205
206
207 final boolean print = false;
208
209
210 final OrbitType orbitType = OrbitType.CARTESIAN;
211 final PositionAngle positionAngle = PositionAngle.TRUE;
212 final boolean perfectStart = true;
213 final double minStep = 1.e-6;
214 final double maxStep = 60.;
215 final double dP = 1.;
216 final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
217 minStep, maxStep, dP,
218 Force.POTENTIAL, Force.THIRD_BODY_MOON,
219 Force.THIRD_BODY_SUN,
220 Force.SOLAR_RADIATION_PRESSURE);
221
222 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
223 propagatorBuilder);
224
225
226 final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
227
228 lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {500., 0., 1e-4});
229 lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {400., 1e-1, 0.});
230 lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {200., 2e-1, 0.});
231
232 lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1e-6});
233 lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
234 lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 1e-5});
235
236
237 final UnivariateFunction[] propagationParametersEvolution =
238 new UnivariateFunction[] {new PolynomialFunction(new double[] {100., 1., 1e-4}),
239 new PolynomialFunction(new double[] {2000., 3., 0.})};
240
241
242 final UnivariateFunction[] measurementsParametersEvolution =
243 new UnivariateFunction[] {new PolynomialFunction(new double[] {100., 1., 1e-3})};
244
245
246 final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(9);
247
248
249
250 final LOFType lofType = LOFType.TNW;
251 final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
252 lofType,
253 positionAngle,
254 lofCartesianOrbitalParametersEvolution,
255 propagationParametersEvolution,
256 measurementsParametersEvolution);
257
258 final SpacecraftState state0 = propagator.getInitialState();
259 final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(3600.));
260 final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate()
261 .shiftedBy(2*context.initialOrbit.getKeplerianPeriod()));
262
263
264 final int sampleNumber = 10000;
265
266
267 final double relativeTolerance = 1.5e-2;
268
269 if (print) {
270 System.out.println("Orbit Type : " + orbitType);
271 System.out.println("Position Angle: " + positionAngle + "\n");
272 }
273 checkCovarianceValue(print, state0, state0, processNoise, 2, sampleNumber, relativeTolerance);
274 checkCovarianceValue(print, state0, state1, processNoise, 2, sampleNumber, relativeTolerance);
275 checkCovarianceValue(print, state0, state2, processNoise, 2, sampleNumber, relativeTolerance);
276 }
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380 private void checkCovarianceValue(final boolean print,
381 final SpacecraftState previous,
382 final SpacecraftState current,
383 final UnivariateProcessNoise univariateProcessNoise,
384 final int propagationParametersSize,
385 final int sampleNumber,
386 final double relativeTolerance) {
387
388
389 final RealMatrix processNoiseMatrix = univariateProcessNoise.getProcessNoiseMatrix(previous, current);
390
391
392 final CorrelatedRandomVectorGenerator randomVectorGenerator = createSampler(processNoiseMatrix);
393
394
395 int measurementsParametersSize = processNoiseMatrix.getColumnDimension() - (6 + propagationParametersSize);
396 if ( measurementsParametersSize < 0) {
397 measurementsParametersSize = 0;
398 }
399
400
401 final StreamingStatistics[] orbitalStatistics = new StreamingStatistics[6];
402 for (int i = 0; i < 6; i++) {
403 orbitalStatistics[i] = new StreamingStatistics();
404 }
405 StreamingStatistics[] propagationStatistics;
406 if (propagationParametersSize > 0) {
407 propagationStatistics = new StreamingStatistics[propagationParametersSize];
408 for (int i = 0; i < propagationParametersSize; i++) {
409 propagationStatistics[i] = new StreamingStatistics();
410 }
411 } else {
412 propagationStatistics = null;
413 }
414 StreamingStatistics[] measurementsStatistics;
415 if (propagationParametersSize > 0) {
416 measurementsStatistics = new StreamingStatistics[measurementsParametersSize];
417 for (int i = 0; i < measurementsParametersSize; i++) {
418 measurementsStatistics[i] = new StreamingStatistics();
419 }
420 } else {
421 measurementsStatistics = null;
422 }
423
424
425
426 final double[] currentOrbitArray = new double[6];
427 current.getOrbit().getType().mapOrbitToArray(current.getOrbit(),
428 univariateProcessNoise.getPositionAngle(),
429 currentOrbitArray,
430 null);
431
432 final Transform inertialToLof = univariateProcessNoise.getLofType().transformFromInertial(current.getDate(),
433 current.getOrbit().getPVCoordinates());
434
435 for (int i = 0; i < sampleNumber; i++) {
436
437
438 final double[] randomVector = randomVectorGenerator.nextVector();
439
440
441
442
443
444 final double[] modifiedOrbitArray = new double[6];
445 for (int j = 0; j < 6; j++) {
446 modifiedOrbitArray[j] = currentOrbitArray[j] + randomVector[j];
447 }
448
449
450 TimeStampedPVCoordinates inertialPV = null;
451 try {
452 inertialPV = current.getOrbit().getType().mapArrayToOrbit(modifiedOrbitArray,
453 null,
454 univariateProcessNoise.getPositionAngle(),
455 current.getDate(),
456 current.getMu(),
457 current.getFrame()).getPVCoordinates();
458 } catch (OrekitIllegalArgumentException e) {
459
460 System.out.println("i = " + i + " / Inconsistent Orbit");
461 System.out.println("\tCurrent Orbit = " + Arrays.toString(currentOrbitArray));
462 System.out.println("\tModified Orbit = " + Arrays.toString(modifiedOrbitArray));
463 System.out.println("\tDelta Orbit = " + Arrays.toString(randomVector));
464 e.printStackTrace();
465 System.err.println(e.getMessage());
466 }
467
468
469
470 final PVCoordinates lofPV = inertialToLof.transformPVCoordinates(inertialPV);
471
472
473 orbitalStatistics[0].addValue(lofPV.getPosition().getX());
474 orbitalStatistics[1].addValue(lofPV.getPosition().getY());
475 orbitalStatistics[2].addValue(lofPV.getPosition().getZ());
476 orbitalStatistics[3].addValue(lofPV.getVelocity().getX());
477 orbitalStatistics[4].addValue(lofPV.getVelocity().getY());
478 orbitalStatistics[5].addValue(lofPV.getVelocity().getZ());
479
480
481
482
483
484
485 if (propagationParametersSize > 0) {
486 for (int j = 6; j < randomVector.length - measurementsParametersSize; j++) {
487 propagationStatistics[j - 6].addValue(randomVector[j]);
488 }
489 }
490
491
492
493
494
495 if (measurementsParametersSize > 0) {
496 for (int j = 6 + propagationParametersSize ; j < randomVector.length; j++) {
497 measurementsStatistics[j - (6 + propagationParametersSize)].addValue(randomVector[j]);
498 }
499 }
500 }
501
502
503
504
505
506 final double dt = current.getDate().durationFrom(previous.getDate());
507
508
509 final double[] orbitalValues = new double[6];
510 final double[] orbitalStatisticsValues = new double[6];
511 final double[] orbitalRelativeValues = new double[6];
512
513 for (int i = 0; i < 6; i++) {
514 orbitalValues[i] = FastMath.abs(univariateProcessNoise.getLofCartesianOrbitalParametersEvolution()[i].value(dt));
515 orbitalStatisticsValues[i] = orbitalStatistics[i].getStandardDeviation();
516
517 if (FastMath.abs(orbitalValues[i]) > 1e-15) {
518 orbitalRelativeValues[i] = FastMath.abs(1. - orbitalStatisticsValues[i]/orbitalValues[i]);
519 } else {
520 orbitalRelativeValues[i] = orbitalStatisticsValues[i];
521 }
522 }
523
524
525 final double[] propagationValues = new double[propagationParametersSize];
526 final double[] propagationStatisticsValues = new double[propagationParametersSize];
527 final double[] propagationRelativeValues = new double[propagationParametersSize];
528 for (int i = 0; i < propagationParametersSize; i++) {
529 propagationValues[i] = FastMath.abs(univariateProcessNoise.getPropagationParametersEvolution()[i].value(dt));
530 propagationStatisticsValues[i] = propagationStatistics[i].getStandardDeviation();
531 if (FastMath.abs(propagationValues[i]) > 1e-15) {
532 propagationRelativeValues[i] = FastMath.abs(1. - propagationStatisticsValues[i]/propagationValues[i]);
533 } else {
534 propagationRelativeValues[i] = propagationStatisticsValues[i];
535 }
536 }
537
538
539 final double[] measurementsValues = new double[measurementsParametersSize];
540 final double[] measurementsStatisticsValues = new double[measurementsParametersSize];
541 final double[] measurementsRelativeValues = new double[measurementsParametersSize];
542 for (int i = 0; i < measurementsParametersSize; i++) {
543 measurementsValues[i] = FastMath.abs(univariateProcessNoise.getMeasurementsParametersEvolution()[i].value(dt));
544 measurementsStatisticsValues[i] = measurementsStatistics[i].getStandardDeviation();
545 if (FastMath.abs(propagationValues[i]) > 1e-15) {
546 measurementsRelativeValues[i] = FastMath.abs(1. - measurementsStatisticsValues[i]/measurementsValues[i]);
547 } else {
548 measurementsRelativeValues[i] = measurementsStatisticsValues[i];
549 }
550 }
551
552
553 if (print) {
554 System.out.println("\tdt = " + dt + " / N = " + sampleNumber + "\n");
555 System.out.println("\tσ orbit ref = " + Arrays.toString(orbitalValues));
556 System.out.println("\tσ orbit stat = " + Arrays.toString(orbitalStatisticsValues));
557 System.out.println("\tσ orbit % = " + Arrays.toString(Arrays.stream(orbitalRelativeValues).map(i -> i*100.).toArray()) + "\n");
558
559 System.out.println("\tσ propag ref = " + Arrays.toString(propagationValues));
560 System.out.println("\tσ propag stat = " + Arrays.toString(propagationStatisticsValues));
561 System.out.println("\tσ propag % = " + Arrays.toString(Arrays.stream(propagationRelativeValues).map(i -> i*100.).toArray()) + "\n");
562
563 System.out.println("\tσ meas ref = " + Arrays.toString(propagationValues));
564 System.out.println("\tσ meas stat = " + Arrays.toString(propagationStatisticsValues));
565 System.out.println("\tσ meas % = " + Arrays.toString(Arrays.stream(propagationRelativeValues).map(i -> i*100.).toArray()) + "\n");
566 }
567
568
569 assertArrayEquals(new double[6],
570 orbitalRelativeValues,
571 relativeTolerance);
572 assertArrayEquals(new double[propagationParametersSize],
573 propagationRelativeValues,
574 relativeTolerance);
575 assertArrayEquals(new double[measurementsParametersSize],
576 measurementsRelativeValues,
577 relativeTolerance);
578 }
579
580
581
582
583
584 private CorrelatedRandomVectorGenerator createSampler(final RealMatrix covarianceMatrix) {
585 double small = 10e-20 * covarianceMatrix.getNorm1();
586 return new CorrelatedRandomVectorGenerator(
587 covarianceMatrix,
588 small,
589 new GaussianRandomGenerator(new Well1024a(0x366a26b94e520f41l)));
590 }
591
592
593
594
595
596
597
598 @Test
599 public void testLofCartesianOrbitalCovarianceFormal() {
600
601
602 Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
603
604
605 final boolean print = false;
606
607
608 final OrbitType orbitType = OrbitType.KEPLERIAN;
609 final PositionAngle positionAngle = PositionAngle.MEAN;
610 final boolean perfectStart = true;
611 final double minStep = 1.e-6;
612 final double maxStep = 60.;
613 final double dP = 1.;
614 final NumericalPropagatorBuilder propagatorBuilder = context.createBuilder(orbitType, positionAngle, perfectStart,
615 minStep, maxStep, dP,
616 Force.POTENTIAL, Force.THIRD_BODY_MOON,
617 Force.THIRD_BODY_SUN,
618 Force.SOLAR_RADIATION_PRESSURE);
619
620
621 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
622 propagatorBuilder);
623
624
625 final UnivariateFunction[] lofCartesianOrbitalParametersEvolution = new UnivariateFunction[6];
626
627 lofCartesianOrbitalParametersEvolution[0] = new PolynomialFunction(new double[] {100., 0., 1e-4});
628 lofCartesianOrbitalParametersEvolution[1] = new PolynomialFunction(new double[] {100., 1e-1, 0.});
629 lofCartesianOrbitalParametersEvolution[2] = new PolynomialFunction(new double[] {100., 0., 0.});
630
631 lofCartesianOrbitalParametersEvolution[3] = new PolynomialFunction(new double[] {1., 0., 1.e-6});
632 lofCartesianOrbitalParametersEvolution[4] = new PolynomialFunction(new double[] {1., 1e-3, 0.});
633 lofCartesianOrbitalParametersEvolution[5] = new PolynomialFunction(new double[] {1., 0., 0.});
634
635
636 final UnivariateFunction[] propagationParametersEvolution =
637 new UnivariateFunction[] {new PolynomialFunction(new double[] {10., 1., 1e-4}),
638 new PolynomialFunction(new double[] {1000., 0., 0.})};
639
640
641
642 final RealMatrix initialCovarianceMatrix = MatrixUtils.createRealIdentityMatrix(8);
643
644
645
646 final LOFType lofType = LOFType.TNW;
647 final UnivariateProcessNoise processNoise = new UnivariateProcessNoise(initialCovarianceMatrix,
648 lofType,
649 positionAngle,
650 lofCartesianOrbitalParametersEvolution,
651 propagationParametersEvolution);
652
653 final SpacecraftState state0 = propagator.getInitialState();
654
655
656 final SpacecraftState state1 = propagator.propagate(context.initialOrbit.getDate()
657 .shiftedBy(context.initialOrbit.getKeplerianPeriod()));
658
659 final SpacecraftState state2 = propagator.propagate(context.initialOrbit.getDate().shiftedBy(86400.));
660
661 if (print) {
662 System.out.println("Orbit Type : " + orbitType);
663 System.out.println("Position Angle: " + positionAngle);
664 }
665
666
667 checkLofOrbitalCovarianceMatrix(print, state0, state0, processNoise, 1.62e-11);
668
669 checkLofOrbitalCovarianceMatrix(print, state0, state1, processNoise, 2.78e-10);
670
671 checkLofOrbitalCovarianceMatrix(print, state0, state2, processNoise, 9.15e-9);
672 }
673
674
675
676
677 private void checkLofOrbitalCovarianceMatrix(final boolean print,
678 final SpacecraftState previous,
679 final SpacecraftState current,
680 final UnivariateProcessNoise univariateProcessNoise,
681 final double relativeTolerance) {
682
683
684 final RealMatrix inertialQ = univariateProcessNoise.getProcessNoiseMatrix(previous, current);
685
686
687 final RealMatrix inertialOrbitalQ = inertialQ.getSubMatrix(0, 5, 0, 5);
688
689
690 final double[][] dCdY = new double[6][6];
691 current.getOrbit().getJacobianWrtParameters(univariateProcessNoise.getPositionAngle(), dCdY);
692 final RealMatrix jacOrbToCar = new Array2DRowRealMatrix(dCdY, false);
693
694
695 final double[][] dLOFdI = new double[6][6];
696 univariateProcessNoise.getLofType().transformFromInertial(current.getDate(),
697 current.getOrbit().getPVCoordinates()).getJacobian(CartesianDerivativesFilter.USE_PV, dLOFdI);
698 final RealMatrix jacIToLOF = new Array2DRowRealMatrix(dLOFdI, false);
699
700
701 final RealMatrix jac = jacIToLOF.multiply(jacOrbToCar);
702
703
704 final RealMatrix lofQ = jac.multiply(inertialOrbitalQ).multiplyTransposed(jac);
705
706
707 final RealVector refLofSig = new ArrayRealVector(6);
708 double dt = current.getDate().durationFrom(previous.getDate());
709 for (int i = 0; i < 6; i++) {
710 refLofSig.setEntry(i, univariateProcessNoise.getLofCartesianOrbitalParametersEvolution()[i].value(dt));
711 }
712
713
714
715 RealVector dLofDiag = new ArrayRealVector(6);
716 for (int i = 0; i < 6; i++) {
717 for (int j = 0; j < 6; j++) {
718 double refI = refLofSig.getEntry(i);
719 if (i == j) {
720
721 dLofDiag.setEntry(i, FastMath.abs(1. - lofQ.getEntry(i, i) / refI / refI));
722 lofQ.setEntry(i, i, 0.);
723 } else {
724
725 lofQ.setEntry(i, j, FastMath.abs(lofQ.getEntry(i,j) / refI / refLofSig.getEntry(j)));
726 }
727 }
728
729 }
730
731
732 double dDiag = dLofDiag.getNorm();
733 double dNonDiag = lofQ.getNorm1();
734
735
736 if (print) {
737 System.out.println("\tdt = " + dt);
738 System.out.format(Locale.US, "\tΔDiagonal norm in Cartesian LOF = %10.4e%n", dDiag);
739 System.out.format(Locale.US, "\tΔNon-Diagonal norm in Cartesian LOF = %10.4e%n%n", dNonDiag);
740 }
741 Assert.assertEquals(0., dDiag , relativeTolerance);
742 Assert.assertEquals(0., dNonDiag, relativeTolerance);
743 }
744 }