1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.estimation.leastsquares;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.geometry.euclidean.threed.Vector3D;
23 import org.hipparchus.optim.nonlinear.vector.leastsquares.LevenbergMarquardtOptimizer;
24 import org.hipparchus.util.FastMath;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.BeforeEach;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.Utils;
29 import org.orekit.errors.OrekitException;
30 import org.orekit.estimation.EphemerisContext;
31 import org.orekit.estimation.KeplerianEstimationTestUtils;
32 import org.orekit.estimation.measurements.AngularAzEl;
33 import org.orekit.estimation.measurements.AngularAzElMeasurementCreator;
34 import org.orekit.estimation.measurements.ObservedMeasurement;
35 import org.orekit.estimation.measurements.Range;
36 import org.orekit.estimation.measurements.TwoWayRangeMeasurementCreator;
37 import org.orekit.estimation.measurements.RangeRateMeasurementCreator;
38 import org.orekit.estimation.measurements.modifiers.Bias;
39 import org.orekit.frames.Frame;
40 import org.orekit.frames.FramesFactory;
41 import org.orekit.orbits.KeplerianOrbit;
42 import org.orekit.orbits.Orbit;
43 import org.orekit.orbits.PositionAngleType;
44 import org.orekit.propagation.Propagator;
45 import org.orekit.propagation.SpacecraftState;
46 import org.orekit.propagation.SpacecraftStateInterpolator;
47 import org.orekit.propagation.analytical.Ephemeris;
48 import org.orekit.propagation.analytical.KeplerianPropagator;
49 import org.orekit.propagation.conversion.EphemerisPropagatorBuilder;
50 import org.orekit.time.AbsoluteDate;
51 import org.orekit.time.DateComponents;
52 import org.orekit.time.TimeComponents;
53 import org.orekit.time.TimeInterpolator;
54 import org.orekit.time.TimeScalesFactory;
55
56 public class EphemerisBatchLSEstimatorTest {
57
58 private AbsoluteDate initDate;
59 private AbsoluteDate finalDate;
60 private Frame inertialFrame;
61 private Propagator propagator;
62 private EphemerisContext context;
63
64 @BeforeEach
65 public void setUp() throws IllegalArgumentException, OrekitException {
66 Utils.setDataRoot("regular-data");
67
68 initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
69 TimeComponents.H00,
70 TimeScalesFactory.getUTC());
71
72 finalDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
73 TimeComponents.H00,
74 TimeScalesFactory.getUTC());
75
76 double a = 7187990.1979844316;
77 double e = 0.5e-4;
78 double i = 1.7105407051081795;
79 double omega = 1.9674147913622104;
80 double OMEGA = FastMath.toRadians(261);
81 double lv = 0;
82 double mu = 3.9860047e14;
83 inertialFrame = FramesFactory.getEME2000();
84
85 Orbit initialState = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngleType.TRUE,
86 inertialFrame, initDate, mu);
87 propagator = new KeplerianPropagator(initialState);
88
89 context = new EphemerisContext();
90
91 }
92
93 @Test
94 public void testRangeWithBias() {
95
96 double dt = finalDate.durationFrom(initDate);
97 double timeStep = dt / 20.0;
98 List<SpacecraftState> states = new ArrayList<SpacecraftState>();
99
100 for(double t = 0 ; t <= dt; t+=timeStep) {
101 states.add(propagator.propagate(initDate.shiftedBy(t)));
102 }
103
104
105 final TimeInterpolator<SpacecraftState> interpolator =
106 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
107
108 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
109
110 final double refBias = 1234.56;
111 final List<ObservedMeasurement<?>> measurements =
112 KeplerianEstimationTestUtils.createMeasurements(ephemeris,
113 new TwoWayRangeMeasurementCreator(context,
114 Vector3D.ZERO, null,
115 Vector3D.ZERO, null,
116 refBias),
117 1.0, 5.0, 10.0);
118
119
120 final Bias<Range> rangeBias = new Bias<Range>(new String[] {"rangeBias"}, new double[] {0.0},
121 new double[] {1.0},
122 new double[] {0.0}, new double[] {10000.0});
123 rangeBias.getParametersDrivers().get(0).setSelected(true);
124
125
126 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
127 new EphemerisPropagatorBuilder(states, interpolator));
128 for (final ObservedMeasurement<?> range : measurements) {
129 ((Range) range).addModifier(rangeBias);
130 estimator.addMeasurement(range);
131 }
132 estimator.setParametersConvergenceThreshold(1.0e-2);
133 estimator.setMaxIterations(30);
134 estimator.setMaxEvaluations(30);
135
136
137 estimator.estimate();
138
139
140 Assertions.assertEquals(refBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 5.0e-6);
141 Assertions.assertEquals(1, estimator.getMeasurementsParametersDrivers(true).getNbParams());
142 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
143 Assertions.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
144
145 }
146
147 @Test
148 public void testRangeRateWithClockDrift() {
149
150 double dt = finalDate.durationFrom(initDate);
151 double timeStep = dt / 20.0;
152 List<SpacecraftState> states = new ArrayList<SpacecraftState>();
153
154 for(double t = 0 ; t <= dt; t+=timeStep) {
155 states.add(propagator.propagate(initDate.shiftedBy(t)));
156 }
157
158
159 final TimeInterpolator<SpacecraftState> interpolator =
160 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
161
162 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
163
164 final double refClockBias = 653.47e-11;
165 final RangeRateMeasurementCreator creator = new RangeRateMeasurementCreator(context, false, refClockBias);
166 creator.getSatellite().getClockDriftDriver().setSelected(true);
167 final List<ObservedMeasurement<?>> measurements =
168 KeplerianEstimationTestUtils.createMeasurements(ephemeris, creator,
169 1.0, 5.0, 10.0);
170
171
172
173 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
174 new EphemerisPropagatorBuilder(states, interpolator));
175 for (final ObservedMeasurement<?> rangeRate : measurements) {
176 estimator.addMeasurement(rangeRate);
177 }
178 estimator.setParametersConvergenceThreshold(1.0e-2);
179 estimator.setMaxIterations(30);
180 estimator.setMaxEvaluations(30);
181
182
183
184 estimator.estimate();
185
186
187 Assertions.assertEquals(refClockBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 6.0e-16);
188 Assertions.assertEquals(1, estimator.getMeasurementsParametersDrivers(true).getNbParams());
189 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
190 Assertions.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
191
192 }
193
194 @Test
195 public void testAzElWithBias() {
196
197 double dt = finalDate.durationFrom(initDate);
198 double timeStep = dt / 20.0;
199 List<SpacecraftState> states = new ArrayList<SpacecraftState>();
200
201 for(double t = 0 ; t <= dt; t+=timeStep) {
202 states.add(propagator.propagate(initDate.shiftedBy(t)));
203 }
204
205
206 final TimeInterpolator<SpacecraftState> interpolator =
207 new SpacecraftStateInterpolator(3, inertialFrame, inertialFrame);
208
209 final Ephemeris ephemeris = new Ephemeris(states, interpolator);
210
211 final double refAzBias = FastMath.toRadians(0.3);
212 final double refElBias = FastMath.toRadians(0.1);
213 final List<ObservedMeasurement<?>> measurements =
214 KeplerianEstimationTestUtils.createMeasurements(ephemeris,
215 new AngularAzElMeasurementCreator(context, refAzBias, refElBias),
216 1.0, 5.0, 10.0);
217
218
219 final Bias<AngularAzEl> azElBias = new Bias<>(new String[] {"azBias", "elBias"}, new double[] {0.0, 0.0},
220 new double[] {1.0, 1.0},
221 new double[] {0.0, 0.0}, new double[] {2.0, 2.0});
222 azElBias.getParametersDrivers().get(0).setSelected(true);
223 azElBias.getParametersDrivers().get(1).setSelected(true);
224
225
226 final BatchLSEstimator estimator = new BatchLSEstimator(new LevenbergMarquardtOptimizer(),
227 new EphemerisPropagatorBuilder(states, interpolator));
228 for (final ObservedMeasurement<?> azEl : measurements) {
229 ((AngularAzEl) azEl).addModifier(azElBias);
230 estimator.addMeasurement(azEl);
231 }
232 estimator.setParametersConvergenceThreshold(1.0e-2);
233 estimator.setMaxIterations(30);
234 estimator.setMaxEvaluations(30);
235
236
237 estimator.estimate();
238
239
240 Assertions.assertEquals(refAzBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(0).getValue(), 1.0e-8);
241 Assertions.assertEquals(refElBias, estimator.getMeasurementsParametersDrivers(true).getDrivers().get(1).getValue(), 1.0e-7);
242 Assertions.assertEquals(2, estimator.getMeasurementsParametersDrivers(true).getNbParams());
243 Assertions.assertEquals(0, estimator.getOrbitalParametersDrivers(true).getNbParams());
244 Assertions.assertEquals(0, estimator.getPropagatorParametersDrivers(true).getNbParams());
245
246 }
247
248 }