1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.orekit.estimation.iod;
19
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.geometry.euclidean.twod.Vector2D;
22 import org.hipparchus.util.FastMath;
23 import org.junit.jupiter.api.Assertions;
24 import org.junit.jupiter.api.BeforeEach;
25 import org.junit.jupiter.api.Test;
26 import org.orekit.Utils;
27 import org.orekit.control.heuristics.lambert.LambertSolver;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.errors.OrekitMessages;
30 import org.orekit.estimation.Context;
31 import org.orekit.estimation.EstimationTestUtils;
32 import org.orekit.estimation.measurements.ObservableSatellite;
33 import org.orekit.estimation.measurements.ObservedMeasurement;
34 import org.orekit.estimation.measurements.PV;
35 import org.orekit.estimation.measurements.PVMeasurementCreator;
36 import org.orekit.estimation.measurements.Position;
37 import org.orekit.frames.Frame;
38 import org.orekit.frames.FramesFactory;
39 import org.orekit.orbits.KeplerianOrbit;
40 import org.orekit.orbits.Orbit;
41 import org.orekit.orbits.OrbitType;
42 import org.orekit.orbits.PositionAngleType;
43 import org.orekit.propagation.Propagator;
44 import org.orekit.propagation.SpacecraftState;
45 import org.orekit.propagation.analytical.KeplerianPropagator;
46 import org.orekit.propagation.analytical.tle.TLE;
47 import org.orekit.propagation.analytical.tle.TLEPropagator;
48 import org.orekit.propagation.conversion.NumericalPropagatorBuilder;
49 import org.orekit.time.AbsoluteDate;
50 import org.orekit.time.TimeScale;
51 import org.orekit.time.TimeScalesFactory;
52 import org.orekit.utils.Constants;
53 import org.orekit.utils.PVCoordinates;
54
55 import java.util.List;
56
57
58
59
60
61
62
63
64
65 public class IodLambertTest {
66
67 @Deprecated
68 @Test
69 void testSolveLambertPb() {
70
71 final IodLambert iodLambert = new IodLambert(1.);
72 final double r1 = 1.;
73 final double r2 = 1.;
74 final double tof = 0.1;
75 final double theta = 2.;
76 final double[] v = new double[2];
77
78 iodLambert.solveLambertPb(r1, r2, theta, tof, 0, v);
79
80 final Vector2D solution = LambertSolver.solveNormalized2D(r1, r2, theta, tof, 0);
81 Assertions.assertEquals(solution.getX(), v[0]);
82 Assertions.assertEquals(solution.getY(), v[1]);
83 }
84
85 @Test
86 public void testLambert() {
87 final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
88
89 final double mu = context.initialOrbit.getMu();
90 final Frame frame = context.initialOrbit.getFrame();
91
92 final NumericalPropagatorBuilder propagatorBuilder =
93 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
94 1.0e-6, 60.0, 0.001);
95
96
97 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
98 propagatorBuilder);
99
100 final List<ObservedMeasurement<?>> measurements =
101 EstimationTestUtils.createMeasurements(propagator,
102 new PVMeasurementCreator(),
103 0.0, 1.0, 60.0);
104
105
106 final int idMeasure1 = 0;
107 final AbsoluteDate date1 = measurements.get(idMeasure1).getDate();
108
109
110
111
112 final Vector3D position1 = new Vector3D(
113 measurements.get(idMeasure1).getObservedValue()[0],
114 measurements.get(idMeasure1).getObservedValue()[1],
115 measurements.get(idMeasure1).getObservedValue()[2]);
116
117
118 final int idMeasure2 = 10;
119 final AbsoluteDate date2 = measurements.get(idMeasure2).getDate();
120
121
122
123
124 final Vector3D position2 = new Vector3D(
125 measurements.get(idMeasure2).getObservedValue()[0],
126 measurements.get(idMeasure2).getObservedValue()[1],
127 measurements.get(idMeasure2).getObservedValue()[2]);
128
129 final int nRev = 0;
130
131
132 final IodLambert iod = new IodLambert(mu);
133
134 final Orbit orbit = iod.estimate(frame,
135 true,
136 nRev,
137 (position1), date1,
138 (position2), date2);
139
140 Assertions.assertEquals(orbit.getA(), context.initialOrbit.getA(), 1.0e-9 * context.initialOrbit.getA());
141 Assertions.assertEquals(orbit.getE(), context.initialOrbit.getE(), 1.0e-9 * context.initialOrbit.getE());
142 Assertions.assertEquals(orbit.getI(), context.initialOrbit.getI(), 1.0e-9 * context.initialOrbit.getI());
143 }
144
145
146
147
148 @Test
149 public void testLambert2() {
150
151
152 final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
153 final double mu = context.initialOrbit.getMu();
154 final Frame frame = context.initialOrbit.getFrame();
155
156
157 final NumericalPropagatorBuilder propagatorBuilder =
158 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
159 1.0e-6, 60.0, 0.001);
160
161
162 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
163 propagatorBuilder);
164
165 final SpacecraftState initialState = propagator.getInitialState();
166 final KeplerianOrbit refOrbit = new KeplerianOrbit(initialState.getOrbit());
167 double[] refOrbitArray = new double[6];
168 OrbitType.KEPLERIAN.mapOrbitToArray(refOrbit, PositionAngleType.TRUE, refOrbitArray, null);
169 final Vector3D position1 = refOrbit.getPosition();
170 final AbsoluteDate date1 = refOrbit.getDate();
171
172
173 final double T = context.initialOrbit.getKeplerianPeriod();
174
175
176
177
178
179
180
181
182 final double[] dts = new double[] {
183 T/4, T/2, 3*T/4, 2*T + T/4, 3*T + 3*T/4};
184 final int[] nRevs = new int[] {0, 0, 0, 2, 3};
185 final boolean[] posigrades = new boolean[] {true, true, false, true, false};
186
187 for (int i = 0; i < dts.length; i++) {
188
189 propagator.resetInitialState(initialState);
190
191
192 final AbsoluteDate date2 = date1.shiftedBy(dts[i]);
193 final Vector3D position2 = propagator.propagate(date2).getPosition();
194
195
196 final IodLambert iod = new IodLambert(mu);
197
198
199 final KeplerianOrbit orbit = new KeplerianOrbit(iod.estimate(frame, posigrades[i], nRevs[i], position1, date1, position2, date2));
200
201
202 final double relTol = 1e-12;
203 Assertions.assertEquals(refOrbit.getA(), orbit.getA(), relTol * refOrbit.getA());
204 Assertions.assertEquals(refOrbit.getE(), orbit.getE(), relTol * refOrbit.getE());
205 Assertions.assertEquals(refOrbit.getI(), orbit.getI(), relTol * refOrbit.getI());
206 Assertions.assertEquals(refOrbit.getPerigeeArgument(), orbit.getPerigeeArgument(), relTol * refOrbit.getPerigeeArgument());
207 Assertions.assertEquals(refOrbit.getRightAscensionOfAscendingNode(), orbit.getRightAscensionOfAscendingNode(), relTol * refOrbit.getRightAscensionOfAscendingNode());
208 Assertions.assertEquals(refOrbit.getTrueAnomaly(), orbit.getTrueAnomaly(), relTol * refOrbit.getTrueAnomaly());
209 }
210 }
211
212
213 @Test
214 public void testMultiRevolutions() {
215
216 Utils.setDataRoot("regular-data");
217 TLE aussatB1 = new TLE("1 22087U 92054A 17084.21270512 -.00000243 +00000-0 +00000-0 0 9999",
218 "2 22087 008.5687 046.5717 0005960 022.3650 173.1619 00.99207999101265");
219 final Propagator propagator = TLEPropagator.selectExtrapolator(aussatB1);
220 final Frame teme = FramesFactory.getTEME();
221
222 final AbsoluteDate t1 = new AbsoluteDate("2017-03-25T23:48:31.282", TimeScalesFactory.getUTC());
223 final Vector3D p1 = propagator.propagate(t1).getPosition(teme);
224 final AbsoluteDate t2 = t1.shiftedBy(40000);
225 final Vector3D p2 = propagator.propagate(t2).getPosition(teme);
226 final AbsoluteDate t3 = t1.shiftedBy(115200.0);
227 final Vector3D p3 = propagator.propagate(t3).getPosition(teme);
228
229 IodLambert lambert = new IodLambert(Constants.WGS84_EARTH_MU);
230 Orbit k0 = lambert.estimate(teme, true, 0, p1, t1, p2, t2);
231 Assertions.assertEquals(6.08e-4, k0.getE(), 1.0e-6);
232 Assertions.assertEquals(8.55, FastMath.toDegrees(k0.getI()), 0.01);
233 Assertions.assertEquals(0.0, Vector3D.distance(p1, k0.getPosition(t1, teme)), 2.0e-8);
234 Assertions.assertEquals(0.0, Vector3D.distance(p2, k0.getPosition(t2, teme)), 2.0e-7);
235
236 Orbit k1 = lambert.estimate(teme, true, 1, p1, t1, p3, t3);
237 Assertions.assertEquals(5.97e-4, k1.getE(), 1.0e-6);
238 Assertions.assertEquals(8.55, FastMath.toDegrees(k1.getI()), 0.01);
239 Assertions.assertEquals(0.0, Vector3D.distance(p1, k1.getPosition(t1, teme)), 1.4e-8);
240 Assertions.assertEquals(0.0, Vector3D.distance(p3, k1.getPosition(t3, teme)), 3.0e-7);
241
242 }
243
244 @Test
245 public void testNonChronologicalObservations() {
246
247
248 final Context context = EstimationTestUtils.eccentricContext("regular-data:potential:tides");
249 final double mu = context.initialOrbit.getMu();
250 final Frame frame = context.initialOrbit.getFrame();
251
252
253 final NumericalPropagatorBuilder propagatorBuilder =
254 context.createBuilder(OrbitType.KEPLERIAN, PositionAngleType.TRUE, true,
255 1.0e-6, 60.0, 0.001);
256
257
258 final Propagator propagator = EstimationTestUtils.createPropagator(context.initialOrbit,
259 propagatorBuilder);
260
261 final SpacecraftState initialState = propagator.getInitialState();
262 final KeplerianOrbit refOrbit = new KeplerianOrbit(initialState.getOrbit());
263 double[] refOrbitArray = new double[6];
264 OrbitType.KEPLERIAN.mapOrbitToArray(refOrbit, PositionAngleType.TRUE, refOrbitArray, null);
265
266 final Vector3D position1 = refOrbit.getPosition();
267 final AbsoluteDate date1 = refOrbit.getDate();
268
269
270 final double T = context.initialOrbit.getKeplerianPeriod();
271
272 final double dts = -T/4;
273 final int nRevs = 0;
274 final boolean posigrades = true;
275
276
277 propagator.resetInitialState(initialState);
278
279
280 final AbsoluteDate date2 = date1.shiftedBy(dts);
281 final Vector3D position2 = propagator.propagate(date2).getPosition();
282
283
284 final IodLambert iod = new IodLambert(mu);
285
286
287 try {
288 iod.estimate(frame, posigrades, nRevs, position1, date1, position2, date2);
289 Assertions.fail("An exception should have been thrown");
290
291 } catch (OrekitException oe) {
292 Assertions.assertEquals(OrekitMessages.NON_CHRONOLOGICAL_DATES_FOR_OBSERVATIONS, oe.getSpecifier());
293 }
294 }
295
296
297
298
299
300
301
302
303 @Test
304 public void testIssue533() {
305
306 double mu = Constants.EGM96_EARTH_MU;
307 Frame j2000 = FramesFactory.getEME2000();
308
309
310
311
312
313
314
315
316
317
318 for (int testCase = 1; testCase < 3; testCase++) {
319
320 double trueAnomalyDifference;
321 boolean posigrade;
322 int nRev;
323 if (testCase == 1) {
324 posigrade = true;
325 nRev = 0;
326 trueAnomalyDifference = 0.5*FastMath.PI;
327 } else {
328 posigrade = false;
329 nRev = 0;
330 trueAnomalyDifference = 1.5*FastMath.PI;
331 }
332
333
334 double a = 24000*1e3;
335 double e = 0.72;
336 double i = FastMath.toRadians(0.);
337 double aop = 0.;
338 double raan = 0.;
339 double nu0 = 0.;
340
341
342 AbsoluteDate t1 = new AbsoluteDate(2010, 1, 1, 0, 0, 0, TimeScalesFactory.getUTC());
343 KeplerianOrbit kep1 = new KeplerianOrbit(a, e, i, aop, raan, nu0, PositionAngleType.TRUE, j2000, t1, mu);
344 Vector3D p1 = kep1.getPosition();
345 Vector3D v1 = kep1.getVelocity();
346
347
348 KeplerianOrbit kep2 = new KeplerianOrbit(a, e, i, aop, raan, trueAnomalyDifference, PositionAngleType.TRUE, j2000, t1, mu);
349 double n = kep2.getKeplerianMeanMotion();
350 double M2 = kep2.getMeanAnomaly();
351 double delta_t = M2 / n;
352 AbsoluteDate t2 = t1.shiftedBy(delta_t);
353
354
355 PVCoordinates pv2 = kep1.getPVCoordinates(t2, j2000);
356 Vector3D p2 = pv2.getPosition();
357 Vector3D v2 = pv2.getVelocity();
358
359
360 IodLambert iodLambert = new IodLambert(mu);
361 Orbit resultOrbit1 = iodLambert.estimate(j2000, posigrade, nRev, p1, t1, p2, t2);
362
363
364 PVCoordinates resultPv1 = resultOrbit1.getPVCoordinates(t1, j2000);
365 PVCoordinates resultPv2 = resultOrbit1.getPVCoordinates(t2, j2000);
366
367 Vector3D resultP1 = resultPv1.getPosition();
368 Vector3D resultV1 = resultPv1.getVelocity();
369 Vector3D resultP2 = resultPv2.getPosition();
370 Vector3D resultV2 = resultPv2.getVelocity();
371
372
373 double dP1 = Vector3D.distance(p1, resultP1);
374 double dV1 = Vector3D.distance(v1, resultV1);
375 double dP2 = Vector3D.distance(p2, resultP2);
376 double dV2 = Vector3D.distance(v2, resultV2);
377
378
379 double dP1Tol, dP2Tol, dV1Tol, dV2Tol;
380 if (testCase == 1) {
381 dP1Tol = 5.65e-25;
382 dV1Tol = 5.97e-12;
383 dP2Tol = 7.57e-9;
384 dV2Tol = 7.50e-12;
385 } else {
386 dP1Tol = 5.47e-25;
387 dV1Tol = 3.03e-12;
388 dP2Tol = 9.86e-7;
389 dV2Tol = 4.01e-10;
390 }
391
392
393 Assertions.assertEquals(0., dP1, dP1Tol);
394 Assertions.assertEquals(0., dV1, dV1Tol);
395 Assertions.assertEquals(0., dP2, dP2Tol);
396 Assertions.assertEquals(0., dV2, dV2Tol);
397 }
398 }
399
400 @Test
401 public void testIssue752() {
402
403
404
405
406 final Frame inertialFrame = FramesFactory.getEME2000();
407 final TimeScale utc = TimeScalesFactory.getUTC();
408
409
410 final IodLambert lambert = new IodLambert(Constants.EGM96_EARTH_MU);
411
412
413 final ObservableSatellite satellite = new ObservableSatellite(0);
414
415
416 final Vector3D posR1 = new Vector3D(22592145.603, -1599915.239, -19783950.506);
417 final Vector3D posR2 = new Vector3D(1922067.697, 4054157.051, -8925727.465);
418
419
420 AbsoluteDate dateRef = new AbsoluteDate(2018, 11, 1, 0, 0, 0.0, utc);
421 AbsoluteDate date2 = dateRef.shiftedBy(36000.0);
422
423
424 final Vector3D velR1 = new Vector3D(2000.652697, 387.688615, -2666.947760);
425 final Vector3D velR2 = new Vector3D(-3792.46619, -1777.07641, 6856.81495);
426
427
428 final Orbit orbit = lambert.estimate(inertialFrame, true, 0,
429 new Position(dateRef, posR1, 1.0, 1.0, satellite),
430 new Position(date2, posR2, 1.0, 1.0, satellite));
431
432
433 Assertions.assertEquals(0.0, orbit.getVelocity().getNorm() - velR1.getNorm(), 1e-3);
434
435
436 final KeplerianPropagator kepler = new KeplerianPropagator(orbit);
437 Assertions.assertEquals(0.0, kepler.propagate(date2).getVelocity().getNorm() - velR2.getNorm(), 1e-3);
438
439
440 }
441
442 @Test
443 public void testWithPVMeasurements() {
444
445
446
447
448 final Frame inertialFrame = FramesFactory.getEME2000();
449 final TimeScale utc = TimeScalesFactory.getUTC();
450
451
452 final IodLambert lambert = new IodLambert(Constants.EGM96_EARTH_MU);
453
454
455 final ObservableSatellite satellite = new ObservableSatellite(0);
456
457
458 final Vector3D posR1 = new Vector3D(22592145.603, -1599915.239, -19783950.506);
459 final Vector3D posR2 = new Vector3D(1922067.697, 4054157.051, -8925727.465);
460
461
462 AbsoluteDate dateRef = new AbsoluteDate(2018, 11, 1, 0, 0, 0.0, utc);
463 AbsoluteDate date2 = dateRef.shiftedBy(36000.0);
464
465
466 final Vector3D velR1 = new Vector3D(2000.652697, 387.688615, -2666.947760);
467 final Vector3D velR2 = new Vector3D(-3792.46619, -1777.07641, 6856.81495);
468
469
470 final Orbit orbit = lambert.estimate(inertialFrame, true, 0,
471 new PV(dateRef, posR1, velR1, 1.0, 1.0, 1.0, satellite),
472 new PV(date2, posR2, velR2, 1.0, 1.0, 1.0, satellite));
473
474
475 Assertions.assertEquals(0.0, orbit.getVelocity().getNorm() - velR1.getNorm(), 1e-3);
476
477
478 final KeplerianPropagator kepler = new KeplerianPropagator(orbit);
479 Assertions.assertEquals(0.0, kepler.propagate(date2).getVelocity().getNorm() - velR2.getNorm(), 1e-3);
480
481
482 }
483
484 @BeforeEach
485 public void setUp() {
486 Utils.setDataRoot("regular-data");
487 }
488
489 }