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