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