1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.analytical.tle;
18
19 import java.util.ArrayList;
20 import java.util.Collections;
21 import java.util.HashMap;
22 import java.util.List;
23 import java.util.Map;
24
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.linear.RealMatrix;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.MathUtils;
29 import org.hipparchus.util.SinCos;
30 import org.orekit.annotation.DefaultDataContext;
31 import org.orekit.attitudes.Attitude;
32 import org.orekit.attitudes.AttitudeProvider;
33 import org.orekit.attitudes.FrameAlignedProvider;
34 import org.orekit.data.DataContext;
35 import org.orekit.errors.OrekitException;
36 import org.orekit.errors.OrekitMessages;
37 import org.orekit.frames.Frame;
38 import org.orekit.orbits.CartesianOrbit;
39 import org.orekit.orbits.Orbit;
40 import org.orekit.propagation.AbstractMatricesHarvester;
41 import org.orekit.propagation.MatricesHarvester;
42 import org.orekit.propagation.SpacecraftState;
43 import org.orekit.propagation.analytical.AbstractAnalyticalPropagator;
44 import org.orekit.propagation.analytical.tle.generation.FixedPointTleGenerationAlgorithm;
45 import org.orekit.propagation.analytical.tle.generation.TleGenerationAlgorithm;
46 import org.orekit.time.AbsoluteDate;
47 import org.orekit.time.TimeScale;
48 import org.orekit.utils.DoubleArrayDictionary;
49 import org.orekit.utils.PVCoordinates;
50 import org.orekit.utils.ParameterDriver;
51 import org.orekit.utils.TimeSpanMap;
52 import org.orekit.utils.TimeSpanMap.Span;
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80 public abstract class TLEPropagator extends AbstractAnalyticalPropagator {
81
82
83
84
85 protected TLE tle;
86
87
88 protected final TimeScale utc;
89
90
91 protected double xnode;
92
93
94 protected double a;
95
96
97 protected double e;
98
99
100 protected double i;
101
102
103 protected double omega;
104
105
106 protected double xl;
107
108
109 protected double a0dp;
110
111
112 protected double xn0dp;
113
114
115 protected double cosi0;
116
117
118 protected double theta2;
119
120
121 protected double sini0;
122
123
124 protected double xmdot;
125
126
127 protected double omgdot;
128
129
130 protected double xnodot;
131
132
133 protected double e0sq;
134
135 protected double beta02;
136
137
138 protected double beta0;
139
140
141 protected double perige;
142
143
144 protected double etasq;
145
146
147 protected double eeta;
148
149
150 protected double s4;
151
152
153 protected double tsi;
154
155
156 protected double eta;
157
158
159 protected double coef;
160
161
162 protected double coef1;
163
164
165 protected double c1;
166
167
168 protected double c2;
169
170
171 protected double c4;
172
173
174 protected double xnodcf;
175
176
177 protected double t2cof;
178
179
180
181
182 private final Frame teme;
183
184
185 private Map<TLE, Double> masses;
186
187
188 private TimeSpanMap<TLE> tles;
189
190
191
192
193
194
195
196
197
198
199 @DefaultDataContext
200 protected TLEPropagator(final TLE initialTLE, final AttitudeProvider attitudeProvider,
201 final double mass) {
202 this(initialTLE, attitudeProvider, mass,
203 DataContext.getDefault().getFrames().getTEME());
204 }
205
206
207
208
209
210
211
212
213 protected TLEPropagator(final TLE initialTLE,
214 final AttitudeProvider attitudeProvider,
215 final double mass,
216 final Frame teme) {
217 super(attitudeProvider);
218 setStartDate(initialTLE.getDate());
219 this.utc = initialTLE.getUtc();
220 initializeTle(initialTLE);
221 this.teme = teme;
222 this.tles = new TimeSpanMap<>(tle);
223 this.masses = new HashMap<>();
224 this.masses.put(tle, mass);
225
226
227 final Orbit orbit = propagateOrbit(initialTLE.getDate());
228 final Attitude attitude = attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
229 super.resetInitialState(new SpacecraftState(orbit, attitude, mass));
230 }
231
232
233
234
235
236
237
238
239
240 @DefaultDataContext
241 public static TLEPropagator selectExtrapolator(final TLE tle) {
242 return selectExtrapolator(tle, DataContext.getDefault().getFrames().getTEME());
243 }
244
245
246
247
248
249
250
251 public static TLEPropagator selectExtrapolator(final TLE tle, final Frame teme) {
252 return selectExtrapolator(
253 tle,
254 FrameAlignedProvider.of(teme),
255 DEFAULT_MASS,
256 teme);
257 }
258
259
260
261
262
263
264
265
266
267
268
269 @DefaultDataContext
270 public static TLEPropagator selectExtrapolator(final TLE tle, final AttitudeProvider attitudeProvider,
271 final double mass) {
272 return selectExtrapolator(tle, attitudeProvider, mass,
273 DataContext.getDefault().getFrames().getTEME());
274 }
275
276
277
278
279
280
281
282
283
284 public static TLEPropagator selectExtrapolator(final TLE tle,
285 final AttitudeProvider attitudeProvider,
286 final double mass,
287 final Frame teme) {
288
289 final double a1 = FastMath.pow( TLEConstants.XKE / (tle.getMeanMotion() * 60.0), TLEConstants.TWO_THIRD);
290 final double cosi0 = FastMath.cos(tle.getI());
291 final double temp = TLEConstants.CK2 * 1.5 * (3 * cosi0 * cosi0 - 1.0) *
292 FastMath.pow(1.0 - tle.getE() * tle.getE(), -1.5);
293 final double delta1 = temp / (a1 * a1);
294 final double a0 = a1 * (1.0 - delta1 * (TLEConstants.ONE_THIRD + delta1 * (delta1 * 134.0 / 81.0 + 1.0)));
295 final double delta0 = temp / (a0 * a0);
296
297
298 final double xn0dp = tle.getMeanMotion() * 60.0 / (delta0 + 1.0);
299
300
301 if (MathUtils.TWO_PI / (xn0dp * TLEConstants.MINUTES_PER_DAY) >= (1.0 / 6.4)) {
302 return new DeepSDP4(tle, attitudeProvider, mass, teme);
303 } else {
304 return new SGP4(tle, attitudeProvider, mass, teme);
305 }
306 }
307
308
309
310
311 public static double getMU() {
312 return TLEConstants.MU;
313 }
314
315
316
317
318
319 public PVCoordinates getPVCoordinates(final AbsoluteDate date) {
320
321 sxpPropagate(date.durationFrom(tle.getDate()) / 60.0);
322
323
324 return computePVCoordinates();
325 }
326
327
328
329 private void initializeCommons() {
330
331
332 final SinCos scI0 = FastMath.sinCos(tle.getI());
333
334 final double a1 = FastMath.pow(TLEConstants.XKE / (tle.getMeanMotion() * 60.0), TLEConstants.TWO_THIRD);
335 cosi0 = scI0.cos();
336 theta2 = cosi0 * cosi0;
337 final double x3thm1 = 3.0 * theta2 - 1.0;
338 e0sq = tle.getE() * tle.getE();
339 beta02 = 1.0 - e0sq;
340 beta0 = FastMath.sqrt(beta02);
341 final double tval = TLEConstants.CK2 * 1.5 * x3thm1 / (beta0 * beta02);
342 final double delta1 = tval / (a1 * a1);
343 final double a0 = a1 * (1.0 - delta1 * (TLEConstants.ONE_THIRD + delta1 * (1.0 + 134.0 / 81.0 * delta1)));
344 final double delta0 = tval / (a0 * a0);
345
346
347 xn0dp = tle.getMeanMotion() * 60.0 / (delta0 + 1.0);
348 a0dp = a0 / (1.0 - delta0);
349
350
351 s4 = TLEConstants.S;
352 double q0ms24 = TLEConstants.QOMS2T;
353
354 perige = (a0dp * (1 - tle.getE()) - TLEConstants.NORMALIZED_EQUATORIAL_RADIUS) * TLEConstants.EARTH_RADIUS;
355
356
357 if (perige < 156.0) {
358 if (perige <= 98.0) {
359 s4 = 20.0;
360 } else {
361 s4 = perige - 78.0;
362 }
363 final double temp_val = (120.0 - s4) * TLEConstants.NORMALIZED_EQUATORIAL_RADIUS / TLEConstants.EARTH_RADIUS;
364 final double temp_val_squared = temp_val * temp_val;
365 q0ms24 = temp_val_squared * temp_val_squared;
366 s4 = s4 / TLEConstants.EARTH_RADIUS + TLEConstants.NORMALIZED_EQUATORIAL_RADIUS;
367 }
368
369 final double pinv = 1.0 / (a0dp * beta02);
370 final double pinvsq = pinv * pinv;
371 tsi = 1.0 / (a0dp - s4);
372 eta = a0dp * tle.getE() * tsi;
373 etasq = eta * eta;
374 eeta = tle.getE() * eta;
375
376 final double psisq = FastMath.abs(1.0 - etasq);
377 final double tsi_squared = tsi * tsi;
378 coef = q0ms24 * tsi_squared * tsi_squared;
379 coef1 = coef / FastMath.pow(psisq, 3.5);
380
381
382 c2 = coef1 * xn0dp * (a0dp * (1.0 + 1.5 * etasq + eeta * (4.0 + etasq)) +
383 0.75 * TLEConstants.CK2 * tsi / psisq * x3thm1 * (8.0 + 3.0 * etasq * (8.0 + etasq)));
384 c1 = tle.getBStar() * c2;
385 sini0 = scI0.sin();
386
387 final double x1mth2 = 1.0 - theta2;
388
389
390 c4 = 2.0 * xn0dp * coef1 * a0dp * beta02 * (eta * (2.0 + 0.5 * etasq) +
391 tle.getE() * (0.5 + 2.0 * etasq) -
392 2 * TLEConstants.CK2 * tsi / (a0dp * psisq) *
393 (-3.0 * x3thm1 * (1.0 - 2.0 * eeta + etasq * (1.5 - 0.5 * eeta)) +
394 0.75 * x1mth2 * (2.0 * etasq - eeta * (1.0 + etasq)) * FastMath.cos(2.0 * tle.getPerigeeArgument())));
395
396 final double theta4 = theta2 * theta2;
397 final double temp1 = 3 * TLEConstants.CK2 * pinvsq * xn0dp;
398 final double temp2 = temp1 * TLEConstants.CK2 * pinvsq;
399 final double temp3 = 1.25 * TLEConstants.CK4 * pinvsq * pinvsq * xn0dp;
400
401
402 xmdot = xn0dp +
403 0.5 * temp1 * beta0 * x3thm1 +
404 0.0625 * temp2 * beta0 * (13.0 - 78.0 * theta2 + 137.0 * theta4);
405
406 final double x1m5th = 1.0 - 5.0 * theta2;
407
408 omgdot = -0.5 * temp1 * x1m5th +
409 0.0625 * temp2 * (7.0 - 114.0 * theta2 + 395.0 * theta4) +
410 temp3 * (3.0 - 36.0 * theta2 + 49.0 * theta4);
411
412 final double xhdot1 = -temp1 * cosi0;
413
414 xnodot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * theta2) + 2.0 * temp3 * (3.0 - 7.0 * theta2)) * cosi0;
415 xnodcf = 3.5 * beta02 * xhdot1 * c1;
416 t2cof = 1.5 * c1;
417
418 }
419
420
421
422
423 private PVCoordinates computePVCoordinates() {
424
425
426 final SinCos scOmega = FastMath.sinCos(omega);
427
428
429 final double axn = e * scOmega.cos();
430 double temp = 1.0 / (a * (1.0 - e * e));
431 final double xlcof = 0.125 * TLEConstants.A3OVK2 * sini0 * (3.0 + 5.0 * cosi0) / (1.0 + cosi0);
432 final double aycof = 0.25 * TLEConstants.A3OVK2 * sini0;
433 final double xll = temp * xlcof * axn;
434 final double aynl = temp * aycof;
435 final double xlt = xl + xll;
436 final double ayn = e * scOmega.sin() + aynl;
437 final double elsq = axn * axn + ayn * ayn;
438 final double capu = MathUtils.normalizeAngle(xlt - xnode, FastMath.PI);
439 double epw = capu;
440 double ecosE = 0;
441 double esinE = 0;
442 double sinEPW = 0;
443 double cosEPW = 0;
444
445
446 final double cosi0Sq = cosi0 * cosi0;
447 final double x3thm1 = 3.0 * cosi0Sq - 1.0;
448 final double x1mth2 = 1.0 - cosi0Sq;
449 final double x7thm1 = 7.0 * cosi0Sq - 1.0;
450
451 if (e > (1 - 1e-6)) {
452 throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL, e);
453 }
454
455
456 final double newtonRaphsonEpsilon = 1e-12;
457 for (int j = 0; j < 10; j++) {
458
459 boolean doSecondOrderNewtonRaphson = true;
460
461 final SinCos scEPW = FastMath.sinCos(epw);
462 sinEPW = scEPW.sin();
463 cosEPW = scEPW.cos();
464 ecosE = axn * cosEPW + ayn * sinEPW;
465 esinE = axn * sinEPW - ayn * cosEPW;
466 final double f = capu - epw + esinE;
467 if (FastMath.abs(f) < newtonRaphsonEpsilon) {
468 break;
469 }
470 final double fdot = 1.0 - ecosE;
471 double delta_epw = f / fdot;
472 if (j == 0) {
473 final double maxNewtonRaphson = 1.25 * FastMath.abs(e);
474 doSecondOrderNewtonRaphson = false;
475 if (delta_epw > maxNewtonRaphson) {
476 delta_epw = maxNewtonRaphson;
477 } else if (delta_epw < -maxNewtonRaphson) {
478 delta_epw = -maxNewtonRaphson;
479 } else {
480 doSecondOrderNewtonRaphson = true;
481 }
482 }
483 if (doSecondOrderNewtonRaphson) {
484 delta_epw = f / (fdot + 0.5 * esinE * delta_epw);
485 }
486 epw += delta_epw;
487 }
488
489
490 temp = 1.0 - elsq;
491 final double pl = a * temp;
492 final double r = a * (1.0 - ecosE);
493 double temp2 = a / r;
494 final double betal = FastMath.sqrt(temp);
495 temp = esinE / (1.0 + betal);
496 final double cosu = temp2 * (cosEPW - axn + ayn * temp);
497 final double sinu = temp2 * (sinEPW - ayn - axn * temp);
498 final double u = FastMath.atan2(sinu, cosu);
499 final double sin2u = 2.0 * sinu * cosu;
500 final double cos2u = 2.0 * cosu * cosu - 1.0;
501 final double temp1 = TLEConstants.CK2 / pl;
502 temp2 = temp1 / pl;
503
504
505 final double rk = r * (1.0 - 1.5 * temp2 * betal * x3thm1) + 0.5 * temp1 * x1mth2 * cos2u;
506 final double uk = u - 0.25 * temp2 * x7thm1 * sin2u;
507 final double xnodek = xnode + 1.5 * temp2 * cosi0 * sin2u;
508 final double xinck = i + 1.5 * temp2 * cosi0 * sini0 * cos2u;
509
510
511 final SinCos scuk = FastMath.sinCos(uk);
512 final SinCos scik = FastMath.sinCos(xinck);
513 final SinCos scnok = FastMath.sinCos(xnodek);
514 final double sinuk = scuk.sin();
515 final double cosuk = scuk.cos();
516 final double sinik = scik.sin();
517 final double cosik = scik.cos();
518 final double sinnok = scnok.sin();
519 final double cosnok = scnok.cos();
520 final double xmx = -sinnok * cosik;
521 final double xmy = cosnok * cosik;
522 final double ux = xmx * sinuk + cosnok * cosuk;
523 final double uy = xmy * sinuk + sinnok * cosuk;
524 final double uz = sinik * sinuk;
525
526
527 final double cr = 1000 * rk * TLEConstants.EARTH_RADIUS;
528 final Vector3D pos = new Vector3D(cr * ux, cr * uy, cr * uz);
529
530 final double rdot = TLEConstants.XKE * FastMath.sqrt(a) * esinE / r;
531 final double rfdot = TLEConstants.XKE * FastMath.sqrt(pl) / r;
532 final double xn = TLEConstants.XKE / (a * FastMath.sqrt(a));
533 final double rdotk = rdot - xn * temp1 * x1mth2 * sin2u;
534 final double rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1);
535 final double vx = xmx * cosuk - cosnok * sinuk;
536 final double vy = xmy * cosuk - sinnok * sinuk;
537 final double vz = sinik * cosuk;
538
539 final double cv = 1000.0 * TLEConstants.EARTH_RADIUS / 60.0;
540 final Vector3D vel = new Vector3D(cv * (rdotk * ux + rfdotk * vx),
541 cv * (rdotk * uy + rfdotk * vy),
542 cv * (rdotk * uz + rfdotk * vz));
543
544 return new PVCoordinates(pos, vel);
545
546 }
547
548
549
550 protected abstract void sxpInitialize();
551
552
553
554
555 protected abstract void sxpPropagate(double t);
556
557
558
559
560
561
562
563
564
565 public void resetInitialState(final SpacecraftState state) {
566 super.resetInitialState(state);
567 resetTle(state);
568 masses = new HashMap<>();
569 masses.put(tle, state.getMass());
570 tles = new TimeSpanMap<>(tle);
571 }
572
573
574 protected void resetIntermediateState(final SpacecraftState state, final boolean forward) {
575 resetTle(state);
576 if (forward) {
577 tles.addValidAfter(tle, state.getDate(), false);
578 } else {
579 tles.addValidBefore(tle, state.getDate(), false);
580 }
581 stateChanged(state);
582 masses.put(tle, state.getMass());
583 }
584
585
586
587
588 private void resetTle(final SpacecraftState state) {
589 final TleGenerationAlgorithm algorithm = getDefaultTleGenerationAlgorithm(utc, teme);
590 final TLE newTle = algorithm.generate(state, tle);
591 initializeTle(newTle);
592 }
593
594
595
596
597 private void initializeTle(final TLE newTle) {
598 tle = newTle;
599 initializeCommons();
600 sxpInitialize();
601 }
602
603
604 protected double getMass(final AbsoluteDate date) {
605 return masses.get(tles.get(date));
606 }
607
608
609 protected Orbit propagateOrbit(final AbsoluteDate date) {
610 final TLE closestTle = tles.get(date);
611 if (!tle.equals(closestTle)) {
612 initializeTle(closestTle);
613 }
614 return new CartesianOrbit(getPVCoordinates(date), teme, date, TLEConstants.MU);
615 }
616
617
618
619
620
621
622 public TLE getTLE() {
623 return tle;
624 }
625
626
627 public Frame getFrame() {
628 return teme;
629 }
630
631
632 @Override
633 protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
634 final DoubleArrayDictionary initialJacobianColumns) {
635
636 final TLEHarvester harvester = new TLEHarvester(this, stmName, initialStm, initialJacobianColumns);
637
638 addAdditionalStateProvider(harvester);
639
640 return harvester;
641 }
642
643
644
645
646
647 protected List<String> getJacobiansColumnsNames() {
648 final List<String> columnsNames = new ArrayList<>();
649 for (final ParameterDriver driver : tle.getParametersDrivers()) {
650
651 if (driver.isSelected() && !columnsNames.contains(driver.getNamesSpanMap().getFirstSpan().getData())) {
652
653
654 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
655 columnsNames.add(span.getData());
656 }
657 }
658 }
659 Collections.sort(columnsNames);
660 return columnsNames;
661 }
662
663
664
665
666
667
668
669
670 public static TleGenerationAlgorithm getDefaultTleGenerationAlgorithm(final TimeScale utc, final Frame teme) {
671 return new FixedPointTleGenerationAlgorithm(FixedPointTleGenerationAlgorithm.EPSILON_DEFAULT,
672 FixedPointTleGenerationAlgorithm.MAX_ITERATIONS_DEFAULT,
673 FixedPointTleGenerationAlgorithm.SCALE_DEFAULT, utc, teme);
674 }
675
676 }