1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.analytical;
18
19 import java.util.Collections;
20 import java.util.List;
21
22 import org.hipparchus.Field;
23 import org.hipparchus.CalculusFieldElement;
24 import org.hipparchus.analysis.differentiation.FieldUnivariateDerivative2;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.util.FastMath;
27 import org.hipparchus.util.FieldSinCos;
28 import org.hipparchus.util.MathArrays;
29 import org.hipparchus.util.MathUtils;
30 import org.orekit.attitudes.AttitudeProvider;
31 import org.orekit.attitudes.InertialProvider;
32 import org.orekit.errors.OrekitException;
33 import org.orekit.errors.OrekitMessages;
34 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
35 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics;
36 import org.orekit.orbits.FieldCartesianOrbit;
37 import org.orekit.orbits.FieldCircularOrbit;
38 import org.orekit.orbits.FieldOrbit;
39 import org.orekit.orbits.OrbitType;
40 import org.orekit.orbits.PositionAngle;
41 import org.orekit.propagation.FieldSpacecraftState;
42 import org.orekit.propagation.PropagationType;
43 import org.orekit.time.FieldAbsoluteDate;
44 import org.orekit.utils.FieldTimeSpanMap;
45 import org.orekit.utils.ParameterDriver;
46 import org.orekit.utils.TimeStampedFieldPVCoordinates;
47
48
49
50
51
52
53
54
55
56
57 public class FieldEcksteinHechlerPropagator<T extends CalculusFieldElement<T>> extends FieldAbstractAnalyticalPropagator<T> {
58
59
60 private FieldEHModel<T> initialModel;
61
62
63 private transient FieldTimeSpanMap<FieldEHModel<T>, T> models;
64
65
66 private double referenceRadius;
67
68
69 private T mu;
70
71
72 private double[] ck0;
73
74
75
76
77
78
79
80
81
82
83
84
85
86 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
87 final UnnormalizedSphericalHarmonicsProvider provider) {
88 this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
89 initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
90 provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
91 }
92
93
94
95
96
97
98
99
100
101
102
103
104 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
105 final AttitudeProvider attitude,
106 final T mass,
107 final UnnormalizedSphericalHarmonicsProvider provider,
108 final UnnormalizedSphericalHarmonicsProvider.UnnormalizedSphericalHarmonics harmonics) {
109 this(initialOrbit, attitude, mass, provider.getAe(), initialOrbit.getA().getField().getZero().add(provider.getMu()),
110 harmonics.getUnnormalizedCnm(2, 0),
111 harmonics.getUnnormalizedCnm(3, 0),
112 harmonics.getUnnormalizedCnm(4, 0),
113 harmonics.getUnnormalizedCnm(5, 0),
114 harmonics.getUnnormalizedCnm(6, 0));
115 }
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
144 final double referenceRadius, final T mu,
145 final double c20, final double c30, final double c40,
146 final double c50, final double c60) {
147 this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
148 initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
149 referenceRadius, mu, c20, c30, c40, c50, c60);
150 }
151
152
153
154
155
156
157
158
159
160
161
162
163 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
164 final UnnormalizedSphericalHarmonicsProvider provider) {
165 this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
166 mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
167 }
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit, final T mass,
196 final double referenceRadius, final T mu,
197 final double c20, final double c30, final double c40,
198 final double c50, final double c60) {
199 this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
200 mass, referenceRadius, mu, c20, c30, c40, c50, c60);
201 }
202
203
204
205
206
207
208
209
210 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
211 final AttitudeProvider attitudeProv,
212 final UnnormalizedSphericalHarmonicsProvider provider) {
213 this(initialOrbit, attitudeProv, initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
214 }
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
241 final AttitudeProvider attitudeProv,
242 final double referenceRadius, final T mu,
243 final double c20, final double c30, final double c40,
244 final double c50, final double c60) {
245 this(initialOrbit, attitudeProv, initialOrbit.getDate().getField().getZero().add(DEFAULT_MASS),
246 referenceRadius, mu, c20, c30, c40, c50, c60);
247 }
248
249
250
251
252
253
254
255
256
257
258 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
259 final AttitudeProvider attitudeProv,
260 final T mass,
261 final UnnormalizedSphericalHarmonicsProvider provider) {
262 this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()));
263 }
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
292 final AttitudeProvider attitudeProv,
293 final T mass,
294 final double referenceRadius, final T mu,
295 final double c20, final double c30, final double c40,
296 final double c50, final double c60) {
297 this(initialOrbit, attitudeProv, mass, referenceRadius, mu, c20, c30, c40, c50, c60, PropagationType.OSCULATING);
298 }
299
300
301
302
303
304
305
306
307
308
309
310
311 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
312 final UnnormalizedSphericalHarmonicsProvider provider,
313 final PropagationType initialType) {
314 this(initialOrbit, InertialProvider.of(initialOrbit.getFrame()),
315 initialOrbit.getA().getField().getZero().add(DEFAULT_MASS), provider,
316 provider.onDate(initialOrbit.getDate().toAbsoluteDate()), initialType);
317 }
318
319
320
321
322
323
324
325
326
327
328
329 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
330 final AttitudeProvider attitudeProv,
331 final T mass,
332 final UnnormalizedSphericalHarmonicsProvider provider,
333 final PropagationType initialType) {
334 this(initialOrbit, attitudeProv, mass, provider, provider.onDate(initialOrbit.getDate().toAbsoluteDate()), initialType);
335 }
336
337
338
339
340
341
342
343
344
345
346
347
348
349 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
350 final AttitudeProvider attitude,
351 final T mass,
352 final UnnormalizedSphericalHarmonicsProvider provider,
353 final UnnormalizedSphericalHarmonics harmonics,
354 final PropagationType initialType) {
355 this(initialOrbit, attitude, mass, provider.getAe(), initialOrbit.getA().getField().getZero().add(provider.getMu()),
356 harmonics.getUnnormalizedCnm(2, 0),
357 harmonics.getUnnormalizedCnm(3, 0),
358 harmonics.getUnnormalizedCnm(4, 0),
359 harmonics.getUnnormalizedCnm(5, 0),
360 harmonics.getUnnormalizedCnm(6, 0),
361 initialType);
362 }
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391 public FieldEcksteinHechlerPropagator(final FieldOrbit<T> initialOrbit,
392 final AttitudeProvider attitudeProv,
393 final T mass,
394 final double referenceRadius, final T mu,
395 final double c20, final double c30, final double c40,
396 final double c50, final double c60,
397 final PropagationType initialType) {
398
399 super(mass.getField(), attitudeProv);
400 try {
401
402
403 this.referenceRadius = referenceRadius;
404 this.mu = mu;
405 this.ck0 = new double[] {
406 0.0, 0.0, c20, c30, c40, c50, c60
407 };
408
409
410
411 resetInitialState(new FieldSpacecraftState<>(initialOrbit,
412 attitudeProv.getAttitude(initialOrbit,
413 initialOrbit.getDate(),
414 initialOrbit.getFrame()),
415 mass),
416 initialType);
417
418 } catch (OrekitException oe) {
419 throw new OrekitException(oe);
420 }
421 }
422
423
424
425
426
427
428 @Override
429 public void resetInitialState(final FieldSpacecraftState<T> state) {
430 resetInitialState(state, PropagationType.OSCULATING);
431 }
432
433
434
435
436
437
438 public void resetInitialState(final FieldSpacecraftState<T> state, final PropagationType stateType) {
439 super.resetInitialState(state);
440 final FieldCircularOrbit<T> circular = (FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit());
441 this.initialModel = (stateType == PropagationType.MEAN) ?
442 new FieldEHModel<>(circular, state.getMass(), referenceRadius, mu, ck0) :
443 computeMeanParameters(circular, state.getMass());
444 this.models = new FieldTimeSpanMap<FieldEHModel<T>, T>(initialModel, state.getA().getField());
445 }
446
447
448 @Override
449 protected void resetIntermediateState(final FieldSpacecraftState<T> state, final boolean forward) {
450 final FieldEHModel<T> newModel = computeMeanParameters((FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(state.getOrbit()),
451 state.getMass());
452 if (forward) {
453 models.addValidAfter(newModel, state.getDate());
454 } else {
455 models.addValidBefore(newModel, state.getDate());
456 }
457 stateChanged(state);
458 }
459
460
461
462
463
464
465 private FieldEHModel<T> computeMeanParameters(final FieldCircularOrbit<T> osculating, final T mass) {
466
467
468 if (osculating.getA().getReal() < referenceRadius) {
469 throw new OrekitException(OrekitMessages.TRAJECTORY_INSIDE_BRILLOUIN_SPHERE,
470 osculating.getA());
471 }
472 final Field<T> field = mass.getField();
473 final T one = field.getOne();
474 final T zero = field.getZero();
475
476 FieldEHModel<T> current = new FieldEHModel<>(osculating, mass, referenceRadius, mu, ck0);
477
478 final T epsilon = one.multiply(1.0e-13);
479 final T thresholdA = epsilon.multiply(current.mean.getA().abs().add(1.0));
480 final T thresholdE = epsilon.multiply(current.mean.getE().add(1.0));
481 final T thresholdAngles = epsilon.multiply(one.getPi());
482
483
484 int i = 0;
485 while (i++ < 100) {
486
487
488 final FieldUnivariateDerivative2<T>[] parameters = current.propagateParameters(current.mean.getDate());
489
490 final T deltaA = osculating.getA() .subtract(parameters[0].getValue());
491 final T deltaEx = osculating.getCircularEx().subtract(parameters[1].getValue());
492 final T deltaEy = osculating.getCircularEy().subtract(parameters[2].getValue());
493 final T deltaI = osculating.getI() .subtract(parameters[3].getValue());
494 final T deltaRAAN = MathUtils.normalizeAngle(osculating.getRightAscensionOfAscendingNode().subtract(
495 parameters[4].getValue()),
496 zero);
497 final T deltaAlphaM = MathUtils.normalizeAngle(osculating.getAlphaM().subtract(parameters[5].getValue()), zero);
498
499 current = new FieldEHModel<>(new FieldCircularOrbit<>(current.mean.getA().add(deltaA),
500 current.mean.getCircularEx().add( deltaEx),
501 current.mean.getCircularEy().add( deltaEy),
502 current.mean.getI() .add( deltaI ),
503 current.mean.getRightAscensionOfAscendingNode().add(deltaRAAN),
504 current.mean.getAlphaM().add(deltaAlphaM),
505 PositionAngle.MEAN,
506 current.mean.getFrame(),
507 current.mean.getDate(), mu),
508 mass, referenceRadius, mu, ck0);
509
510 if (FastMath.abs(deltaA.getReal()) < thresholdA.getReal() &&
511 FastMath.abs(deltaEx.getReal()) < thresholdE.getReal() &&
512 FastMath.abs(deltaEy.getReal()) < thresholdE.getReal() &&
513 FastMath.abs(deltaI.getReal()) < thresholdAngles.getReal() &&
514 FastMath.abs(deltaRAAN.getReal()) < thresholdAngles.getReal() &&
515 FastMath.abs(deltaAlphaM.getReal()) < thresholdAngles.getReal()) {
516 return current;
517 }
518
519 }
520
521 throw new OrekitException(OrekitMessages.UNABLE_TO_COMPUTE_ECKSTEIN_HECHLER_MEAN_PARAMETERS, i);
522
523 }
524
525
526 @Override
527 public FieldCartesianOrbit<T> propagateOrbit(final FieldAbsoluteDate<T> date, final T[] parameters) {
528
529
530 final FieldEHModel<T> current = models.get(date);
531 return new FieldCartesianOrbit<>(toCartesian(date, current.propagateParameters(date)),
532 current.mean.getFrame(), mu);
533 }
534
535
536 private static class FieldEHModel<T extends CalculusFieldElement<T>> {
537
538
539 private final FieldCircularOrbit<T> mean;
540
541
542 private final T mass;
543
544
545
546 private final T xnotDot;
547 private final T rdpom;
548 private final T rdpomp;
549 private final T eps1;
550 private final T eps2;
551 private final T xim;
552 private final T ommD;
553 private final T rdl;
554 private final T aMD;
555
556 private final T kh;
557 private final T kl;
558
559 private final T ax1;
560 private final T ay1;
561 private final T as1;
562 private final T ac2;
563 private final T axy3;
564 private final T as3;
565 private final T ac4;
566 private final T as5;
567 private final T ac6;
568
569 private final T ex1;
570 private final T exx2;
571 private final T exy2;
572 private final T ex3;
573 private final T ex4;
574
575 private final T ey1;
576 private final T eyx2;
577 private final T eyy2;
578 private final T ey3;
579 private final T ey4;
580
581 private final T rx1;
582 private final T ry1;
583 private final T r2;
584 private final T r3;
585 private final T rl;
586
587 private final T iy1;
588 private final T ix1;
589 private final T i2;
590 private final T i3;
591 private final T ih;
592
593 private final T lx1;
594 private final T ly1;
595 private final T l2;
596 private final T l3;
597 private final T ll;
598
599
600
601
602
603
604
605
606
607
608 FieldEHModel(final FieldCircularOrbit<T> mean, final T mass,
609 final double referenceRadius, final T mu, final double[] ck0) {
610
611 this.mean = mean;
612 this.mass = mass;
613 final T zero = mass.getField().getZero();
614 final T one = mass.getField().getOne();
615
616 T q = zero.add(referenceRadius).divide(mean.getA());
617 T ql = q.multiply(q);
618 final T g2 = ql.multiply(ck0[2]);
619 ql = ql.multiply(q);
620 final T g3 = ql.multiply(ck0[3]);
621 ql = ql.multiply(q);
622 final T g4 = ql.multiply(ck0[4]);
623 ql = ql.multiply(q);
624 final T g5 = ql.multiply(ck0[5]);
625 ql = ql.multiply(q);
626 final T g6 = ql.multiply(ck0[6]);
627
628 final FieldSinCos<T> sc = FastMath.sinCos(mean.getI());
629 final T cosI1 = sc.cos();
630 final T sinI1 = sc.sin();
631 final T sinI2 = sinI1.multiply(sinI1);
632 final T sinI4 = sinI2.multiply(sinI2);
633 final T sinI6 = sinI2.multiply(sinI4);
634
635 if (sinI2.getReal() < 1.0e-10) {
636 throw new OrekitException(OrekitMessages.ALMOST_EQUATORIAL_ORBIT,
637 FastMath.toDegrees(mean.getI().getReal()));
638 }
639
640 if (FastMath.abs(sinI2.getReal() - 4.0 / 5.0) < 1.0e-3) {
641 throw new OrekitException(OrekitMessages.ALMOST_CRITICALLY_INCLINED_ORBIT,
642 FastMath.toDegrees(mean.getI().getReal()));
643 }
644
645 if (mean.getE().getReal() > 0.1) {
646
647 throw new OrekitException(OrekitMessages.TOO_LARGE_ECCENTRICITY_FOR_PROPAGATION_MODEL,
648 mean.getE());
649 }
650
651 xnotDot = mu.divide(mean.getA()).sqrt().divide(mean.getA());
652
653 rdpom = g2.multiply(-0.75).multiply(sinI2.multiply(-5.0).add(4.0));
654 rdpomp = g4.multiply(7.5).multiply(sinI2.multiply(-31.0 / 8.0).add(1.0).add( sinI4.multiply(49.0 / 16.0))).subtract(
655 g6.multiply(13.125).multiply(one.subtract(sinI2.multiply(8.0)).add(sinI4.multiply(129.0 / 8.0)).subtract(sinI6.multiply(297.0 / 32.0)) ));
656
657
658 q = zero.add(3.0).divide(rdpom.multiply(32.0));
659 eps1 = q.multiply(g4).multiply(sinI2).multiply(sinI2.multiply(-35.0).add(30.0)).subtract(
660 q.multiply(175.0).multiply(g6).multiply(sinI2).multiply(sinI2.multiply(-3.0).add(sinI4.multiply(2.0625)).add(1.0)));
661 q = sinI1.multiply(3.0).divide(rdpom.multiply(8.0));
662 eps2 = q.multiply(g3).multiply(sinI2.multiply(-5.0).add(4.0)).subtract(q.multiply(g5).multiply(sinI2.multiply(-35.0).add(sinI4.multiply(26.25)).add(10.0)));
663
664 xim = mean.getI();
665 ommD = cosI1.multiply(g2.multiply(1.50).subtract(g2.multiply(2.25).multiply(g2).multiply(sinI2.multiply(-19.0 / 6.0).add(2.5))).add(
666 g4.multiply(0.9375).multiply(sinI2.multiply(7.0).subtract(4.0))).add(
667 g6.multiply(3.28125).multiply(sinI2.multiply(-9.0).add(2.0).add(sinI4.multiply(8.25)))));
668
669 rdl = g2.multiply(-1.50).multiply(sinI2.multiply(-4.0).add(3.0)).add(1.0);
670 aMD = rdl.add(
671 g2.multiply(2.25).multiply(g2.multiply(sinI2.multiply(-263.0 / 12.0 ).add(9.0).add(sinI4.multiply(341.0 / 24.0))))).add(
672 g4.multiply(15.0 / 16.0).multiply(sinI2.multiply(-31.0).add(8.0).add(sinI4.multiply(24.5)))).add(
673 g6.multiply(105.0 / 32.0).multiply(sinI2.multiply(25.0).add(-10.0 / 3.0).subtract(sinI4.multiply(48.75)).add(sinI6.multiply(27.5))));
674
675 final T qq = g2.divide(rdl).multiply(-1.5);
676 final T qA = g2.multiply(0.75).multiply(g2).multiply(sinI2);
677 final T qB = g4.multiply(0.25).multiply(sinI2);
678 final T qC = g6.multiply(105.0 / 16.0).multiply(sinI2);
679 final T qD = g3.multiply(-0.75).multiply(sinI1);
680 final T qE = g5.multiply(3.75).multiply(sinI1);
681 kh = zero.add(0.375).divide(rdpom);
682 kl = kh.divide(sinI1);
683
684 ax1 = qq.multiply(sinI2.multiply(-3.5).add(2.0));
685 ay1 = qq.multiply(sinI2.multiply(-2.5).add(2.0));
686 as1 = qD.multiply(sinI2.multiply(-5.0).add(4.0)).add(
687 qE.multiply(sinI4.multiply(2.625).add(sinI2.multiply(-3.5)).add(1.0)));
688 ac2 = qq.multiply(sinI2).add(
689 qA.multiply(7.0).multiply(sinI2.multiply(-3.0).add(2.0))).add(
690 qB.multiply(sinI2.multiply(-17.5).add(15.0))).add(
691 qC.multiply(sinI2.multiply(3.0).subtract(1.0).subtract(sinI4.multiply(33.0 / 16.0))));
692 axy3 = qq.multiply(3.5).multiply(sinI2);
693 as3 = qD.multiply(5.0 / 3.0).multiply(sinI2).add(
694 qE.multiply(7.0 / 6.0).multiply(sinI2).multiply(sinI2.multiply(-1.125).add(1)));
695 ac4 = qA.multiply(sinI2).add(
696 qB.multiply(4.375).multiply(sinI2)).add(
697 qC.multiply(0.75).multiply(sinI4.multiply(1.1).subtract(sinI2)));
698
699 as5 = qE.multiply(21.0 / 80.0).multiply(sinI4);
700
701 ac6 = qC.multiply(-11.0 / 80.0).multiply(sinI4);
702
703 ex1 = qq.multiply(sinI2.multiply(-1.25).add(1.0));
704 exx2 = qq.multiply(0.5).multiply(sinI2.multiply(-5.0).add(3.0));
705 exy2 = qq.multiply(sinI2.multiply(-1.5).add(2.0));
706 ex3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
707 ex4 = qq.multiply(17.0 / 8.0).multiply(sinI2);
708
709 ey1 = qq.multiply(sinI2.multiply(-1.75).add(1.0));
710 eyx2 = qq.multiply(sinI2.multiply(-3.0).add(1.0));
711 eyy2 = qq.multiply(sinI2.multiply(2.0).subtract(1.5));
712 ey3 = qq.multiply(7.0 / 12.0).multiply(sinI2);
713 ey4 = qq.multiply(17.0 / 8.0).multiply(sinI2);
714
715 q = cosI1.multiply(qq).negate();
716 rx1 = q.multiply(3.5);
717 ry1 = q.multiply(-2.5);
718 r2 = q.multiply(-0.5);
719 r3 = q.multiply(7.0 / 6.0);
720 rl = g3 .multiply( cosI1).multiply(sinI2.multiply(-15.0).add(4.0)).subtract(
721 g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-42.0).add(4.0).add(sinI4.multiply(52.5))));
722
723 q = qq.multiply(0.5).multiply(sinI1).multiply(cosI1);
724 iy1 = q;
725 ix1 = q.negate();
726 i2 = q;
727 i3 = q.multiply(7.0 / 3.0);
728 ih = g3.negate().multiply(cosI1).multiply(sinI2.multiply(-5.0).add(4)).add(
729 g5.multiply(2.5).multiply(cosI1).multiply(sinI2.multiply(-14.0).add(4.0).add(sinI4.multiply(10.5))));
730 lx1 = qq.multiply(sinI2.multiply(-77.0 / 8.0).add(7.0));
731 ly1 = qq.multiply(sinI2.multiply(55.0 / 8.0).subtract(7.50));
732 l2 = qq.multiply(sinI2.multiply(1.25).subtract(0.5));
733 l3 = qq.multiply(sinI2.multiply(77.0 / 24.0).subtract(7.0 / 6.0));
734 ll = g3.multiply(sinI2.multiply(53.0).subtract(4.0).add(sinI4.multiply(-57.5))).add(
735 g5.multiply(2.5).multiply(sinI2.multiply(-96.0).add(4.0).add(sinI4.multiply(269.5).subtract(sinI6.multiply(183.75)))));
736
737 }
738
739
740
741
742
743 public FieldUnivariateDerivative2<T>[] propagateParameters(final FieldAbsoluteDate<T> date) {
744 final Field<T> field = date.durationFrom(mean.getDate()).getField();
745 final T one = field.getOne();
746 final T zero = field.getZero();
747
748 final FieldUnivariateDerivative2<T> dt = new FieldUnivariateDerivative2<>(date.durationFrom(mean.getDate()), one, zero);
749 final FieldUnivariateDerivative2<T> xnot = dt.multiply(xnotDot);
750
751
752
753
754 final FieldUnivariateDerivative2<T> x = xnot.multiply(rdpom.add(rdpomp));
755 final FieldUnivariateDerivative2<T> cx = x.cos();
756 final FieldUnivariateDerivative2<T> sx = x.sin();
757 final FieldUnivariateDerivative2<T> exm = cx.multiply(mean.getCircularEx()).
758 add(sx.multiply(eps2.subtract(one.subtract(eps1).multiply(mean.getCircularEy()))));
759 final FieldUnivariateDerivative2<T> eym = sx.multiply(eps1.add(1.0).multiply(mean.getCircularEx())).
760 add(cx.multiply(mean.getCircularEy().subtract(eps2))).
761 add(eps2);
762
763
764
765 final FieldUnivariateDerivative2<T> omm =
766 new FieldUnivariateDerivative2<>(MathUtils.normalizeAngle(mean.getRightAscensionOfAscendingNode().add(ommD.multiply(xnot.getValue())),
767 one.getPi()),
768 ommD.multiply(xnotDot),
769 zero);
770
771 final FieldUnivariateDerivative2<T> xlm =
772 new FieldUnivariateDerivative2<>(MathUtils.normalizeAngle(mean.getAlphaM().add(aMD.multiply(xnot.getValue())),
773 one.getPi()),
774 aMD.multiply(xnotDot),
775 zero);
776
777
778 final FieldUnivariateDerivative2<T> cl1 = xlm.cos();
779 final FieldUnivariateDerivative2<T> sl1 = xlm.sin();
780 final FieldUnivariateDerivative2<T> cl2 = cl1.multiply(cl1).subtract(sl1.multiply(sl1));
781 final FieldUnivariateDerivative2<T> sl2 = cl1.multiply(sl1).add(sl1.multiply(cl1));
782 final FieldUnivariateDerivative2<T> cl3 = cl2.multiply(cl1).subtract(sl2.multiply(sl1));
783 final FieldUnivariateDerivative2<T> sl3 = cl2.multiply(sl1).add(sl2.multiply(cl1));
784 final FieldUnivariateDerivative2<T> cl4 = cl3.multiply(cl1).subtract(sl3.multiply(sl1));
785 final FieldUnivariateDerivative2<T> sl4 = cl3.multiply(sl1).add(sl3.multiply(cl1));
786 final FieldUnivariateDerivative2<T> cl5 = cl4.multiply(cl1).subtract(sl4.multiply(sl1));
787 final FieldUnivariateDerivative2<T> sl5 = cl4.multiply(sl1).add(sl4.multiply(cl1));
788 final FieldUnivariateDerivative2<T> cl6 = cl5.multiply(cl1).subtract(sl5.multiply(sl1));
789
790 final FieldUnivariateDerivative2<T> qh = eym.subtract(eps2).multiply(kh);
791 final FieldUnivariateDerivative2<T> ql = exm.multiply(kl);
792
793 final FieldUnivariateDerivative2<T> exmCl1 = exm.multiply(cl1);
794 final FieldUnivariateDerivative2<T> exmSl1 = exm.multiply(sl1);
795 final FieldUnivariateDerivative2<T> eymCl1 = eym.multiply(cl1);
796 final FieldUnivariateDerivative2<T> eymSl1 = eym.multiply(sl1);
797 final FieldUnivariateDerivative2<T> exmCl2 = exm.multiply(cl2);
798 final FieldUnivariateDerivative2<T> exmSl2 = exm.multiply(sl2);
799 final FieldUnivariateDerivative2<T> eymCl2 = eym.multiply(cl2);
800 final FieldUnivariateDerivative2<T> eymSl2 = eym.multiply(sl2);
801 final FieldUnivariateDerivative2<T> exmCl3 = exm.multiply(cl3);
802 final FieldUnivariateDerivative2<T> exmSl3 = exm.multiply(sl3);
803 final FieldUnivariateDerivative2<T> eymCl3 = eym.multiply(cl3);
804 final FieldUnivariateDerivative2<T> eymSl3 = eym.multiply(sl3);
805 final FieldUnivariateDerivative2<T> exmCl4 = exm.multiply(cl4);
806 final FieldUnivariateDerivative2<T> exmSl4 = exm.multiply(sl4);
807 final FieldUnivariateDerivative2<T> eymCl4 = eym.multiply(cl4);
808 final FieldUnivariateDerivative2<T> eymSl4 = eym.multiply(sl4);
809
810
811 final FieldUnivariateDerivative2<T> rda = exmCl1.multiply(ax1).
812 add(eymSl1.multiply(ay1)).
813 add(sl1.multiply(as1)).
814 add(cl2.multiply(ac2)).
815 add(exmCl3.add(eymSl3).multiply(axy3)).
816 add(sl3.multiply(as3)).
817 add(cl4.multiply(ac4)).
818 add(sl5.multiply(as5)).
819 add(cl6.multiply(ac6));
820
821
822 final FieldUnivariateDerivative2<T> rdex = cl1.multiply(ex1).
823 add(exmCl2.multiply(exx2)).
824 add(eymSl2.multiply(exy2)).
825 add(cl3.multiply(ex3)).
826 add(exmCl4.add(eymSl4).multiply(ex4));
827 final FieldUnivariateDerivative2<T> rdey = sl1.multiply(ey1).
828 add(exmSl2.multiply(eyx2)).
829 add(eymCl2.multiply(eyy2)).
830 add(sl3.multiply(ey3)).
831 add(exmSl4.subtract(eymCl4).multiply(ey4));
832
833
834 final FieldUnivariateDerivative2<T> rdom = exmSl1.multiply(rx1).
835 add(eymCl1.multiply(ry1)).
836 add(sl2.multiply(r2)).
837 add(eymCl3.subtract(exmSl3).multiply(r3)).
838 add(ql.multiply(rl));
839
840
841 final FieldUnivariateDerivative2<T> rdxi = eymSl1.multiply(iy1).
842 add(exmCl1.multiply(ix1)).
843 add(cl2.multiply(i2)).
844 add(exmCl3.add(eymSl3).multiply(i3)).
845 add(qh.multiply(ih));
846
847
848 final FieldUnivariateDerivative2<T> rdxl = exmSl1.multiply(lx1).
849 add(eymCl1.multiply(ly1)).
850 add(sl2.multiply(l2)).
851 add(exmSl3.subtract(eymCl3).multiply(l3)).
852 add(ql.multiply(ll));
853
854 final FieldUnivariateDerivative2<T>[] FTD = MathArrays.buildArray(rdxl.getField(), 6);
855
856 FTD[0] = rda.add(1.0).multiply(mean.getA());
857 FTD[1] = rdex.add(exm);
858 FTD[2] = rdey.add(eym);
859 FTD[3] = rdxi.add(xim);
860 FTD[4] = rdom.add(omm);
861 FTD[5] = rdxl.add(xlm);
862 return FTD;
863
864 }
865
866 }
867
868
869
870
871
872
873 private TimeStampedFieldPVCoordinates<T> toCartesian(final FieldAbsoluteDate<T> date, final FieldUnivariateDerivative2<T>[] parameters) {
874
875
876 final FieldUnivariateDerivative2<T> cosOmega = parameters[4].cos();
877 final FieldUnivariateDerivative2<T> sinOmega = parameters[4].sin();
878 final FieldUnivariateDerivative2<T> cosI = parameters[3].cos();
879 final FieldUnivariateDerivative2<T> sinI = parameters[3].sin();
880 final FieldUnivariateDerivative2<T> alphaE = meanToEccentric(parameters[5], parameters[1], parameters[2]);
881 final FieldUnivariateDerivative2<T> cosAE = alphaE.cos();
882 final FieldUnivariateDerivative2<T> sinAE = alphaE.sin();
883 final FieldUnivariateDerivative2<T> ex2 = parameters[1].multiply(parameters[1]);
884 final FieldUnivariateDerivative2<T> ey2 = parameters[2].multiply(parameters[2]);
885 final FieldUnivariateDerivative2<T> exy = parameters[1].multiply(parameters[2]);
886 final FieldUnivariateDerivative2<T> q = ex2.add(ey2).subtract(1).negate().sqrt();
887 final FieldUnivariateDerivative2<T> beta = q.add(1).reciprocal();
888 final FieldUnivariateDerivative2<T> bx2 = beta.multiply(ex2);
889 final FieldUnivariateDerivative2<T> by2 = beta.multiply(ey2);
890 final FieldUnivariateDerivative2<T> bxy = beta.multiply(exy);
891 final FieldUnivariateDerivative2<T> u = bxy.multiply(sinAE).subtract(parameters[1].add(by2.subtract(1).multiply(cosAE)));
892 final FieldUnivariateDerivative2<T> v = bxy.multiply(cosAE).subtract(parameters[2].add(bx2.subtract(1).multiply(sinAE)));
893 final FieldUnivariateDerivative2<T> x = parameters[0].multiply(u);
894 final FieldUnivariateDerivative2<T> y = parameters[0].multiply(v);
895
896
897 final FieldVector3D<FieldUnivariateDerivative2<T>> p =
898 new FieldVector3D<>(x.multiply(cosOmega).subtract(y.multiply(cosI.multiply(sinOmega))),
899 x.multiply(sinOmega).add(y.multiply(cosI.multiply(cosOmega))),
900 y.multiply(sinI));
901
902
903 final FieldVector3D<T> p0 = new FieldVector3D<>(p.getX().getValue(),
904 p.getY().getValue(),
905 p.getZ().getValue());
906 final FieldVector3D<T> p1 = new FieldVector3D<>(p.getX().getFirstDerivative(),
907 p.getY().getFirstDerivative(),
908 p.getZ().getFirstDerivative());
909 final FieldVector3D<T> p2 = new FieldVector3D<>(p.getX().getSecondDerivative(),
910 p.getY().getSecondDerivative(),
911 p.getZ().getSecondDerivative());
912 return new TimeStampedFieldPVCoordinates<>(date, p0, p1, p2);
913
914 }
915
916
917
918
919
920
921
922 private FieldUnivariateDerivative2<T> meanToEccentric(final FieldUnivariateDerivative2<T> alphaM,
923 final FieldUnivariateDerivative2<T> ex,
924 final FieldUnivariateDerivative2<T> ey) {
925
926
927
928 FieldUnivariateDerivative2<T> alphaE = alphaM;
929 FieldUnivariateDerivative2<T> shift = alphaM.getField().getZero();
930 FieldUnivariateDerivative2<T> alphaEMalphaM = alphaM.getField().getZero();
931 FieldUnivariateDerivative2<T> cosAlphaE = alphaE.cos();
932 FieldUnivariateDerivative2<T> sinAlphaE = alphaE.sin();
933 int iter = 0;
934 do {
935 final FieldUnivariateDerivative2<T> f2 = ex.multiply(sinAlphaE).subtract(ey.multiply(cosAlphaE));
936 final FieldUnivariateDerivative2<T> f1 = alphaM.getField().getOne().subtract(ex.multiply(cosAlphaE)).subtract(ey.multiply(sinAlphaE));
937 final FieldUnivariateDerivative2<T> f0 = alphaEMalphaM.subtract(f2);
938
939 final FieldUnivariateDerivative2<T> f12 = f1.multiply(2);
940 shift = f0.multiply(f12).divide(f1.multiply(f12).subtract(f0.multiply(f2)));
941
942 alphaEMalphaM = alphaEMalphaM.subtract(shift);
943 alphaE = alphaM.add(alphaEMalphaM);
944 cosAlphaE = alphaE.cos();
945 sinAlphaE = alphaE.sin();
946
947 } while (++iter < 50 && FastMath.abs(shift.getValue().getReal()) > 1.0e-12);
948
949 return alphaE;
950
951 }
952
953
954 @Override
955 protected T getMass(final FieldAbsoluteDate<T> date) {
956 return models.get(date).mass;
957 }
958
959
960 @Override
961 protected List<ParameterDriver> getParametersDrivers() {
962
963 return Collections.emptyList();
964 }
965
966 }