1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.measurements.generation;
18
19 import org.hipparchus.linear.MatrixUtils;
20 import org.hipparchus.linear.RealMatrix;
21 import org.hipparchus.random.CorrelatedRandomVectorGenerator;
22 import org.hipparchus.random.GaussianRandomGenerator;
23 import org.hipparchus.random.RandomGenerator;
24 import org.hipparchus.random.Well19937a;
25 import org.hipparchus.util.FastMath;
26 import org.junit.jupiter.api.Assertions;
27 import org.junit.jupiter.api.BeforeEach;
28 import org.junit.jupiter.api.Test;
29 import org.orekit.estimation.Context;
30 import org.orekit.estimation.EstimationTestUtils;
31 import org.orekit.estimation.Force;
32 import org.orekit.estimation.measurements.EstimatedMeasurementBase;
33 import org.orekit.estimation.measurements.ObservableSatellite;
34 import org.orekit.estimation.measurements.PV;
35 import org.orekit.estimation.measurements.modifiers.Bias;
36 import org.orekit.orbits.OrbitType;
37 import org.orekit.orbits.PositionAngleType;
38 import org.orekit.propagation.Propagator;
39 import org.orekit.propagation.SpacecraftState;
40 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
41 import org.orekit.time.AbsoluteDate;
42 import org.orekit.time.BurstSelector;
43 import org.orekit.time.TimeScalesFactory;
44
45 import java.util.SortedSet;
46
47 public class PVBuilderTest {
48
49 private static final double SIGMA_P = 10.0;
50 private static final double SIGMA_V = 0.01;
51 private static final double BIAS_P = 5.0;
52 private static final double BIAS_V = -0.003;
53
54 private MeasurementBuilder<PV> getBuilder(final RandomGenerator random, final ObservableSatellite satellite) {
55 final RealMatrix covariance = MatrixUtils.createRealDiagonalMatrix(new double[] {
56 SIGMA_P * SIGMA_P, SIGMA_P * SIGMA_P, SIGMA_P * SIGMA_P,
57 SIGMA_V * SIGMA_V, SIGMA_V * SIGMA_V, SIGMA_V * SIGMA_V,
58 });
59 MeasurementBuilder<PV> pvb =
60 new PVBuilder(random == null ? null : new CorrelatedRandomVectorGenerator(covariance,
61 1.0e-10,
62 new GaussianRandomGenerator(random)),
63 SIGMA_P, SIGMA_V, 1.0, satellite);
64 pvb.addModifier(new Bias<>(new String[] { "pxBias", "pyBias", "pzBias", "vxBias", "vyBias", "vzBias" },
65 new double[] { BIAS_P, BIAS_P, BIAS_P, BIAS_V, BIAS_V, BIAS_V },
66 new double[] { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 },
67 new double[] { Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY,
68 Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY, Double.NEGATIVE_INFINITY },
69 new double[] { Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY,
70 Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY }));
71 return pvb;
72 }
73
74 @Test
75 public void testForward() {
76 doTest(0x292b6e87436fe4c7L, 0.0, 1.2, 3.7 * SIGMA_P, 3.3 * SIGMA_V);
77 }
78
79 @Test
80 public void testBackward() {
81 doTest(0x2f3285aa70b83c47L, 0.0, -1.0, 3.1 * SIGMA_P, 3.3 * SIGMA_V);
82 }
83
84 private Propagator buildPropagator() {
85 return EstimationTestUtils.createPropagator(context.initialOrbit, propagatorBuilder);
86 }
87
88 private void doTest(long seed, double startPeriod, double endPeriod, double toleranceP, double toleranceV) {
89 Generator generator = new Generator();
90 final int maxBurstSize = 10;
91 final double highRateStep = 5.0;
92 final double burstPeriod = 300.0;
93
94 generator.addPropagator(buildPropagator());
95 generator.addPropagator(buildPropagator());
96 ObservableSatellite satellite = generator.addPropagator(buildPropagator());
97 generator.addPropagator(buildPropagator());
98 generator.addScheduler(new ContinuousScheduler<>(getBuilder(new Well19937a(seed), satellite),
99 new BurstSelector(maxBurstSize, highRateStep, burstPeriod,
100 TimeScalesFactory.getUTC())));
101 final GatheringSubscriber gatherer = new GatheringSubscriber();
102 generator.addSubscriber(gatherer);
103 final double period = context.initialOrbit.getKeplerianPeriod();
104 AbsoluteDate t0 = context.initialOrbit.getDate().shiftedBy(startPeriod * period);
105 AbsoluteDate t1 = context.initialOrbit.getDate().shiftedBy(endPeriod * period);
106 generator.generate(t0, t1);
107 SortedSet<EstimatedMeasurementBase<?>> measurements = gatherer.getGeneratedMeasurements();
108 Propagator propagator = buildPropagator();
109 double maxErrorP = 0;
110 double maxErrorV = 0;
111 AbsoluteDate previous = null;
112 AbsoluteDate tInf = t0.isBefore(t1) ? t0 : t1;
113 AbsoluteDate tSup = t0.isBefore(t1) ? t1 : t0;
114 int count = 0;
115 for (EstimatedMeasurementBase<?> measurement : measurements) {
116 AbsoluteDate date = measurement.getDate();
117 double[] m = measurement.getObservedValue();
118 Assertions.assertTrue(date.compareTo(tInf) >= 0);
119 Assertions.assertTrue(date.compareTo(tSup) <= 0);
120 if (previous != null) {
121
122
123
124 final double expected = (count % maxBurstSize == 0) ?
125 burstPeriod - (maxBurstSize - 1) * highRateStep :
126 highRateStep;
127 if (t0.isBefore(t1)) {
128
129 Assertions.assertEquals(expected, date.durationFrom(previous), 1.0e-10 * expected);
130 } else {
131
132 Assertions.assertEquals(expected, previous.durationFrom(date), 1.0e-10 * expected);
133 }
134 }
135 previous = date;
136 ++count;
137 SpacecraftState state = propagator.propagate(date);
138 double[] e = measurement.
139 getObservedMeasurement().
140 estimateWithoutDerivatives(new SpacecraftState[] { state }).
141 getEstimatedValue();
142 for (int i = 0; i < 3; ++i) {
143 maxErrorP = FastMath.max(maxErrorP, FastMath.abs(e[i] - m[i]));
144 }
145 for (int i = 3; i < m.length; ++i) {
146 maxErrorV = FastMath.max(maxErrorV, FastMath.abs(e[i] - m[i]));
147 }
148 }
149 Assertions.assertEquals(0.0, maxErrorP, toleranceP);
150 Assertions.assertEquals(0.0, maxErrorV, toleranceV);
151 }
152
153 @BeforeEach
154 public void setUp() {
155 context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
156
157 propagatorBuilder = context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
158 1.0e-6, 300.0, 0.001, Force.POTENTIAL,
159 Force.THIRD_BODY_SUN, Force.THIRD_BODY_MOON);
160 }
161
162 Context context;
163 NumericalPropagatorBuilder propagatorBuilder;
164
165 }