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