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