1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import java.io.Serializable;
20 import java.util.List;
21 import java.util.stream.Collectors;
22 import java.util.stream.Stream;
23
24 import org.hipparchus.analysis.differentiation.DSFactory;
25 import org.hipparchus.analysis.differentiation.DerivativeStructure;
26 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.hipparchus.util.FastMath;
29 import org.hipparchus.util.MathUtils;
30 import org.orekit.errors.OrekitIllegalArgumentException;
31 import org.orekit.errors.OrekitInternalError;
32 import org.orekit.errors.OrekitMessages;
33 import org.orekit.frames.Frame;
34 import org.orekit.time.AbsoluteDate;
35 import org.orekit.utils.PVCoordinates;
36 import org.orekit.utils.TimeStampedPVCoordinates;
37
38
39
40
41
42
43
44
45
46
47
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
76
77
78 public class CircularOrbit
79 extends Orbit {
80
81
82 private static final long serialVersionUID = 20170414L;
83
84
85 private static final DSFactory FACTORY = new DSFactory(1, 1);
86
87
88 private final double a;
89
90
91 private final double ex;
92
93
94 private final double ey;
95
96
97 private final double i;
98
99
100 private final double raan;
101
102
103 private final double alphaV;
104
105
106 private final double aDot;
107
108
109 private final double exDot;
110
111
112 private final double eyDot;
113
114
115 private final double iDot;
116
117
118 private final double raanDot;
119
120
121 private final double alphaVDot;
122
123
124 private final boolean serializePV;
125
126
127 private transient PVCoordinates partialPV;
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144 public CircularOrbit(final double a, final double ex, final double ey,
145 final double i, final double raan, final double alpha,
146 final PositionAngle type,
147 final Frame frame, final AbsoluteDate date, final double mu)
148 throws IllegalArgumentException {
149 this(a, ex, ey, i, raan, alpha,
150 Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN,
151 type, frame, date, mu);
152 }
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175 public CircularOrbit(final double a, final double ex, final double ey,
176 final double i, final double raan, final double alpha,
177 final double aDot, final double exDot, final double eyDot,
178 final double iDot, final double raanDot, final double alphaDot,
179 final PositionAngle type,
180 final Frame frame, final AbsoluteDate date, final double mu)
181 throws IllegalArgumentException {
182 super(frame, date, mu);
183 if (ex * ex + ey * ey >= 1.0) {
184 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
185 getClass().getName());
186 }
187 this.a = a;
188 this.aDot = aDot;
189 this.ex = ex;
190 this.exDot = exDot;
191 this.ey = ey;
192 this.eyDot = eyDot;
193 this.i = i;
194 this.iDot = iDot;
195 this.raan = raan;
196 this.raanDot = raanDot;
197
198 if (hasDerivatives()) {
199 final DerivativeStructure exDS = FACTORY.build(ex, exDot);
200 final DerivativeStructure eyDS = FACTORY.build(ey, eyDot);
201 final DerivativeStructure alphaDS = FACTORY.build(alpha, alphaDot);
202 final DerivativeStructure alphavDS;
203 switch (type) {
204 case MEAN :
205 alphavDS = FieldCircularOrbit.eccentricToTrue(FieldCircularOrbit.meanToEccentric(alphaDS, exDS, eyDS), exDS, eyDS);
206 break;
207 case ECCENTRIC :
208 alphavDS = FieldCircularOrbit.eccentricToTrue(alphaDS, exDS, eyDS);
209 break;
210 case TRUE :
211 alphavDS = alphaDS;
212 break;
213 default :
214 throw new OrekitInternalError(null);
215 }
216 this.alphaV = alphavDS.getValue();
217 this.alphaVDot = alphavDS.getPartialDerivative(1);
218 } else {
219 switch (type) {
220 case MEAN :
221 this.alphaV = eccentricToTrue(meanToEccentric(alpha, ex, ey), ex, ey);
222 break;
223 case ECCENTRIC :
224 this.alphaV = eccentricToTrue(alpha, ex, ey);
225 break;
226 case TRUE :
227 this.alphaV = alpha;
228 break;
229 default :
230 throw new OrekitInternalError(null);
231 }
232 this.alphaVDot = Double.NaN;
233 }
234
235 serializePV = false;
236 partialPV = null;
237
238 }
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260 private CircularOrbit(final double a, final double ex, final double ey,
261 final double i, final double raan, final double alphaV,
262 final double aDot, final double exDot, final double eyDot,
263 final double iDot, final double raanDot, final double alphaVDot,
264 final TimeStampedPVCoordinates pvCoordinates, final Frame frame,
265 final double mu)
266 throws IllegalArgumentException {
267 super(pvCoordinates, frame, mu);
268 this.a = a;
269 this.aDot = aDot;
270 this.ex = ex;
271 this.exDot = exDot;
272 this.ey = ey;
273 this.eyDot = eyDot;
274 this.i = i;
275 this.iDot = iDot;
276 this.raan = raan;
277 this.raanDot = raanDot;
278 this.alphaV = alphaV;
279 this.alphaVDot = alphaVDot;
280 this.serializePV = true;
281 this.partialPV = null;
282 }
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298 public CircularOrbit(final TimeStampedPVCoordinates pvCoordinates, final Frame frame, final double mu)
299 throws IllegalArgumentException {
300 super(pvCoordinates, frame, mu);
301
302
303 final Vector3D pvP = pvCoordinates.getPosition();
304 final Vector3D pvV = pvCoordinates.getVelocity();
305 final Vector3D pvA = pvCoordinates.getAcceleration();
306 final double r2 = pvP.getNormSq();
307 final double r = FastMath.sqrt(r2);
308 final double V2 = pvV.getNormSq();
309 final double rV2OnMu = r * V2 / mu;
310
311 if (rV2OnMu > 2) {
312 throw new OrekitIllegalArgumentException(OrekitMessages.HYPERBOLIC_ORBIT_NOT_HANDLED_AS,
313 getClass().getName());
314 }
315
316 a = r / (2 - rV2OnMu);
317
318
319 final Vector3D momentum = pvCoordinates.getMomentum();
320 i = Vector3D.angle(momentum, Vector3D.PLUS_K);
321
322
323 final Vector3D node = Vector3D.crossProduct(Vector3D.PLUS_K, momentum);
324 raan = FastMath.atan2(node.getY(), node.getX());
325
326
327 final double cosRaan = FastMath.cos(raan);
328 final double sinRaan = FastMath.sin(raan);
329 final double cosI = FastMath.cos(i);
330 final double sinI = FastMath.sin(i);
331 final double xP = pvP.getX();
332 final double yP = pvP.getY();
333 final double zP = pvP.getZ();
334 final double x2 = (xP * cosRaan + yP * sinRaan) / a;
335 final double y2 = ((yP * cosRaan - xP * sinRaan) * cosI + zP * sinI) / a;
336
337
338 final double eSE = Vector3D.dotProduct(pvP, pvV) / FastMath.sqrt(mu * a);
339 final double eCE = rV2OnMu - 1;
340 final double e2 = eCE * eCE + eSE * eSE;
341 final double f = eCE - e2;
342 final double g = FastMath.sqrt(1 - e2) * eSE;
343 final double aOnR = a / r;
344 final double a2OnR2 = aOnR * aOnR;
345 ex = a2OnR2 * (f * x2 + g * y2);
346 ey = a2OnR2 * (f * y2 - g * x2);
347
348
349 final double beta = 1 / (1 + FastMath.sqrt(1 - ex * ex - ey * ey));
350 alphaV = eccentricToTrue(FastMath.atan2(y2 + ey + eSE * beta * ex, x2 + ex - eSE * beta * ey), ex, ey);
351
352 partialPV = pvCoordinates;
353
354 if (hasNonKeplerianAcceleration(pvCoordinates, mu)) {
355
356
357 final double[][] jacobian = new double[6][6];
358 getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);
359
360 final Vector3D keplerianAcceleration = new Vector3D(-mu / (r * r2), pvP);
361 final Vector3D nonKeplerianAcceleration = pvA.subtract(keplerianAcceleration);
362 final double aX = nonKeplerianAcceleration.getX();
363 final double aY = nonKeplerianAcceleration.getY();
364 final double aZ = nonKeplerianAcceleration.getZ();
365 aDot = jacobian[0][3] * aX + jacobian[0][4] * aY + jacobian[0][5] * aZ;
366 exDot = jacobian[1][3] * aX + jacobian[1][4] * aY + jacobian[1][5] * aZ;
367 eyDot = jacobian[2][3] * aX + jacobian[2][4] * aY + jacobian[2][5] * aZ;
368 iDot = jacobian[3][3] * aX + jacobian[3][4] * aY + jacobian[3][5] * aZ;
369 raanDot = jacobian[4][3] * aX + jacobian[4][4] * aY + jacobian[4][5] * aZ;
370
371
372
373 final double alphaMDot = getKeplerianMeanMotion() +
374 jacobian[5][3] * aX + jacobian[5][4] * aY + jacobian[5][5] * aZ;
375 final DerivativeStructure exDS = FACTORY.build(ex, exDot);
376 final DerivativeStructure eyDS = FACTORY.build(ey, eyDot);
377 final DerivativeStructure alphaMDS = FACTORY.build(getAlphaM(), alphaMDot);
378 final DerivativeStructure alphavDS = FieldCircularOrbit.eccentricToTrue(FieldCircularOrbit.meanToEccentric(alphaMDS, exDS, eyDS), exDS, eyDS);
379 alphaVDot = alphavDS.getPartialDerivative(1);
380
381 } else {
382
383
384
385 aDot = Double.NaN;
386 exDot = Double.NaN;
387 eyDot = Double.NaN;
388 iDot = Double.NaN;
389 raanDot = Double.NaN;
390 alphaVDot = Double.NaN;
391 }
392
393 serializePV = true;
394
395 }
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412 public CircularOrbit(final PVCoordinates pvCoordinates, final Frame frame,
413 final AbsoluteDate date, final double mu)
414 throws IllegalArgumentException {
415 this(new TimeStampedPVCoordinates(date, pvCoordinates), frame, mu);
416 }
417
418
419
420
421 public CircularOrbit(final Orbit op) {
422
423 super(op.getFrame(), op.getDate(), op.getMu());
424
425 a = op.getA();
426 i = op.getI();
427 final double hx = op.getHx();
428 final double hy = op.getHy();
429 final double h2 = hx * hx + hy * hy;
430 final double h = FastMath.sqrt(h2);
431 raan = FastMath.atan2(hy, hx);
432 final double cosRaan = h == 0 ? FastMath.cos(raan) : hx / h;
433 final double sinRaan = h == 0 ? FastMath.sin(raan) : hy / h;
434 final double equiEx = op.getEquinoctialEx();
435 final double equiEy = op.getEquinoctialEy();
436 ex = equiEx * cosRaan + equiEy * sinRaan;
437 ey = equiEy * cosRaan - equiEx * sinRaan;
438 alphaV = op.getLv() - raan;
439
440 if (op.hasDerivatives()) {
441 aDot = op.getADot();
442 final double hxDot = op.getHxDot();
443 final double hyDot = op.getHyDot();
444 iDot = 2 * (cosRaan * hxDot + sinRaan * hyDot) / (1 + h2);
445 raanDot = (hx * hyDot - hy * hxDot) / h2;
446 final double equiExDot = op.getEquinoctialExDot();
447 final double equiEyDot = op.getEquinoctialEyDot();
448 exDot = (equiExDot + equiEy * raanDot) * cosRaan +
449 (equiEyDot - equiEx * raanDot) * sinRaan;
450 eyDot = (equiEyDot - equiEx * raanDot) * cosRaan -
451 (equiExDot + equiEy * raanDot) * sinRaan;
452 alphaVDot = op.getLvDot() - raanDot;
453 } else {
454 aDot = Double.NaN;
455 exDot = Double.NaN;
456 eyDot = Double.NaN;
457 iDot = Double.NaN;
458 raanDot = Double.NaN;
459 alphaVDot = Double.NaN;
460 }
461
462 serializePV = false;
463 partialPV = null;
464
465 }
466
467
468 public OrbitType getType() {
469 return OrbitType.CIRCULAR;
470 }
471
472
473 public double getA() {
474 return a;
475 }
476
477
478 public double getADot() {
479 return aDot;
480 }
481
482
483 public double getEquinoctialEx() {
484 final double cosRaan = FastMath.cos(raan);
485 final double sinRaan = FastMath.sin(raan);
486 return ex * cosRaan - ey * sinRaan;
487 }
488
489
490 public double getEquinoctialExDot() {
491 final double cosRaan = FastMath.cos(raan);
492 final double sinRaan = FastMath.sin(raan);
493 return (exDot - ey * raanDot) * cosRaan - (eyDot + ex * raanDot) * sinRaan;
494 }
495
496
497 public double getEquinoctialEy() {
498 final double cosRaan = FastMath.cos(raan);
499 final double sinRaan = FastMath.sin(raan);
500 return ey * cosRaan + ex * sinRaan;
501 }
502
503
504 public double getEquinoctialEyDot() {
505 final double cosRaan = FastMath.cos(raan);
506 final double sinRaan = FastMath.sin(raan);
507 return (eyDot + ex * raanDot) * cosRaan + (exDot - ey * raanDot) * sinRaan;
508 }
509
510
511
512
513 public double getCircularEx() {
514 return ex;
515 }
516
517
518
519
520
521 public double getCircularExDot() {
522 return exDot;
523 }
524
525
526
527
528 public double getCircularEy() {
529 return ey;
530 }
531
532
533
534
535 public double getCircularEyDot() {
536 return eyDot;
537 }
538
539
540 public double getHx() {
541
542 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
543 return Double.NaN;
544 }
545 return FastMath.cos(raan) * FastMath.tan(i / 2);
546 }
547
548
549 public double getHxDot() {
550
551 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
552 return Double.NaN;
553 }
554 final double cosRaan = FastMath.cos(raan);
555 final double sinRaan = FastMath.sin(raan);
556 final double tan = FastMath.tan(0.5 * i);
557 return 0.5 * cosRaan * (1 + tan * tan) * iDot - sinRaan * tan * raanDot;
558 }
559
560
561 public double getHy() {
562
563 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
564 return Double.NaN;
565 }
566 return FastMath.sin(raan) * FastMath.tan(i / 2);
567 }
568
569
570 public double getHyDot() {
571
572 if (FastMath.abs(i - FastMath.PI) < 1.0e-10) {
573 return Double.NaN;
574 }
575 final double cosRaan = FastMath.cos(raan);
576 final double sinRaan = FastMath.sin(raan);
577 final double tan = FastMath.tan(0.5 * i);
578 return 0.5 * sinRaan * (1 + tan * tan) * iDot + cosRaan * tan * raanDot;
579 }
580
581
582
583
584 public double getAlphaV() {
585 return alphaV;
586 }
587
588
589
590
591
592
593
594
595 public double getAlphaVDot() {
596 return alphaVDot;
597 }
598
599
600
601
602 public double getAlphaE() {
603 return trueToEccentric(alphaV, ex, ey);
604 }
605
606
607
608
609
610
611
612
613 public double getAlphaEDot() {
614 final DerivativeStructure alphaVDS = FACTORY.build(alphaV, alphaVDot);
615 final DerivativeStructure exDS = FACTORY.build(ex, exDot);
616 final DerivativeStructure eyDS = FACTORY.build(ey, eyDot);
617 final DerivativeStructure alphaEDS = FieldCircularOrbit.trueToEccentric(alphaVDS, exDS, eyDS);
618 return alphaEDS.getPartialDerivative(1);
619 }
620
621
622
623
624 public double getAlphaM() {
625 return eccentricToMean(trueToEccentric(alphaV, ex, ey), ex, ey);
626 }
627
628
629
630
631
632
633
634
635 public double getAlphaMDot() {
636 final DerivativeStructure alphaVDS = FACTORY.build(alphaV, alphaVDot);
637 final DerivativeStructure exDS = FACTORY.build(ex, exDot);
638 final DerivativeStructure eyDS = FACTORY.build(ey, eyDot);
639 final DerivativeStructure alphaMDS = FieldCircularOrbit.eccentricToMean(FieldCircularOrbit.trueToEccentric(alphaVDS, exDS, eyDS), exDS, eyDS);
640 return alphaMDS.getPartialDerivative(1);
641 }
642
643
644
645
646
647 public double getAlpha(final PositionAngle type) {
648 return (type == PositionAngle.MEAN) ? getAlphaM() :
649 ((type == PositionAngle.ECCENTRIC) ? getAlphaE() :
650 getAlphaV());
651 }
652
653
654
655
656
657
658
659
660
661 public double getAlphaDot(final PositionAngle type) {
662 return (type == PositionAngle.MEAN) ? getAlphaMDot() :
663 ((type == PositionAngle.ECCENTRIC) ? getAlphaEDot() :
664 getAlphaVDot());
665 }
666
667
668
669
670
671
672
673 public static double eccentricToTrue(final double alphaE, final double ex, final double ey) {
674 final double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
675 final double cosAlphaE = FastMath.cos(alphaE);
676 final double sinAlphaE = FastMath.sin(alphaE);
677 return alphaE + 2 * FastMath.atan((ex * sinAlphaE - ey * cosAlphaE) /
678 (epsilon + 1 - ex * cosAlphaE - ey * sinAlphaE));
679 }
680
681
682
683
684
685
686
687 public static double trueToEccentric(final double alphaV, final double ex, final double ey) {
688 final double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
689 final double cosAlphaV = FastMath.cos(alphaV);
690 final double sinAlphaV = FastMath.sin(alphaV);
691 return alphaV + 2 * FastMath.atan((ey * cosAlphaV - ex * sinAlphaV) /
692 (epsilon + 1 + ex * cosAlphaV + ey * sinAlphaV));
693 }
694
695
696
697
698
699
700
701 public static double meanToEccentric(final double alphaM, final double ex, final double ey) {
702
703
704
705 double alphaE = alphaM;
706 double shift = 0.0;
707 double alphaEMalphaM = 0.0;
708 double cosAlphaE = FastMath.cos(alphaE);
709 double sinAlphaE = FastMath.sin(alphaE);
710 int iter = 0;
711 do {
712 final double f2 = ex * sinAlphaE - ey * cosAlphaE;
713 final double f1 = 1.0 - ex * cosAlphaE - ey * sinAlphaE;
714 final double f0 = alphaEMalphaM - f2;
715
716 final double f12 = 2.0 * f1;
717 shift = f0 * f12 / (f1 * f12 - f0 * f2);
718
719 alphaEMalphaM -= shift;
720 alphaE = alphaM + alphaEMalphaM;
721 cosAlphaE = FastMath.cos(alphaE);
722 sinAlphaE = FastMath.sin(alphaE);
723
724 } while ((++iter < 50) && (FastMath.abs(shift) > 1.0e-12));
725
726 return alphaE;
727
728 }
729
730
731
732
733
734
735
736 public static double eccentricToMean(final double alphaE, final double ex, final double ey) {
737 return alphaE + (ey * FastMath.cos(alphaE) - ex * FastMath.sin(alphaE));
738 }
739
740
741 public double getE() {
742 return FastMath.sqrt(ex * ex + ey * ey);
743 }
744
745
746 public double getEDot() {
747 return (ex * exDot + ey * eyDot) / getE();
748 }
749
750
751 public double getI() {
752 return i;
753 }
754
755
756 public double getIDot() {
757 return iDot;
758 }
759
760
761
762
763 public double getRightAscensionOfAscendingNode() {
764 return raan;
765 }
766
767
768
769
770
771
772
773
774 public double getRightAscensionOfAscendingNodeDot() {
775 return raanDot;
776 }
777
778
779 public double getLv() {
780 return alphaV + raan;
781 }
782
783
784 public double getLvDot() {
785 return alphaVDot + raanDot;
786 }
787
788
789 public double getLE() {
790 return getAlphaE() + raan;
791 }
792
793
794 public double getLEDot() {
795 return getAlphaEDot() + raanDot;
796 }
797
798
799 public double getLM() {
800 return getAlphaM() + raan;
801 }
802
803
804 public double getLMDot() {
805 return getAlphaMDot() + raanDot;
806 }
807
808
809
810 private void computePVWithoutA() {
811
812 if (partialPV != null) {
813
814 return;
815 }
816
817
818 final double equEx = getEquinoctialEx();
819 final double equEy = getEquinoctialEy();
820 final double hx = getHx();
821 final double hy = getHy();
822 final double lE = getLE();
823
824
825 final double hx2 = hx * hx;
826 final double hy2 = hy * hy;
827 final double factH = 1. / (1 + hx2 + hy2);
828
829
830 final double ux = (1 + hx2 - hy2) * factH;
831 final double uy = 2 * hx * hy * factH;
832 final double uz = -2 * hy * factH;
833
834 final double vx = uy;
835 final double vy = (1 - hx2 + hy2) * factH;
836 final double vz = 2 * hx * factH;
837
838
839 final double exey = equEx * equEy;
840 final double ex2 = equEx * equEx;
841 final double ey2 = equEy * equEy;
842 final double e2 = ex2 + ey2;
843 final double eta = 1 + FastMath.sqrt(1 - e2);
844 final double beta = 1. / eta;
845
846
847 final double cLe = FastMath.cos(lE);
848 final double sLe = FastMath.sin(lE);
849 final double exCeyS = equEx * cLe + equEy * sLe;
850
851
852 final double x = a * ((1 - beta * ey2) * cLe + beta * exey * sLe - equEx);
853 final double y = a * ((1 - beta * ex2) * sLe + beta * exey * cLe - equEy);
854
855 final double factor = FastMath.sqrt(getMu() / a) / (1 - exCeyS);
856 final double xdot = factor * (-sLe + beta * equEy * exCeyS);
857 final double ydot = factor * ( cLe - beta * equEx * exCeyS);
858
859 final Vector3D position =
860 new Vector3D(x * ux + y * vx, x * uy + y * vy, x * uz + y * vz);
861 final Vector3D velocity =
862 new Vector3D(xdot * ux + ydot * vx, xdot * uy + ydot * vy, xdot * uz + ydot * vz);
863
864 partialPV = new PVCoordinates(position, velocity);
865
866 }
867
868
869
870
871
872
873
874 private Vector3D nonKeplerianAcceleration() {
875
876 final double[][] dCdP = new double[6][6];
877 getJacobianWrtParameters(PositionAngle.MEAN, dCdP);
878
879 final double nonKeplerianMeanMotion = getAlphaMDot() - getKeplerianMeanMotion();
880 final double nonKeplerianAx = dCdP[3][0] * aDot + dCdP[3][1] * exDot + dCdP[3][2] * eyDot +
881 dCdP[3][3] * iDot + dCdP[3][4] * raanDot + dCdP[3][5] * nonKeplerianMeanMotion;
882 final double nonKeplerianAy = dCdP[4][0] * aDot + dCdP[4][1] * exDot + dCdP[4][2] * eyDot +
883 dCdP[4][3] * iDot + dCdP[4][4] * raanDot + dCdP[4][5] * nonKeplerianMeanMotion;
884 final double nonKeplerianAz = dCdP[5][0] * aDot + dCdP[5][1] * exDot + dCdP[5][2] * eyDot +
885 dCdP[5][3] * iDot + dCdP[5][4] * raanDot + dCdP[5][5] * nonKeplerianMeanMotion;
886
887 return new Vector3D(nonKeplerianAx, nonKeplerianAy, nonKeplerianAz);
888
889 }
890
891
892 protected TimeStampedPVCoordinates initPVCoordinates() {
893
894
895 computePVWithoutA();
896
897
898 final double r2 = partialPV.getPosition().getNormSq();
899 final Vector3D keplerianAcceleration = new Vector3D(-getMu() / (r2 * FastMath.sqrt(r2)), partialPV.getPosition());
900 final Vector3D acceleration = hasDerivatives() ?
901 keplerianAcceleration.add(nonKeplerianAcceleration()) :
902 keplerianAcceleration;
903
904 return new TimeStampedPVCoordinates(getDate(), partialPV.getPosition(), partialPV.getVelocity(), acceleration);
905
906 }
907
908
909 public CircularOrbit shiftedBy(final double dt) {
910
911
912 final CircularOrbit keplerianShifted = new CircularOrbit(a, ex, ey, i, raan,
913 getAlphaM() + getKeplerianMeanMotion() * dt,
914 PositionAngle.MEAN, getFrame(),
915 getDate().shiftedBy(dt), getMu());
916
917 if (hasDerivatives()) {
918
919
920 final Vector3D nonKeplerianAcceleration = nonKeplerianAcceleration();
921
922
923 keplerianShifted.computePVWithoutA();
924 final Vector3D fixedP = new Vector3D(1, keplerianShifted.partialPV.getPosition(),
925 0.5 * dt * dt, nonKeplerianAcceleration);
926 final double fixedR2 = fixedP.getNormSq();
927 final double fixedR = FastMath.sqrt(fixedR2);
928 final Vector3D fixedV = new Vector3D(1, keplerianShifted.partialPV.getVelocity(),
929 dt, nonKeplerianAcceleration);
930 final Vector3D fixedA = new Vector3D(-getMu() / (fixedR2 * fixedR), keplerianShifted.partialPV.getPosition(),
931 1, nonKeplerianAcceleration);
932
933
934 return new CircularOrbit(new TimeStampedPVCoordinates(keplerianShifted.getDate(),
935 fixedP, fixedV, fixedA),
936 keplerianShifted.getFrame(), keplerianShifted.getMu());
937
938 } else {
939
940 return keplerianShifted;
941 }
942
943 }
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965 public CircularOrbit interpolate(final AbsoluteDate date, final Stream<Orbit> sample) {
966
967
968 final List<Orbit> list = sample.collect(Collectors.toList());
969 boolean useDerivatives = true;
970 for (final Orbit orbit : list) {
971 useDerivatives = useDerivatives && orbit.hasDerivatives();
972 }
973
974
975 final HermiteInterpolator interpolator = new HermiteInterpolator();
976
977
978 AbsoluteDate previousDate = null;
979 double previousRAAN = Double.NaN;
980 double previousAlphaM = Double.NaN;
981 for (final Orbit orbit : list) {
982 final CircularOrbit circ = (CircularOrbit) OrbitType.CIRCULAR.convertType(orbit);
983 final double continuousRAAN;
984 final double continuousAlphaM;
985 if (previousDate == null) {
986 continuousRAAN = circ.getRightAscensionOfAscendingNode();
987 continuousAlphaM = circ.getAlphaM();
988 } else {
989 final double dt = circ.getDate().durationFrom(previousDate);
990 final double keplerAM = previousAlphaM + circ.getKeplerianMeanMotion() * dt;
991 continuousRAAN = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
992 continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
993 }
994 previousDate = circ.getDate();
995 previousRAAN = continuousRAAN;
996 previousAlphaM = continuousAlphaM;
997 if (useDerivatives) {
998 interpolator.addSamplePoint(circ.getDate().durationFrom(date),
999 new double[] {
1000 circ.getA(),
1001 circ.getCircularEx(),
1002 circ.getCircularEy(),
1003 circ.getI(),
1004 continuousRAAN,
1005 continuousAlphaM
1006 }, new double[] {
1007 circ.getADot(),
1008 circ.getCircularExDot(),
1009 circ.getCircularEyDot(),
1010 circ.getIDot(),
1011 circ.getRightAscensionOfAscendingNodeDot(),
1012 circ.getAlphaMDot()
1013 });
1014 } else {
1015 interpolator.addSamplePoint(circ.getDate().durationFrom(date),
1016 new double[] {
1017 circ.getA(),
1018 circ.getCircularEx(),
1019 circ.getCircularEy(),
1020 circ.getI(),
1021 continuousRAAN,
1022 continuousAlphaM
1023 });
1024 }
1025 }
1026
1027
1028 final double[][] interpolated = interpolator.derivatives(0.0, 1);
1029
1030
1031 return new CircularOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
1032 interpolated[0][3], interpolated[0][4], interpolated[0][5],
1033 interpolated[1][0], interpolated[1][1], interpolated[1][2],
1034 interpolated[1][3], interpolated[1][4], interpolated[1][5],
1035 PositionAngle.MEAN, getFrame(), date, getMu());
1036
1037 }
1038
1039
1040 protected double[][] computeJacobianMeanWrtCartesian() {
1041
1042
1043 final double[][] jacobian = new double[6][6];
1044
1045 computePVWithoutA();
1046 final Vector3D position = partialPV.getPosition();
1047 final Vector3D velocity = partialPV.getVelocity();
1048 final double x = position.getX();
1049 final double y = position.getY();
1050 final double z = position.getZ();
1051 final double vx = velocity.getX();
1052 final double vy = velocity.getY();
1053 final double vz = velocity.getZ();
1054 final double pv = Vector3D.dotProduct(position, velocity);
1055 final double r2 = position.getNormSq();
1056 final double r = FastMath.sqrt(r2);
1057 final double v2 = velocity.getNormSq();
1058
1059 final double mu = getMu();
1060 final double oOsqrtMuA = 1 / FastMath.sqrt(mu * a);
1061 final double rOa = r / a;
1062 final double aOr = a / r;
1063 final double aOr2 = a / r2;
1064 final double a2 = a * a;
1065
1066 final double ex2 = ex * ex;
1067 final double ey2 = ey * ey;
1068 final double e2 = ex2 + ey2;
1069 final double epsilon = FastMath.sqrt(1 - e2);
1070 final double beta = 1 / (1 + epsilon);
1071
1072 final double eCosE = 1 - rOa;
1073 final double eSinE = pv * oOsqrtMuA;
1074
1075 final double cosI = FastMath.cos(i);
1076 final double sinI = FastMath.sin(i);
1077 final double cosRaan = FastMath.cos(raan);
1078 final double sinRaan = FastMath.sin(raan);
1079
1080
1081 fillHalfRow(2 * aOr * aOr2, position, jacobian[0], 0);
1082 fillHalfRow(2 * a2 / mu, velocity, jacobian[0], 3);
1083
1084
1085 final Vector3D danP = new Vector3D(v2, position, -pv, velocity);
1086 final Vector3D danV = new Vector3D(r2, velocity, -pv, position);
1087 final double recip = 1 / partialPV.getMomentum().getNorm();
1088 final double recip2 = recip * recip;
1089 final Vector3D dwXP = new Vector3D(recip, new Vector3D( 0, vz, -vy), -recip2 * sinRaan * sinI, danP);
1090 final Vector3D dwYP = new Vector3D(recip, new Vector3D(-vz, 0, vx), recip2 * cosRaan * sinI, danP);
1091 final Vector3D dwZP = new Vector3D(recip, new Vector3D( vy, -vx, 0), -recip2 * cosI, danP);
1092 final Vector3D dwXV = new Vector3D(recip, new Vector3D( 0, -z, y), -recip2 * sinRaan * sinI, danV);
1093 final Vector3D dwYV = new Vector3D(recip, new Vector3D( z, 0, -x), recip2 * cosRaan * sinI, danV);
1094 final Vector3D dwZV = new Vector3D(recip, new Vector3D( -y, x, 0), -recip2 * cosI, danV);
1095
1096
1097 fillHalfRow(sinRaan * cosI, dwXP, -cosRaan * cosI, dwYP, -sinI, dwZP, jacobian[3], 0);
1098 fillHalfRow(sinRaan * cosI, dwXV, -cosRaan * cosI, dwYV, -sinI, dwZV, jacobian[3], 3);
1099
1100
1101 fillHalfRow(sinRaan / sinI, dwYP, cosRaan / sinI, dwXP, jacobian[4], 0);
1102 fillHalfRow(sinRaan / sinI, dwYV, cosRaan / sinI, dwXV, jacobian[4], 3);
1103
1104
1105
1106 final double u = x * cosRaan + y * sinRaan;
1107 final double cv = -x * sinRaan + y * cosRaan;
1108 final double v = cv * cosI + z * sinI;
1109
1110
1111 final Vector3D duP = new Vector3D(cv * cosRaan / sinI, dwXP,
1112 cv * sinRaan / sinI, dwYP,
1113 1, new Vector3D(cosRaan, sinRaan, 0));
1114 final Vector3D duV = new Vector3D(cv * cosRaan / sinI, dwXV,
1115 cv * sinRaan / sinI, dwYV);
1116
1117
1118 final Vector3D dvP = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXP,
1119 -u * sinRaan * cosI / sinI - cosRaan * z, dwYP,
1120 cv, dwZP,
1121 1, new Vector3D(-sinRaan * cosI, cosRaan * cosI, sinI));
1122 final Vector3D dvV = new Vector3D(-u * cosRaan * cosI / sinI + sinRaan * z, dwXV,
1123 -u * sinRaan * cosI / sinI - cosRaan * z, dwYV,
1124 cv, dwZV);
1125
1126 final Vector3D dc1P = new Vector3D(aOr2 * (2 * eSinE * eSinE + 1 - eCosE) / r2, position,
1127 -2 * aOr2 * eSinE * oOsqrtMuA, velocity);
1128 final Vector3D dc1V = new Vector3D(-2 * aOr2 * eSinE * oOsqrtMuA, position,
1129 2 / mu, velocity);
1130 final Vector3D dc2P = new Vector3D(aOr2 * eSinE * (eSinE * eSinE - (1 - e2)) / (r2 * epsilon), position,
1131 aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, velocity);
1132 final Vector3D dc2V = new Vector3D(aOr2 * (1 - e2 - eSinE * eSinE) * oOsqrtMuA / epsilon, position,
1133 eSinE / (mu * epsilon), velocity);
1134
1135 final double cof1 = aOr2 * (eCosE - e2);
1136 final double cof2 = aOr2 * epsilon * eSinE;
1137 final Vector3D dexP = new Vector3D(u, dc1P, v, dc2P, cof1, duP, cof2, dvP);
1138 final Vector3D dexV = new Vector3D(u, dc1V, v, dc2V, cof1, duV, cof2, dvV);
1139 final Vector3D deyP = new Vector3D(v, dc1P, -u, dc2P, cof1, dvP, -cof2, duP);
1140 final Vector3D deyV = new Vector3D(v, dc1V, -u, dc2V, cof1, dvV, -cof2, duV);
1141 fillHalfRow(1, dexP, jacobian[1], 0);
1142 fillHalfRow(1, dexV, jacobian[1], 3);
1143 fillHalfRow(1, deyP, jacobian[2], 0);
1144 fillHalfRow(1, deyV, jacobian[2], 3);
1145
1146 final double cle = u / a + ex - eSinE * beta * ey;
1147 final double sle = v / a + ey + eSinE * beta * ex;
1148 final double m1 = beta * eCosE;
1149 final double m2 = 1 - m1 * eCosE;
1150 final double m3 = (u * ey - v * ex) + eSinE * beta * (u * ex + v * ey);
1151 final double m4 = -sle + cle * eSinE * beta;
1152 final double m5 = cle + sle * eSinE * beta;
1153 fillHalfRow((2 * m3 / r + aOr * eSinE + m1 * eSinE * (1 + m1 - (1 + aOr) * m2) / epsilon) / r2, position,
1154 (m1 * m2 / epsilon - 1) * oOsqrtMuA, velocity,
1155 m4, dexP, m5, deyP, -sle / a, duP, cle / a, dvP,
1156 jacobian[5], 0);
1157 fillHalfRow((m1 * m2 / epsilon - 1) * oOsqrtMuA, position,
1158 (2 * m3 + eSinE * a + m1 * eSinE * r * (eCosE * beta * 2 - aOr * m2) / epsilon) / mu, velocity,
1159 m4, dexV, m5, deyV, -sle / a, duV, cle / a, dvV,
1160 jacobian[5], 3);
1161
1162 return jacobian;
1163
1164 }
1165
1166
1167 protected double[][] computeJacobianEccentricWrtCartesian() {
1168
1169
1170 final double[][] jacobian = computeJacobianMeanWrtCartesian();
1171
1172
1173
1174
1175
1176 final double alphaE = getAlphaE();
1177 final double cosAe = FastMath.cos(alphaE);
1178 final double sinAe = FastMath.sin(alphaE);
1179 final double aOr = 1 / (1 - ex * cosAe - ey * sinAe);
1180
1181
1182 final double[] rowEx = jacobian[1];
1183 final double[] rowEy = jacobian[2];
1184 final double[] rowL = jacobian[5];
1185 for (int j = 0; j < 6; ++j) {
1186 rowL[j] = aOr * (rowL[j] + sinAe * rowEx[j] - cosAe * rowEy[j]);
1187 }
1188
1189 return jacobian;
1190
1191 }
1192
1193
1194 protected double[][] computeJacobianTrueWrtCartesian() {
1195
1196
1197 final double[][] jacobian = computeJacobianEccentricWrtCartesian();
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211 final double alphaE = getAlphaE();
1212 final double cosAe = FastMath.cos(alphaE);
1213 final double sinAe = FastMath.sin(alphaE);
1214 final double eSinE = ex * sinAe - ey * cosAe;
1215 final double ecosE = ex * cosAe + ey * sinAe;
1216 final double e2 = ex * ex + ey * ey;
1217 final double epsilon = FastMath.sqrt(1 - e2);
1218 final double onePeps = 1 + epsilon;
1219 final double d = onePeps - ecosE;
1220 final double cT = (d * d + eSinE * eSinE) / 2;
1221 final double cE = ecosE * onePeps - e2;
1222 final double cX = ex * eSinE / epsilon - ey + sinAe * onePeps;
1223 final double cY = ey * eSinE / epsilon + ex - cosAe * onePeps;
1224 final double factorLe = (cT + cE) / cT;
1225 final double factorEx = cX / cT;
1226 final double factorEy = cY / cT;
1227
1228
1229 final double[] rowEx = jacobian[1];
1230 final double[] rowEy = jacobian[2];
1231 final double[] rowA = jacobian[5];
1232 for (int j = 0; j < 6; ++j) {
1233 rowA[j] = factorLe * rowA[j] + factorEx * rowEx[j] + factorEy * rowEy[j];
1234 }
1235
1236 return jacobian;
1237
1238 }
1239
1240
1241 public void addKeplerContribution(final PositionAngle type, final double gm,
1242 final double[] pDot) {
1243 final double oMe2;
1244 final double ksi;
1245 final double n = FastMath.sqrt(gm / a) / a;
1246 switch (type) {
1247 case MEAN :
1248 pDot[5] += n;
1249 break;
1250 case ECCENTRIC :
1251 oMe2 = 1 - ex * ex - ey * ey;
1252 ksi = 1 + ex * FastMath.cos(alphaV) + ey * FastMath.sin(alphaV);
1253 pDot[5] += n * ksi / oMe2;
1254 break;
1255 case TRUE :
1256 oMe2 = 1 - ex * ex - ey * ey;
1257 ksi = 1 + ex * FastMath.cos(alphaV) + ey * FastMath.sin(alphaV);
1258 pDot[5] += n * ksi * ksi / (oMe2 * FastMath.sqrt(oMe2));
1259 break;
1260 default :
1261 throw new OrekitInternalError(null);
1262 }
1263 }
1264
1265
1266
1267
1268 public String toString() {
1269 return new StringBuffer().append("circular parameters: ").append('{').
1270 append("a: ").append(a).
1271 append(", ex: ").append(ex).append(", ey: ").append(ey).
1272 append(", i: ").append(FastMath.toDegrees(i)).
1273 append(", raan: ").append(FastMath.toDegrees(raan)).
1274 append(", alphaV: ").append(FastMath.toDegrees(alphaV)).
1275 append(";}").toString();
1276 }
1277
1278
1279
1280
1281 private Object writeReplace() {
1282 return new DTO(this);
1283 }
1284
1285
1286 private static class DTO implements Serializable {
1287
1288
1289 private static final long serialVersionUID = 20170414L;
1290
1291
1292 private double[] d;
1293
1294
1295 private final Frame frame;
1296
1297
1298
1299
1300 private DTO(final CircularOrbit orbit) {
1301
1302 final AbsoluteDate date = orbit.getDate();
1303
1304
1305 final double epoch = FastMath.floor(date.durationFrom(AbsoluteDate.J2000_EPOCH));
1306 final double offset = date.durationFrom(AbsoluteDate.J2000_EPOCH.shiftedBy(epoch));
1307
1308 if (orbit.serializePV) {
1309 final TimeStampedPVCoordinates pv = orbit.getPVCoordinates();
1310 if (orbit.hasDerivatives()) {
1311 this.d = new double[] {
1312
1313 epoch, offset, orbit.getMu(),
1314 orbit.a, orbit.ex, orbit.ey,
1315 orbit.i, orbit.raan, orbit.alphaV,
1316 orbit.aDot, orbit.exDot, orbit.eyDot,
1317 orbit.iDot, orbit.raanDot, orbit.alphaVDot,
1318 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
1319 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
1320 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
1321 };
1322 } else {
1323 this.d = new double[] {
1324
1325 epoch, offset, orbit.getMu(),
1326 orbit.a, orbit.ex, orbit.ey,
1327 orbit.i, orbit.raan, orbit.alphaV,
1328 pv.getPosition().getX(), pv.getPosition().getY(), pv.getPosition().getZ(),
1329 pv.getVelocity().getX(), pv.getVelocity().getY(), pv.getVelocity().getZ(),
1330 pv.getAcceleration().getX(), pv.getAcceleration().getY(), pv.getAcceleration().getZ(),
1331 };
1332 }
1333 } else {
1334 if (orbit.hasDerivatives()) {
1335
1336 this.d = new double[] {
1337 epoch, offset, orbit.getMu(),
1338 orbit.a, orbit.ex, orbit.ey,
1339 orbit.i, orbit.raan, orbit.alphaV,
1340 orbit.aDot, orbit.exDot, orbit.eyDot,
1341 orbit.iDot, orbit.raanDot, orbit.alphaVDot
1342 };
1343 } else {
1344
1345 this.d = new double[] {
1346 epoch, offset, orbit.getMu(),
1347 orbit.a, orbit.ex, orbit.ey,
1348 orbit.i, orbit.raan, orbit.alphaV
1349 };
1350 }
1351 }
1352
1353 this.frame = orbit.getFrame();
1354
1355 }
1356
1357
1358
1359
1360 private Object readResolve() {
1361 switch (d.length) {
1362 case 24 :
1363 return new CircularOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
1364 d[ 9], d[10], d[11], d[12], d[13], d[14],
1365 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
1366 new Vector3D(d[15], d[16], d[17]),
1367 new Vector3D(d[18], d[19], d[20]),
1368 new Vector3D(d[21], d[22], d[23])),
1369 frame,
1370 d[2]);
1371 case 18 :
1372 return new CircularOrbit(d[3], d[4], d[5], d[6], d[7], d[8],
1373 Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN, Double.NaN,
1374 new TimeStampedPVCoordinates(AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
1375 new Vector3D(d[ 9], d[10], d[11]),
1376 new Vector3D(d[12], d[13], d[14]),
1377 new Vector3D(d[15], d[16], d[17])),
1378 frame,
1379 d[2]);
1380 case 15 :
1381 return new CircularOrbit(d[ 3], d[ 4], d[ 5], d[ 6], d[ 7], d[ 8],
1382 d[ 9], d[10], d[11], d[12], d[13], d[14],
1383 PositionAngle.TRUE,
1384 frame, AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
1385 d[2]);
1386 default :
1387 return new CircularOrbit(d[3], d[4], d[5], d[6], d[7], d[8], PositionAngle.TRUE,
1388 frame, AbsoluteDate.J2000_EPOCH.shiftedBy(d[0]).shiftedBy(d[1]),
1389 d[2]);
1390
1391 }
1392 }
1393
1394 }
1395
1396 }