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.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.linear.MatrixUtils;
24 import org.hipparchus.linear.RealMatrix;
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.Utils;
30 import org.orekit.errors.OrekitException;
31 import org.orekit.estimation.EphemerisContext;
32 import org.orekit.estimation.KeplerianEstimationTestUtils;
33 import org.orekit.estimation.measurements.AngularAzEl;
34 import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
35 import org.orekit.estimation.measurements.ObservedMeasurement;
36 import org.orekit.estimation.measurements.Range;
37 import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
38 import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
39 import org.orekit.estimation.measurements.modifiers.Bias;
40 import org.orekit.frames.Frame;
41 import org.orekit.frames.FramesFactory;
42 import org.orekit.orbits.KeplerianOrbit;
43 import org.orekit.orbits.Orbit;
44 import org.orekit.orbits.PositionAngleType;
45 import org.orekit.propagation.Propagator;
46 import org.orekit.propagation.SpacecraftState;
47 import org.orekit.propagation.SpacecraftStateInterpolator;
48 import org.orekit.propagation.analytical.Ephemeris;
49 import org.orekit.propagation.analytical.KeplerianPropagator;
50 import org.orekit.propagation.conversion.EphemerisPropagatorBuilder;
51 import org.orekit.time.AbsoluteDate;
52 import org.orekit.time.DateComponents;
53 import org.orekit.time.TimeComponents;
54 import org.orekit.time.TimeInterpolator;
55 import org.orekit.time.TimeScalesFactory;
56 import org.orekit.utils.ParameterDriversList;
57
58 public class EphemerisKalmanEstimatorTest {
59
60 private AbsoluteDate initDate;
61 private AbsoluteDate finalDate;
62 private Frame inertialFrame;
63 private Propagator propagator;
64 private EphemerisContext context;
65
66 @BeforeEach
67 public void setUp() throws IllegalArgumentException, OrekitException {
68 Utils.setDataRoot("regular-data");
69
70 initDate = new AbsoluteDate(new DateComponents(2004, 1, 1),
71 TimeComponents.H00,
72 TimeScalesFactory.getUTC());
73
74 finalDate = new AbsoluteDate(new DateComponents(2004, 1, 2),
75 TimeComponents.H00,
76 TimeScalesFactory.getUTC());
77
78 double a = 7187990.1979844316;
79 double e = 0.5e-4;
80 double i = 1.7105407051081795;
81 double omega = 1.9674147913622104;
82 double OMEGA = FastMath.toRadians(261);
83 double lv = 0;
84 double mu = 3.9860047e14;
85 inertialFrame = FramesFactory.getEME2000();
86
87 Orbit initialState = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngleType.TRUE,
88 inertialFrame, initDate, mu);
89 propagator = new KeplerianPropagator(initialState);
90
91 context = new EphemerisContext();
92
93 }
94
95 @Test
96 public void testRangeWithBias() {
97
98 double dt = finalDate.durationFrom(initDate);
99 double timeStep = dt / 20.0;
100 List<SpacecraftState> states = new ArrayList<>();
101
102 for(double t = 0 ; t <= dt; t+=timeStep) {
103 states.add(propagator.propagate(initDate.shiftedBy(t)));
104 }
105
106
107 final TimeInterpolator<SpacecraftState> interpolator =
108 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
109
110
111 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
112
113 final double refBias = 1234.56;
114 final List<ObservedMeasurement<?>> measurements =
115 KeplerianEstimationTestUtils.createMeasurements(ephemeris,
116 new TwoWayRangeMeasurementCreator(context,
117 Vector3D.ZERO, null,
118 Vector3D.ZERO, null,
119 refBias),
120 1.0, 5.0, 10.0);
121
122
123 final Bias<Range> rangeBias = new Bias<>(new String[] {"rangeBias"}, new double[] {0.0},
124 new double[] {1.0},
125 new double[] {0.0}, new double[] {10000.0});
126 rangeBias.getParametersDrivers().get(0).setSelected(true);
127
128
129 final ParameterDriversList drivers = new ParameterDriversList();
130 drivers.add(rangeBias.getParametersDrivers().get(0));
131
132
133 final EphemerisPropagatorBuilder builder = new EphemerisPropagatorBuilder(states, interpolator);
134
135
136 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
137 refBias * refBias
138 });
139
140
141 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
142 0.0
143 });
144
145
146 final KalmanEstimator estimator = new KalmanEstimatorBuilder().estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(initialP, Q))
147 .addPropagationConfiguration(builder, null)
148 .build();
149
150
151 measurements.forEach(range -> ((Range) range).addModifier(rangeBias));
152
153
154 estimator.processMeasurements(measurements);
155
156
157 Assertions.assertEquals(refBias, estimator.getPhysicalEstimatedState().getEntry(0), 3.0e-5);
158 Assertions.assertEquals(1, estimator.getEstimatedMeasurementsParameters().getNbParams());
159 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
160 Assertions.assertEquals(0, estimator.getPropagationParametersDrivers(true).getNbParams());
161
162 }
163
164 @Test
165 public void testRangeRateWithClockDrift() {
166
167 double dt = finalDate.durationFrom(initDate);
168 double timeStep = dt / 20.0;
169 List<SpacecraftState> states = new ArrayList<>();
170
171 for(double t = 0 ; t <= dt; t+=timeStep) {
172 states.add(propagator.propagate(initDate.shiftedBy(t)));
173 }
174
175
176 final TimeInterpolator<SpacecraftState> interpolator =
177 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
178
179
180 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
181
182 final double refClockBias = 653.47e-11;
183 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, refClockBias);
184 creator.getSatellite().getClockDriftDriver().setSelected(true);
185 final List<ObservedMeasurement<?>> measurements =
186 KeplerianEstimationTestUtils.createMeasurements(ephemeris, creator,
187 1.0, 5.0, 10.0);
188
189
190
191 final ParameterDriversList drivers = new ParameterDriversList();
192 drivers.add(creator.getSatellite().getClockDriftDriver());
193
194
195 final EphemerisPropagatorBuilder builder = new EphemerisPropagatorBuilder(states, interpolator);
196
197
198 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
199 refClockBias * refClockBias
200 });
201
202
203 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
204 0.0
205 });
206
207
208 final KalmanEstimator estimator = new KalmanEstimatorBuilder().estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(initialP, Q))
209 .addPropagationConfiguration(builder, null)
210 .build();
211
212
213 estimator.processMeasurements(measurements);
214
215
216 Assertions.assertEquals(refClockBias, estimator.getPhysicalEstimatedState().getEntry(0), 6.0e-16);
217 Assertions.assertEquals(1, estimator.getEstimatedMeasurementsParameters().getNbParams());
218 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
219 Assertions.assertEquals(0, estimator.getPropagationParametersDrivers(true).getNbParams());
220
221 }
222
223 @Test
224 public void testAzElWithBias() {
225
226 double dt = finalDate.durationFrom(initDate);
227 double timeStep = dt / 20.0;
228 List<SpacecraftState> states = new ArrayList<>();
229
230 for(double t = 0 ; t <= dt; t+=timeStep) {
231 states.add(propagator.propagate(initDate.shiftedBy(t)));
232 }
233
234
235 final TimeInterpolator<SpacecraftState> interpolator =
236 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
237
238
239 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
240
241
242
243 final double refAzBias = 1.2;
244 final double refElBias = 0.9;
245 final List<ObservedMeasurement<?>> measurements =
246 KeplerianEstimationTestUtils.createMeasurements(ephemeris,
247 new AngularAzElMeasurementCreator(context, refAzBias, refElBias),
248 1.0, 5.0, 10.0);
249
250
251 final Bias<AngularAzEl> azElBias = new Bias<>(new String[] {"azBias", "elBias"}, new double[] {0.0, 0.0},
252 new double[] {1.0, 1.0},
253 new double[] {0.0, 0.0}, new double[] {2.0, 2.0});
254 azElBias.getParametersDrivers().get(0).setSelected(true);
255 azElBias.getParametersDrivers().get(1).setSelected(true);
256
257
258 final ParameterDriversList drivers = new ParameterDriversList();
259 azElBias.getParametersDrivers().forEach(drivers::add);
260
261
262 final EphemerisPropagatorBuilder builder = new EphemerisPropagatorBuilder(states, interpolator);
263
264
265 final RealMatrix initialP = MatrixUtils.createRealDiagonalMatrix(new double [] {
266 refAzBias * refAzBias, refElBias * refElBias
267 });
268
269
270 RealMatrix Q = MatrixUtils.createRealDiagonalMatrix(new double [] {
271 0.0, 0.0
272 });
273
274
275 final KalmanEstimator estimator = new KalmanEstimatorBuilder().estimatedMeasurementsParameters(drivers, new ConstantProcessNoise(initialP, Q))
276 .addPropagationConfiguration(builder, null)
277 .build();
278
279
280 measurements.forEach(azEl -> ((AngularAzEl) azEl).addModifier(azElBias));
281
282
283 estimator.processMeasurements(measurements);
284
285
286 Assertions.assertEquals(refAzBias, estimator.getPhysicalEstimatedState().getEntry(0), 0.017);
287 Assertions.assertEquals(refElBias, estimator.getPhysicalEstimatedState().getEntry(1), 0.022);
288 Assertions.assertEquals(2, estimator.getEstimatedMeasurementsParameters().getNbParams());
289 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
290 Assertions.assertEquals(0, estimator.getPropagationParametersDrivers(true).getNbParams());
291
292 }
293
294 }