1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import org.hamcrest.MatcherAssert;
20 import org.hipparchus.analysis.UnivariateFunction;
21 import org.hipparchus.analysis.differentiation.DSFactory;
22 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
23 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
24 import org.hipparchus.geometry.euclidean.threed.Vector3D;
25 import org.hipparchus.linear.MatrixUtils;
26 import org.hipparchus.linear.RealMatrixPreservingVisitor;
27 import org.hipparchus.util.FastMath;
28 import org.hipparchus.util.MathUtils;
29 import org.junit.jupiter.api.*;
30 import org.junit.jupiter.params.ParameterizedTest;
31 import org.junit.jupiter.params.provider.EnumSource;
32 import org.orekit.Utils;
33 import org.orekit.errors.OrekitException;
34 import org.orekit.errors.OrekitIllegalArgumentException;
35 import org.orekit.errors.OrekitMessages;
36 import org.orekit.frames.Frame;
37 import org.orekit.frames.FramesFactory;
38 import org.orekit.frames.Transform;
39 import org.orekit.time.AbsoluteDate;
40 import org.orekit.time.TimeScalesFactory;
41 import org.orekit.utils.Constants;
42 import org.orekit.utils.PVCoordinates;
43 import org.orekit.utils.TimeStampedPVCoordinates;
44
45 import java.util.function.Function;
46
47 import static org.orekit.OrekitMatchers.relativelyCloseTo;
48
49 class KeplerianOrbitTest {
50
51
52 private AbsoluteDate date;
53
54
55 private double mu;
56
57 @ParameterizedTest
58 @EnumSource(PositionAngleType.class)
59 void testWithCachedPositionAngleType(final PositionAngleType positionAngleType) {
60
61 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
62 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
63 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
64 final double muEarth = 3.9860047e14;
65 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, muEarth);
66 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(cartesianOrbit);
67
68 final KeplerianOrbit orbit = keplerianOrbit.withCachedPositionAngleType(positionAngleType);
69
70 Assertions.assertEquals(keplerianOrbit.getFrame(), orbit.getFrame());
71 Assertions.assertEquals(keplerianOrbit.getDate(), orbit.getDate());
72 Assertions.assertEquals(keplerianOrbit.getMu(), orbit.getMu());
73 final Vector3D relativePosition = keplerianOrbit.getPosition(orbit.getFrame()).subtract(
74 orbit.getPosition());
75 Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
76 Assertions.assertEquals(keplerianOrbit.hasNonKeplerianAcceleration(),
77 orbit.hasNonKeplerianAcceleration());
78 }
79
80 @ParameterizedTest
81 @EnumSource(PositionAngleType.class)
82 void testInFrameKeplerian(final PositionAngleType positionAngleType) {
83 testTemplateInFrame(Vector3D.ZERO, positionAngleType);
84 }
85
86 @Test
87 void testInFrameNonKeplerian() {
88 testTemplateInFrame(Vector3D.PLUS_K, PositionAngleType.TRUE);
89 }
90
91 private void testTemplateInFrame(final Vector3D acceleration, final PositionAngleType positionAngleType) {
92
93 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
94 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
95 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
96 final double muEarth = 3.9860047e14;
97 final CartesianOrbit cartesianOrbit = new CartesianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, muEarth);
98 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(cartesianOrbit).withCachedPositionAngleType(positionAngleType);
99
100 final KeplerianOrbit orbitWithOtherFrame = keplerianOrbit.inFrame(FramesFactory.getGCRF());
101
102 Assertions.assertNotEquals(keplerianOrbit.getFrame(), orbitWithOtherFrame.getFrame());
103 Assertions.assertEquals(keplerianOrbit.getDate(), orbitWithOtherFrame.getDate());
104 Assertions.assertEquals(keplerianOrbit.getMu(), orbitWithOtherFrame.getMu());
105 final Vector3D relativePosition = keplerianOrbit.getPosition(orbitWithOtherFrame.getFrame()).subtract(
106 orbitWithOtherFrame.getPosition());
107 Assertions.assertEquals(0., relativePosition.getNorm(), 1e-6);
108 Assertions.assertEquals(keplerianOrbit.hasNonKeplerianAcceleration(),
109 orbitWithOtherFrame.hasNonKeplerianAcceleration());
110 }
111
112 @Test
113 void testKeplerianToKeplerian() {
114
115
116 KeplerianOrbit kep =
117 new KeplerianOrbit(24464560.0, 0.7311, 0.122138, 3.10686, 1.00681,
118 0.048363, PositionAngleType.MEAN,
119 FramesFactory.getEME2000(), date, mu);
120
121 Vector3D pos = kep.getPosition();
122 Vector3D vit = kep.getPVCoordinates().getVelocity();
123
124 KeplerianOrbit param = new KeplerianOrbit(new PVCoordinates(pos, vit),
125 FramesFactory.getEME2000(), date, mu);
126 Assertions.assertEquals(param.getA(), kep.getA(), Utils.epsilonTest * kep.getA());
127 Assertions.assertEquals(param.getE(), kep.getE(), Utils.epsilonE * FastMath.abs(kep.getE()));
128 Assertions.assertEquals(MathUtils.normalizeAngle(param.getI(), kep.getI()), kep.getI(), Utils.epsilonAngle * FastMath.abs(kep.getI()));
129 Assertions.assertEquals(MathUtils.normalizeAngle(param.getPerigeeArgument(), kep.getPerigeeArgument()), kep.getPerigeeArgument(), Utils.epsilonAngle * FastMath.abs(kep.getPerigeeArgument()));
130 Assertions.assertEquals(MathUtils.normalizeAngle(param.getRightAscensionOfAscendingNode(), kep.getRightAscensionOfAscendingNode()), kep.getRightAscensionOfAscendingNode(), Utils.epsilonAngle * FastMath.abs(kep.getRightAscensionOfAscendingNode()));
131 Assertions.assertEquals(MathUtils.normalizeAngle(param.getMeanAnomaly(), kep.getMeanAnomaly()), kep.getMeanAnomaly(), Utils.epsilonAngle * FastMath.abs(kep.getMeanAnomaly()));
132
133
134 KeplerianOrbit kepCir =
135 new KeplerianOrbit(24464560.0, 0.0, 0.122138, 3.10686, 1.00681,
136 0.048363, PositionAngleType.MEAN,
137 FramesFactory.getEME2000(), date, mu);
138
139 Vector3D posCir = kepCir.getPosition();
140 Vector3D vitCir = kepCir.getPVCoordinates().getVelocity();
141
142 KeplerianOrbit paramCir = new KeplerianOrbit(new PVCoordinates(posCir, vitCir),
143 FramesFactory.getEME2000(), date, mu);
144 Assertions.assertEquals(paramCir.getA(), kepCir.getA(), Utils.epsilonTest * kepCir.getA());
145 Assertions.assertEquals(paramCir.getE(), kepCir.getE(), Utils.epsilonE * FastMath.max(1., FastMath.abs(kepCir.getE())));
146 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getI(), kepCir.getI()), kepCir.getI(), Utils.epsilonAngle * FastMath.abs(kepCir.getI()));
147 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLM(), kepCir.getLM()), kepCir.getLM(), Utils.epsilonAngle * FastMath.abs(kepCir.getLM()));
148 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLE(), kepCir.getLE()), kepCir.getLE(), Utils.epsilonAngle * FastMath.abs(kepCir.getLE()));
149 Assertions.assertEquals(MathUtils.normalizeAngle(paramCir.getLv(), kepCir.getLv()), kepCir.getLv(), Utils.epsilonAngle * FastMath.abs(kepCir.getLv()));
150
151
152 KeplerianOrbit kepHyp =
153 new KeplerianOrbit(-24464560.0, 1.7311, 0.122138, 3.10686, 1.00681,
154 0.048363, PositionAngleType.MEAN,
155 FramesFactory.getEME2000(), date, mu);
156
157 Vector3D posHyp = kepHyp.getPosition();
158 Vector3D vitHyp = kepHyp.getPVCoordinates().getVelocity();
159
160 KeplerianOrbit paramHyp = new KeplerianOrbit(new PVCoordinates(posHyp, vitHyp),
161 FramesFactory.getEME2000(), date, mu);
162 Assertions.assertEquals(paramHyp.getA(), kepHyp.getA(), Utils.epsilonTest * FastMath.abs(kepHyp.getA()));
163 Assertions.assertEquals(paramHyp.getE(), kepHyp.getE(), Utils.epsilonE * FastMath.abs(kepHyp.getE()));
164 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getI(), kepHyp.getI()), kepHyp.getI(), Utils.epsilonAngle * FastMath.abs(kepHyp.getI()));
165 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getPerigeeArgument(), kepHyp.getPerigeeArgument()), kepHyp.getPerigeeArgument(), Utils.epsilonAngle * FastMath.abs(kepHyp.getPerigeeArgument()));
166 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getRightAscensionOfAscendingNode(), kepHyp.getRightAscensionOfAscendingNode()), kepHyp.getRightAscensionOfAscendingNode(), Utils.epsilonAngle * FastMath.abs(kepHyp.getRightAscensionOfAscendingNode()));
167 Assertions.assertEquals(MathUtils.normalizeAngle(paramHyp.getMeanAnomaly(), kepHyp.getMeanAnomaly()), kepHyp.getMeanAnomaly(), Utils.epsilonAngle * FastMath.abs(kepHyp.getMeanAnomaly()));
168
169 }
170
171 @Test
172 void testKeplerianToCartesian() {
173
174 KeplerianOrbit kep =
175 new KeplerianOrbit(24464560.0, 0.7311, 0.122138, 3.10686, 1.00681,
176 0.048363, PositionAngleType.MEAN,
177 FramesFactory.getEME2000(), date, mu);
178
179 Vector3D pos = kep.getPosition();
180 Vector3D vit = kep.getPVCoordinates().getVelocity();
181 Assertions.assertEquals(-0.107622532467967e+07, pos.getX(), Utils.epsilonTest * FastMath.abs(pos.getX()));
182 Assertions.assertEquals(-0.676589636432773e+07, pos.getY(), Utils.epsilonTest * FastMath.abs(pos.getY()));
183 Assertions.assertEquals(-0.332308783350379e+06, pos.getZ(), Utils.epsilonTest * FastMath.abs(pos.getZ()));
184
185 Assertions.assertEquals( 0.935685775154103e+04, vit.getX(), Utils.epsilonTest * FastMath.abs(vit.getX()));
186 Assertions.assertEquals(-0.331234775037644e+04, vit.getY(), Utils.epsilonTest * FastMath.abs(vit.getY()));
187 Assertions.assertEquals(-0.118801577532701e+04, vit.getZ(), Utils.epsilonTest * FastMath.abs(vit.getZ()));
188 }
189
190 @Test
191 void testKeplerianToEquinoctial() {
192
193 KeplerianOrbit kep =
194 new KeplerianOrbit(24464560.0, 0.7311, 0.122138, 3.10686, 1.00681,
195 0.048363, PositionAngleType.MEAN,
196 FramesFactory.getEME2000(), date, mu);
197
198 Assertions.assertEquals(24464560.0, kep.getA(), Utils.epsilonTest * kep.getA());
199 Assertions.assertEquals(-0.412036802887626, kep.getEquinoctialEx(), Utils.epsilonE * FastMath.abs(kep.getE()));
200 Assertions.assertEquals(-0.603931190671706, kep.getEquinoctialEy(), Utils.epsilonE * FastMath.abs(kep.getE()));
201 Assertions.assertEquals(MathUtils.normalizeAngle(2*FastMath.asin(FastMath.sqrt((FastMath.pow(0.652494417368829e-01, 2)+FastMath.pow(0.103158450084864, 2))/4.)), kep.getI()), kep.getI(), Utils.epsilonAngle * FastMath.abs(kep.getI()));
202 Assertions.assertEquals(MathUtils.normalizeAngle(0.416203300000000e+01, kep.getLM()), kep.getLM(), Utils.epsilonAngle * FastMath.abs(kep.getLM()));
203
204 }
205
206 @Test
207 void testAnomaly() {
208
209 Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
210 Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
211 double mu = 3.9860047e14;
212
213 KeplerianOrbit p = new KeplerianOrbit(new PVCoordinates(position, velocity),
214 FramesFactory.getEME2000(), date, mu);
215
216
217 double e = p.getE();
218 double eRatio = FastMath.sqrt((1 - e) / (1 + e));
219
220 double v = 1.1;
221
222 double E = 2 * FastMath.atan(eRatio * FastMath.tan(v / 2));
223 double M = E - e * FastMath.sin(E);
224
225 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
226 p.getRightAscensionOfAscendingNode(), v , PositionAngleType.TRUE,
227 p.getFrame(), p.getDate(), p.getMu());
228 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
229 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
230 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
231 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
232 p.getRightAscensionOfAscendingNode(), 0 , PositionAngleType.TRUE,
233 p.getFrame(), p.getDate(), p.getMu());
234
235 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
236 p.getRightAscensionOfAscendingNode(), E , PositionAngleType.ECCENTRIC,
237 p.getFrame(), p.getDate(), p.getMu());
238 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
239 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
240 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
241 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
242 p.getRightAscensionOfAscendingNode(), 0 , PositionAngleType.TRUE,
243 p.getFrame(), p.getDate(), p.getMu());
244
245 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
246 p.getRightAscensionOfAscendingNode(), M, PositionAngleType.MEAN,
247 p.getFrame(), p.getDate(), p.getMu());
248 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
249 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
250 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
251
252
253 p = new KeplerianOrbit(p.getA(), 0, p.getI(), p.getPerigeeArgument(),
254 p.getRightAscensionOfAscendingNode(), p.getLv() , PositionAngleType.TRUE,
255 p.getFrame(), p.getDate(), p.getMu());
256
257 E = v;
258 M = E;
259
260 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
261 p.getRightAscensionOfAscendingNode(), v , PositionAngleType.TRUE,
262 p.getFrame(), p.getDate(), p.getMu());
263 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
264 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
265 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
266 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
267 p.getRightAscensionOfAscendingNode(), 0 , PositionAngleType.TRUE,
268 p.getFrame(), p.getDate(), p.getMu());
269
270 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
271 p.getRightAscensionOfAscendingNode(), E , PositionAngleType.ECCENTRIC, p.getFrame(), p.getDate(), p.getMu());
272 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
273 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
274 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
275 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
276 p.getRightAscensionOfAscendingNode(), 0 , PositionAngleType.TRUE,
277 p.getFrame(), p.getDate(), p.getMu());
278
279 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
280 p.getRightAscensionOfAscendingNode(), M, PositionAngleType.MEAN,
281 p.getFrame(), p.getDate(), p.getMu());
282 Assertions.assertEquals(p.getTrueAnomaly(), v, Utils.epsilonAngle * FastMath.abs(v));
283 Assertions.assertEquals(p.getEccentricAnomaly(), E, Utils.epsilonAngle * FastMath.abs(E));
284 Assertions.assertEquals(p.getMeanAnomaly(), M, Utils.epsilonAngle * FastMath.abs(M));
285
286 }
287
288 @Test
289 void testPositionVelocityNorms() {
290 double mu = 3.9860047e14;
291
292
293 KeplerianOrbit p =
294 new KeplerianOrbit(24464560.0, 0.7311, 2.1, 3.10686, 1.00681,
295 0.67, PositionAngleType.TRUE,
296 FramesFactory.getEME2000(), date, mu);
297
298 double e = p.getE();
299 double v = p.getTrueAnomaly();
300 double ksi = 1 + e * FastMath.cos(v);
301 double nu = e * FastMath.sin(v);
302 double epsilon = FastMath.sqrt((1 - e) * (1 + e));
303
304 double a = p.getA();
305 double na = FastMath.sqrt(mu / a);
306
307
308 Assertions.assertEquals(a * epsilon * epsilon / ksi,
309 p.getPosition().getNorm(),
310 Utils.epsilonTest * FastMath.abs(p.getPosition().getNorm()));
311
312
313 Assertions.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon,
314 p.getPVCoordinates().getVelocity().getNorm(),
315 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm()));
316
317
318
319 KeplerianOrbit pCirEqua =
320 new KeplerianOrbit(24464560.0, 0.1e-10, 0.1e-8, 3.10686, 1.00681,
321 0.67, PositionAngleType.TRUE,
322 FramesFactory.getEME2000(), date, mu);
323
324 e = pCirEqua.getE();
325 v = pCirEqua.getTrueAnomaly();
326 ksi = 1 + e * FastMath.cos(v);
327 nu = e * FastMath.sin(v);
328 epsilon = FastMath.sqrt((1 - e) * (1 + e));
329
330 a = pCirEqua.getA();
331 na = FastMath.sqrt(mu / a);
332
333
334 Assertions.assertEquals(a * epsilon * epsilon / ksi,
335 pCirEqua.getPosition().getNorm(),
336 Utils.epsilonTest * FastMath.abs(pCirEqua.getPosition().getNorm()));
337
338
339 Assertions.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon,
340 pCirEqua.getPVCoordinates().getVelocity().getNorm(),
341 Utils.epsilonTest * FastMath.abs(pCirEqua.getPVCoordinates().getVelocity().getNorm()));
342 }
343
344 @Test
345 void testGeometry() {
346 double mu = 3.9860047e14;
347
348
349 KeplerianOrbit p =
350 new KeplerianOrbit(24464560.0, 0.7311, 2.1, 3.10686, 1.00681,
351 0.67, PositionAngleType.TRUE,
352 FramesFactory.getEME2000(), date, mu);
353
354 Vector3D position = p.getPosition();
355 Vector3D velocity = p.getPVCoordinates().getVelocity();
356 Vector3D momentum = p.getPVCoordinates().getMomentum().normalize();
357
358 double apogeeRadius = p.getA() * (1 + p.getE());
359 double perigeeRadius = p.getA() * (1 - p.getE());
360
361 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI/100.) {
362 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
363 p.getRightAscensionOfAscendingNode(), lv , PositionAngleType.TRUE,
364 p.getFrame(), p.getDate(), p.getMu());
365 position = p.getPosition();
366
367
368 Assertions.assertTrue((position.getNorm() - apogeeRadius) <= ( apogeeRadius * Utils.epsilonTest));
369 Assertions.assertTrue((position.getNorm() - perigeeRadius) >= (- perigeeRadius * Utils.epsilonTest));
370
371 position = position.normalize();
372 velocity = p.getPVCoordinates().getVelocity();
373 velocity = velocity.normalize();
374
375
376
377
378 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
379
380 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
381
382 }
383
384
385 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
386 p.getRightAscensionOfAscendingNode(), 0 , PositionAngleType.TRUE, p.getFrame(), p.getDate(), p.getMu());
387 Assertions.assertEquals(p.getPosition().getNorm(), perigeeRadius, perigeeRadius * Utils.epsilonTest);
388
389 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
390 p.getRightAscensionOfAscendingNode(), FastMath.PI , PositionAngleType.TRUE, p.getFrame(), p.getDate(), p.getMu());
391 Assertions.assertEquals(p.getPosition().getNorm(), apogeeRadius, apogeeRadius * Utils.epsilonTest);
392
393
394
395 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
396 p.getRightAscensionOfAscendingNode(), FastMath.PI - p.getPerigeeArgument() , PositionAngleType.TRUE,
397 p.getFrame(), p.getDate(), p.getMu());
398 Assertions.assertTrue(FastMath.abs(p.getPosition().getZ()) < p.getPosition().getNorm() * Utils.epsilonTest);
399 Assertions.assertTrue(p.getPVCoordinates().getVelocity().getZ() < 0);
400
401
402 p = new KeplerianOrbit(p.getA(), p.getE(), p.getI(), p.getPerigeeArgument(),
403 p.getRightAscensionOfAscendingNode(), 2.0 * FastMath.PI - p.getPerigeeArgument() , PositionAngleType.TRUE,
404 p.getFrame(), p.getDate(), p.getMu());
405 Assertions.assertTrue(FastMath.abs(p.getPosition().getZ()) < p.getPosition().getNorm() * Utils.epsilonTest);
406 Assertions.assertTrue(p.getPVCoordinates().getVelocity().getZ() > 0);
407
408
409
410 KeplerianOrbit pCirEqua =
411 new KeplerianOrbit(24464560.0, 0.1e-10, 0.1e-8, 3.10686, 1.00681,
412 0.67, PositionAngleType.TRUE, FramesFactory.getEME2000(), date, mu);
413
414 position = pCirEqua.getPosition();
415 velocity = pCirEqua.getPVCoordinates().getVelocity();
416 momentum = Vector3D.crossProduct(position, velocity).normalize();
417
418 apogeeRadius = pCirEqua.getA() * (1 + pCirEqua.getE());
419 perigeeRadius = pCirEqua.getA() * (1 - pCirEqua.getE());
420
421 Assertions.assertEquals(perigeeRadius, apogeeRadius, 1.e+4 * Utils.epsilonTest * apogeeRadius);
422
423 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI/100.) {
424 pCirEqua = new KeplerianOrbit(pCirEqua.getA(), pCirEqua.getE(), pCirEqua.getI(), pCirEqua.getPerigeeArgument(),
425 pCirEqua.getRightAscensionOfAscendingNode(), lv, PositionAngleType.TRUE,
426 pCirEqua.getFrame(), pCirEqua.getDate(), pCirEqua.getMu());
427 position = pCirEqua.getPosition();
428
429
430
431 Assertions.assertTrue((position.getNorm() - apogeeRadius) <= ( apogeeRadius * Utils.epsilonTest));
432 Assertions.assertTrue((position.getNorm() - perigeeRadius) >= (- perigeeRadius * Utils.epsilonTest));
433
434 position = position.normalize();
435 velocity = pCirEqua.getPVCoordinates().getVelocity();
436 velocity = velocity.normalize();
437
438
439
440
441 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
442
443 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
444
445 }
446 }
447
448 @Test
449 void testSymmetry() {
450
451
452 Vector3D position = new Vector3D(-4947831., -3765382., -3708221.);
453 Vector3D velocity = new Vector3D(-2079., 5291., -7842.);
454 double mu = 3.9860047e14;
455
456 KeplerianOrbit p = new KeplerianOrbit(new PVCoordinates(position, velocity),
457 FramesFactory.getEME2000(), date, mu);
458 Vector3D positionOffset = p.getPosition().subtract(position);
459 Vector3D velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
460
461 Assertions.assertEquals(0.0, positionOffset.getNorm(), 2.9e-9);
462 Assertions.assertEquals(0.0, velocityOffset.getNorm(), 1.0e-15);
463
464
465 position = new Vector3D(1742382., -2.440243e7, -0.014517);
466 velocity = new Vector3D(4026.2, 287.479, -3.e-6);
467
468
469 p = new KeplerianOrbit(new PVCoordinates(position, velocity),
470 FramesFactory.getEME2000(), date, mu);
471 positionOffset = p.getPosition().subtract(position);
472 velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
473
474 Assertions.assertEquals(0.0, positionOffset.getNorm(), 4.8e-9);
475 Assertions.assertEquals(0.0, velocityOffset.getNorm(), 1.0e-100);
476
477 }
478
479 @Test
480 void testNonInertialFrame() throws IllegalArgumentException {
481 Assertions.assertThrows(IllegalArgumentException.class, () -> {
482 Vector3D position = new Vector3D(-4947831., -3765382., -3708221.);
483 Vector3D velocity = new Vector3D(-2079., 5291., -7842.);
484 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
485 new KeplerianOrbit(pvCoordinates,
486 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
487 date, mu);
488 });
489 }
490
491 @Test
492 void testPeriod() {
493 KeplerianOrbit orbit = new KeplerianOrbit(7654321.0, 0.1, 0.2, 0, 0, 0,
494 PositionAngleType.TRUE,
495 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
496 mu);
497 Assertions.assertEquals(6664.5521723383589487, orbit.getKeplerianPeriod(), 1.0e-12);
498 Assertions.assertEquals(0.00094277682051291315229, orbit.getKeplerianMeanMotion(), 1.0e-16);
499 }
500
501 @Test
502 void testHyperbola1() {
503 KeplerianOrbit orbit = new KeplerianOrbit(-10000000.0, 2.5, 0.3, 0, 0, 0.0,
504 PositionAngleType.TRUE,
505 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
506 mu);
507 Vector3D perigeeP = orbit.getPosition();
508 Vector3D u = perigeeP.normalize();
509 Vector3D focus1 = Vector3D.ZERO;
510 Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
511 for (double dt = -5000; dt < 5000; dt += 60) {
512 PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
513 double d1 = Vector3D.distance(pv.getPosition(), focus1);
514 double d2 = Vector3D.distance(pv.getPosition(), focus2);
515 Assertions.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
516 KeplerianOrbit rebuilt =
517 new KeplerianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
518 Assertions.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
519 Assertions.assertEquals(2.5, rebuilt.getE(), 1.0e-13);
520 }
521 }
522
523 @Test
524 void testHyperbola2() {
525 KeplerianOrbit orbit = new KeplerianOrbit(-10000000.0, 1.2, 0.3, 0, 0, -1.75,
526 PositionAngleType.MEAN,
527 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
528 mu);
529 Vector3D perigeeP = new KeplerianOrbit(orbit.getA(), orbit.getE(), orbit.getI(),
530 orbit.getPerigeeArgument(), orbit.getRightAscensionOfAscendingNode(),
531 0.0, PositionAngleType.TRUE, orbit.getFrame(),
532 orbit.getDate(), orbit.getMu()).getPosition();
533 Vector3D u = perigeeP.normalize();
534 Vector3D focus1 = Vector3D.ZERO;
535 Vector3D focus2 = new Vector3D(-2 * orbit.getA() * orbit.getE(), u);
536 for (double dt = -5000; dt < 5000; dt += 60) {
537 PVCoordinates pv = orbit.shiftedBy(dt).getPVCoordinates();
538 double d1 = Vector3D.distance(pv.getPosition(), focus1);
539 double d2 = Vector3D.distance(pv.getPosition(), focus2);
540 Assertions.assertEquals(-2 * orbit.getA(), FastMath.abs(d1 - d2), 1.0e-6);
541 KeplerianOrbit rebuilt =
542 new KeplerianOrbit(pv, orbit.getFrame(), orbit.getDate().shiftedBy(dt), mu);
543 Assertions.assertEquals(-10000000.0, rebuilt.getA(), 1.0e-6);
544 Assertions.assertEquals(1.2, rebuilt.getE(), 1.0e-13);
545 }
546 }
547
548 @Test
549 void testInconsistentHyperbola() {
550 try {
551 new KeplerianOrbit(+10000000.0, 2.5, 0.3, 0, 0, 0.0,
552 PositionAngleType.TRUE,
553 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
554 mu);
555 Assertions.fail("an exception should have been thrown");
556 } catch (OrekitIllegalArgumentException oe) {
557 Assertions.assertEquals(OrekitMessages.ORBIT_A_E_MISMATCH_WITH_CONIC_TYPE, oe.getSpecifier());
558 Assertions.assertEquals(+10000000.0, ((Double) oe.getParts()[0]).doubleValue(), 1.0e-3);
559 Assertions.assertEquals(2.5, ((Double) oe.getParts()[1]).doubleValue(), 1.0e-15);
560 }
561 }
562
563 @Test
564 void testVeryLargeEccentricity() {
565
566 final Frame eme2000 = FramesFactory.getEME2000();
567 final double meanAnomaly = 1.;
568 final KeplerianOrbit orb0 = new KeplerianOrbit(42600e3, 0.9, 0.00001, 0, 0,
569 FastMath.toRadians(meanAnomaly),
570 PositionAngleType.MEAN, eme2000, date, mu);
571
572
573 final Vector3D deltaV = new Vector3D(0.0, 110000.0, 0.0);
574 final PVCoordinates pv1 = new PVCoordinates(orb0.getPosition(),
575 orb0.getPVCoordinates().getVelocity().add(deltaV));
576 final KeplerianOrbit orb1 = new KeplerianOrbit(pv1, eme2000, date, mu);
577
578
579
580 final PVCoordinates pvTested = orb1.shiftedBy(0).getPVCoordinates();
581 final Vector3D pTested = pvTested.getPosition();
582 final Vector3D vTested = pvTested.getVelocity();
583
584 final PVCoordinates pvReference = orb1.getPVCoordinates();
585 final Vector3D pReference = pvReference.getPosition();
586 final Vector3D vReference = pvReference.getVelocity();
587
588 final double threshold = 1.e-15;
589 Assertions.assertEquals(0, pTested.subtract(pReference).getNorm(), threshold * pReference.getNorm());
590 Assertions.assertEquals(0, vTested.subtract(vReference).getNorm(), threshold * vReference.getNorm());
591
592 }
593
594 @Test
595 void testKeplerEquation() {
596
597 for (double M = -6 * FastMath.PI; M < 6 * FastMath.PI; M += 0.01) {
598 KeplerianOrbit pElliptic =
599 new KeplerianOrbit(24464560.0, 0.7311, 2.1, 3.10686, 1.00681,
600 M, PositionAngleType.MEAN,
601 FramesFactory.getEME2000(), date, mu);
602 double E = pElliptic.getEccentricAnomaly();
603 double e = pElliptic.getE();
604 Assertions.assertEquals(M, E - e * FastMath.sin(E), 2.0e-14);
605 }
606
607 for (double M = -6 * FastMath.PI; M < 6 * FastMath.PI; M += 0.01) {
608 KeplerianOrbit pAlmostParabolic =
609 new KeplerianOrbit(24464560.0, 0.9999, 2.1, 3.10686, 1.00681,
610 M, PositionAngleType.MEAN,
611 FramesFactory.getEME2000(), date, mu);
612 double E = pAlmostParabolic.getEccentricAnomaly();
613 double e = pAlmostParabolic.getE();
614 Assertions.assertEquals(M, E - e * FastMath.sin(E), 4.0e-13);
615 }
616
617 }
618
619 @Test
620 void testOutOfRangeV() {
621 Assertions.assertThrows(IllegalArgumentException.class, () -> {
622 new KeplerianOrbit(-7000434.460140012, 1.1999785407363386, 1.3962787004479158,
623 1.3962320168955138, 0.3490728321331678, -2.55593407037698,
624 PositionAngleType.TRUE, FramesFactory.getEME2000(),
625 new AbsoluteDate("2000-01-01T12:00:00.391", TimeScalesFactory.getUTC()),
626 3.986004415E14);
627 });
628 }
629
630 @Test
631 void testNumericalIssue25() {
632 Vector3D position = new Vector3D(3782116.14107698, 416663.11924914, 5875541.62103057);
633 Vector3D velocity = new Vector3D(-6349.7848910501, 288.4061811651, 4066.9366759691);
634 KeplerianOrbit orbit = new KeplerianOrbit(new PVCoordinates(position, velocity),
635 FramesFactory.getEME2000(),
636 new AbsoluteDate("2004-01-01T23:00:00.000",
637 TimeScalesFactory.getUTC()),
638 3.986004415E14);
639 Assertions.assertEquals(0.0, orbit.getE(), 2.0e-14);
640 }
641
642 @Test
643 void testPerfectlyEquatorial() {
644 Vector3D position = new Vector3D(6957904.3624652653594, 766529.11411558074507, 0);
645 Vector3D velocity = new Vector3D(-7538.2817012412102845, 342.38751001881413381, 0.);
646 KeplerianOrbit orbit = new KeplerianOrbit(new PVCoordinates(position, velocity),
647 FramesFactory.getEME2000(),
648 new AbsoluteDate("2004-01-01T23:00:00.000",
649 TimeScalesFactory.getUTC()),
650 3.986004415E14);
651 Assertions.assertEquals(0.0, orbit.getI(), 2.0e-14);
652 Assertions.assertEquals(0.0, orbit.getRightAscensionOfAscendingNode(), 2.0e-14);
653 }
654
655 @Test
656 void testJacobianReferenceEllipse() {
657
658 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
659 double mu = 3.986004415e+14;
660 KeplerianOrbit orbKep = new KeplerianOrbit(7000000.0, 0.01, FastMath.toRadians(80.), FastMath.toRadians(80.), FastMath.toRadians(20.),
661 FastMath.toRadians(40.), PositionAngleType.MEAN,
662 FramesFactory.getEME2000(), dateTca, mu);
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708 Vector3D pRef = new Vector3D(-3691555.569874833337963, -240330.253992714860942, 5879700.285850423388183);
709 Vector3D vRef = new Vector3D(-5936.229884450408463, -2871.067660163344044, -3786.209549192726627);
710 double[][] jRef = {
711 { -1.0792090588217809, -7.02594292049818631E-002, 1.7189029642216496, -1459.4829009393857, -705.88138246206040, -930.87838644776593 },
712 { -1.31195762636625214E-007, -3.90087231593959271E-008, 4.65917592901869866E-008, -2.02467187867647177E-004, -7.89767994436215424E-005, -2.81639203329454407E-005 },
713 { 4.18334478744371316E-008, -1.14936453412947957E-007, 2.15670500707930151E-008, -2.26450325965329431E-005, 6.22167157217876380E-005, -1.16745469637130306E-005 },
714 { 3.52735168061691945E-006, 3.82555734454450974E-006, 1.34715077236557634E-005, -8.06586262922115264E-003, -6.13725651685311825E-003, -1.71765290503914092E-002 },
715 { 2.48948022169790885E-008, -6.83979069529389238E-008, 1.28344057971888544E-008, 3.86597661353874888E-005, -1.06216834498373629E-004, 1.99308724078785540E-005 },
716 { -3.41911705254704525E-006, -3.75913623359912437E-006, -1.34013845492518465E-005, 8.19851888816422458E-003, 6.16449264680494959E-003, 1.69495878276556648E-002 }
717 };
718
719 PVCoordinates pv = orbKep.getPVCoordinates();
720 Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm(), 1.0e-15 * pRef.getNorm());
721 Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm(), 1.0e-16 * vRef.getNorm());
722
723 double[][] jacobian = new double[6][6];
724 orbKep.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
725
726 for (int i = 0; i < jacobian.length; i++) {
727 double[] row = jacobian[i];
728 double[] rowRef = jRef[i];
729 for (int j = 0; j < row.length; j++) {
730 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 2.0e-12);
731 }
732 }
733
734 }
735
736 @Test
737 void testJacobianFinitedifferencesEllipse() {
738
739 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
740 double mu = 3.986004415e+14;
741 KeplerianOrbit orbKep = new KeplerianOrbit(7000000.0, 0.01, FastMath.toRadians(80.), FastMath.toRadians(80.), FastMath.toRadians(20.),
742 FastMath.toRadians(40.), PositionAngleType.MEAN,
743 FramesFactory.getEME2000(), dateTca, mu);
744
745 for (PositionAngleType type : PositionAngleType.values()) {
746 double hP = 2.0;
747 double[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbKep, hP);
748 double[][] jacobian = new double[6][6];
749 orbKep.getJacobianWrtCartesian(type, jacobian);
750
751 for (int i = 0; i < jacobian.length; i++) {
752 double[] row = jacobian[i];
753 double[] rowRef = finiteDiffJacobian[i];
754 for (int j = 0; j < row.length; j++) {
755 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 2.0e-7);
756 }
757 }
758
759 double[][] invJacobian = new double[6][6];
760 orbKep.getJacobianWrtParameters(type, invJacobian);
761 MatrixUtils.createRealMatrix(jacobian).
762 multiply(MatrixUtils.createRealMatrix(invJacobian)).
763 walkInRowOrder(new RealMatrixPreservingVisitor() {
764 public void start(int rows, int columns,
765 int startRow, int endRow, int startColumn, int endColumn) {
766 }
767
768 public void visit(int row, int column, double value) {
769 Assertions.assertEquals(row == column ? 1.0 : 0.0, value, 5.0e-9);
770 }
771
772 public double end() {
773 return Double.NaN;
774 }
775 });
776
777 }
778
779 }
780
781 @Test
782 void testJacobianReferenceHyperbola() {
783
784 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
785 double mu = 3.986004415e+14;
786 KeplerianOrbit orbKep = new KeplerianOrbit(-7000000.0, 1.2, FastMath.toRadians(80.), FastMath.toRadians(80.), FastMath.toRadians(20.),
787 FastMath.toRadians(40.), PositionAngleType.MEAN,
788 FramesFactory.getEME2000(), dateTca, mu);
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837 Vector3D pRef = new Vector3D(-7654711.206549182534218, -3460171.872979687992483, -3592374.514463655184954);
838 Vector3D vRef = new Vector3D( -7886.368091820805603, -4359.739012331759113, -7937.060044548694350);
839 double[][] jRef = {
840 { -0.98364725131848019, -0.44463970750901238, -0.46162803814668391, -1938.9443476028839, -1071.8864775981751, -1951.4074832397598 },
841 { -1.10548813242982574E-007, -2.52906747183730431E-008, 7.96500937398593591E-008, -9.70479823470940108E-006, -2.93209076428001017E-005, -1.37434463892791042E-004 },
842 { 8.55737680891616672E-008, -2.35111995522618220E-007, 4.41171797903162743E-008, -8.05235180390949802E-005, 2.21236547547460423E-004, -4.15135455876865407E-005 },
843 { -1.52641427784095578E-007, 1.10250447958827901E-008, 1.21265251605359894E-007, 7.63347077200903542E-005, -3.54738331412232378E-005, -2.31400737283033359E-004 },
844 { 7.86711766048035274E-008, -2.16147281283624453E-007, 4.05585791077187359E-008, -3.56071805267582894E-005, 9.78299244677127374E-005, -1.83571253224293247E-005 },
845 { -2.41488884881911384E-007, -1.00119615610276537E-007, -6.51494225096757969E-008, -2.43295075073248163E-004, -1.43273725071890463E-004, -2.91625510452094873E-004 }
846 };
847
848 PVCoordinates pv = orbKep.getPVCoordinates();
849 Assertions.assertEquals(0, pv.getPosition().subtract(pRef).getNorm() / pRef.getNorm(), 1.0e-16);
850
851 Assertions.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm() / vRef.getNorm(), 3.0e-16);
852
853 double[][] jacobian = new double[6][6];
854 orbKep.getJacobianWrtCartesian(PositionAngleType.MEAN, jacobian);
855
856 for (int i = 0; i < jacobian.length; i++) {
857 double[] row = jacobian[i];
858 double[] rowRef = jRef[i];
859 for (int j = 0; j < row.length; j++) {
860 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 1.0e-14);
861 }
862 }
863
864 }
865
866 @Test
867 void testJacobianFinitedifferencesHyperbola() {
868
869 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
870 double mu = 3.986004415e+14;
871 KeplerianOrbit orbKep = new KeplerianOrbit(-7000000.0, 1.2, FastMath.toRadians(80.), FastMath.toRadians(80.), FastMath.toRadians(20.),
872 FastMath.toRadians(40.), PositionAngleType.MEAN,
873 FramesFactory.getEME2000(), dateTca, mu);
874
875 for (PositionAngleType type : PositionAngleType.values()) {
876 double hP = 2.0;
877 double[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbKep, hP);
878 double[][] jacobian = new double[6][6];
879 orbKep.getJacobianWrtCartesian(type, jacobian);
880 for (int i = 0; i < jacobian.length; i++) {
881 double[] row = jacobian[i];
882 double[] rowRef = finiteDiffJacobian[i];
883 for (int j = 0; j < row.length; j++) {
884 Assertions.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 3.0e-8);
885 }
886 }
887
888 double[][] invJacobian = new double[6][6];
889 orbKep.getJacobianWrtParameters(type, invJacobian);
890 MatrixUtils.createRealMatrix(jacobian).
891 multiply(MatrixUtils.createRealMatrix(invJacobian)).
892 walkInRowOrder(new RealMatrixPreservingVisitor() {
893 public void start(int rows, int columns,
894 int startRow, int endRow, int startColumn, int endColumn) {
895 }
896
897 public void visit(int row, int column, double value) {
898 Assertions.assertEquals(row == column ? 1.0 : 0.0, value, 2.0e-8);
899 }
900
901 public double end() {
902 return Double.NaN;
903 }
904 });
905
906 }
907
908 }
909
910 private double[][] finiteDifferencesJacobian(PositionAngleType type, KeplerianOrbit orbit, double hP)
911 {
912 double[][] jacobian = new double[6][6];
913 for (int i = 0; i < 6; ++i) {
914 fillColumn(type, i, orbit, hP, jacobian);
915 }
916 return jacobian;
917 }
918
919 private void fillColumn(PositionAngleType type, int i, KeplerianOrbit orbit, double hP, double[][] jacobian) {
920
921
922
923 Vector3D p = orbit.getPosition();
924 Vector3D v = orbit.getPVCoordinates().getVelocity();
925 double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq());
926
927 double h;
928 Vector3D dP = Vector3D.ZERO;
929 Vector3D dV = Vector3D.ZERO;
930 switch (i) {
931 case 0:
932 h = hP;
933 dP = new Vector3D(hP, 0, 0);
934 break;
935 case 1:
936 h = hP;
937 dP = new Vector3D(0, hP, 0);
938 break;
939 case 2:
940 h = hP;
941 dP = new Vector3D(0, 0, hP);
942 break;
943 case 3:
944 h = hV;
945 dV = new Vector3D(hV, 0, 0);
946 break;
947 case 4:
948 h = hV;
949 dV = new Vector3D(0, hV, 0);
950 break;
951 default:
952 h = hV;
953 dV = new Vector3D(0, 0, hV);
954 break;
955 }
956
957 KeplerianOrbit oM4h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)),
958 orbit.getFrame(), orbit.getDate(), orbit.getMu());
959 KeplerianOrbit oM3h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)),
960 orbit.getFrame(), orbit.getDate(), orbit.getMu());
961 KeplerianOrbit oM2h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)),
962 orbit.getFrame(), orbit.getDate(), orbit.getMu());
963 KeplerianOrbit oM1h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)),
964 orbit.getFrame(), orbit.getDate(), orbit.getMu());
965 KeplerianOrbit oP1h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)),
966 orbit.getFrame(), orbit.getDate(), orbit.getMu());
967 KeplerianOrbit oP2h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)),
968 orbit.getFrame(), orbit.getDate(), orbit.getMu());
969 KeplerianOrbit oP3h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)),
970 orbit.getFrame(), orbit.getDate(), orbit.getMu());
971 KeplerianOrbit oP4h = new KeplerianOrbit(new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)),
972 orbit.getFrame(), orbit.getDate(), orbit.getMu());
973
974 jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) +
975 32 * (oP3h.getA() - oM3h.getA()) -
976 168 * (oP2h.getA() - oM2h.getA()) +
977 672 * (oP1h.getA() - oM1h.getA())) / (840 * h);
978 jacobian[1][i] = (-3 * (oP4h.getE() - oM4h.getE()) +
979 32 * (oP3h.getE() - oM3h.getE()) -
980 168 * (oP2h.getE() - oM2h.getE()) +
981 672 * (oP1h.getE() - oM1h.getE())) / (840 * h);
982 jacobian[2][i] = (-3 * (oP4h.getI() - oM4h.getI()) +
983 32 * (oP3h.getI() - oM3h.getI()) -
984 168 * (oP2h.getI() - oM2h.getI()) +
985 672 * (oP1h.getI() - oM1h.getI())) / (840 * h);
986 jacobian[3][i] = (-3 * (oP4h.getPerigeeArgument() - oM4h.getPerigeeArgument()) +
987 32 * (oP3h.getPerigeeArgument() - oM3h.getPerigeeArgument()) -
988 168 * (oP2h.getPerigeeArgument() - oM2h.getPerigeeArgument()) +
989 672 * (oP1h.getPerigeeArgument() - oM1h.getPerigeeArgument())) / (840 * h);
990 jacobian[4][i] = (-3 * (oP4h.getRightAscensionOfAscendingNode() - oM4h.getRightAscensionOfAscendingNode()) +
991 32 * (oP3h.getRightAscensionOfAscendingNode() - oM3h.getRightAscensionOfAscendingNode()) -
992 168 * (oP2h.getRightAscensionOfAscendingNode() - oM2h.getRightAscensionOfAscendingNode()) +
993 672 * (oP1h.getRightAscensionOfAscendingNode() - oM1h.getRightAscensionOfAscendingNode())) / (840 * h);
994 jacobian[5][i] = (-3 * (oP4h.getAnomaly(type) - oM4h.getAnomaly(type)) +
995 32 * (oP3h.getAnomaly(type) - oM3h.getAnomaly(type)) -
996 168 * (oP2h.getAnomaly(type) - oM2h.getAnomaly(type)) +
997 672 * (oP1h.getAnomaly(type) - oM1h.getAnomaly(type))) / (840 * h);
998
999 }
1000
1001 @Test
1002 void testPerfectlyEquatorialConversion() {
1003 KeplerianOrbit initial = new KeplerianOrbit(13378000.0, 0.05, 0.0, 0.0, FastMath.PI,
1004 0.0, PositionAngleType.MEAN,
1005 FramesFactory.getEME2000(), date,
1006 Constants.EIGEN5C_EARTH_MU);
1007 EquinoctialOrbit equ = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(initial);
1008 KeplerianOrbit converted = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(equ);
1009 Assertions.assertEquals(FastMath.PI,
1010 MathUtils.normalizeAngle(converted.getRightAscensionOfAscendingNode() +
1011 converted.getPerigeeArgument(), FastMath.PI),
1012 1.0e-10);
1013 }
1014
1015 @Test
1016 void testKeplerianDerivatives() {
1017
1018 final KeplerianOrbit orbit = new KeplerianOrbit(new PVCoordinates(new Vector3D(-4947831., -3765382., -3708221.),
1019 new Vector3D(-2079., 5291., -7842.)),
1020 FramesFactory.getEME2000(), date, 3.9860047e14);
1021 final Vector3D p = orbit.getPosition();
1022 final Vector3D v = orbit.getPVCoordinates().getVelocity();
1023 final Vector3D a = orbit.getPVCoordinates().getAcceleration();
1024
1025
1026 Assertions.assertEquals(7.605422, a.getNorm(), 1.0e-6);
1027
1028
1029 final double tolerance = 3e-10;
1030 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getX()),
1031 orbit.getPVCoordinates().getVelocity().getX(),
1032 tolerance * v.getNorm());
1033 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getY()),
1034 orbit.getPVCoordinates().getVelocity().getY(),
1035 tolerance * v.getNorm());
1036 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPosition().getZ()),
1037 orbit.getPVCoordinates().getVelocity().getZ(),
1038 tolerance * v.getNorm());
1039
1040
1041 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getVelocity().getX()),
1042 orbit.getPVCoordinates().getAcceleration().getX(),
1043 tolerance * a.getNorm());
1044 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getVelocity().getY()),
1045 orbit.getPVCoordinates().getAcceleration().getY(),
1046 tolerance * a.getNorm());
1047 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getVelocity().getZ()),
1048 orbit.getPVCoordinates().getAcceleration().getZ(),
1049 tolerance * a.getNorm());
1050
1051
1052 final double r2 = p.getNormSq();
1053 final double r = FastMath.sqrt(r2);
1054 Vector3D keplerianJerk = new Vector3D(-3 * Vector3D.dotProduct(p, v) / r2, a, -a.getNorm() / r, v);
1055 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getX()),
1056 keplerianJerk.getX(),
1057 3.0e-12 * keplerianJerk.getNorm());
1058 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getY()),
1059 keplerianJerk.getY(),
1060 3.0e-12 * keplerianJerk.getNorm());
1061 Assertions.assertEquals(differentiate(orbit, shifted -> shifted.getPVCoordinates().getAcceleration().getZ()),
1062 keplerianJerk.getZ(),
1063 3.0e-12 * keplerianJerk.getNorm());
1064
1065 Assertions.assertEquals(0., orbit.getADot());
1066 Assertions.assertEquals(0., orbit.getEquinoctialExDot());
1067 Assertions.assertEquals(0., orbit.getEquinoctialEyDot());
1068 Assertions.assertEquals(0., orbit.getHxDot());
1069 Assertions.assertEquals(0., orbit.getHyDot());
1070 Assertions.assertEquals(0., orbit.getEDot());
1071 Assertions.assertEquals(0., orbit.getIDot());
1072 Assertions.assertEquals(0., orbit.getPerigeeArgumentDot());
1073 Assertions.assertEquals(0., orbit.getRightAscensionOfAscendingNodeDot());
1074
1075 }
1076
1077 private <S extends Function<KeplerianOrbit, Double>>
1078 double differentiate(KeplerianOrbit orbit, S picker) {
1079 final DSFactory factory = new DSFactory(1, 1);
1080 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
1081 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
1082 public double value(double dt) {
1083 return picker.apply(orbit.shiftedBy(dt));
1084 }
1085 });
1086 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
1087 }
1088
1089 @Test
1090 void testNonKeplerianEllipticDerivatives() {
1091 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1092 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
1093 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
1094 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
1095 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
1096 final Frame frame = FramesFactory.getEME2000();
1097 final double mu = Constants.EIGEN5C_EARTH_MU;
1098 final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, mu);
1099
1100 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
1101 orbit.getADot(),
1102 4.3e-8);
1103 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
1104 orbit.getEquinoctialExDot(),
1105 2.1e-15);
1106 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
1107 orbit.getEquinoctialEyDot(),
1108 5.3e-16);
1109 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
1110 orbit.getHxDot(),
1111 1.6e-15);
1112 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
1113 orbit.getHyDot(),
1114 7.3e-17);
1115 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
1116 orbit.getLvDot(),
1117 1.1e-14);
1118 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
1119 orbit.getLEDot(),
1120 7.2e-15);
1121 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
1122 orbit.getLMDot(),
1123 4.7e-15);
1124 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
1125 orbit.getEDot(),
1126 6.9e-16);
1127 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
1128 orbit.getIDot(),
1129 5.8e-16);
1130 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getPerigeeArgument()),
1131 orbit.getPerigeeArgumentDot(),
1132 1.5e-12);
1133 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getRightAscensionOfAscendingNode()),
1134 orbit.getRightAscensionOfAscendingNodeDot(),
1135 1.5e-15);
1136 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getTrueAnomaly()),
1137 orbit.getTrueAnomalyDot(),
1138 1.5e-12);
1139 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEccentricAnomaly()),
1140 orbit.getEccentricAnomalyDot(),
1141 1.5e-12);
1142 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getMeanAnomaly()),
1143 orbit.getMeanAnomalyDot(),
1144 1.5e-12);
1145 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.TRUE)),
1146 orbit.getAnomalyDot(PositionAngleType.TRUE),
1147 1.5e-12);
1148 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.ECCENTRIC)),
1149 orbit.getAnomalyDot(PositionAngleType.ECCENTRIC),
1150 1.5e-12);
1151 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.MEAN)),
1152 orbit.getAnomalyDot(PositionAngleType.MEAN),
1153 1.5e-12);
1154
1155 }
1156
1157 @Test
1158 void testNonKeplerianHyperbolicDerivatives() {
1159 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1160 final Vector3D position = new Vector3D(224267911.905821, 290251613.109399, 45534292.777492);
1161 final Vector3D velocity = new Vector3D(-1494.068165293, 1124.771027677, 526.915286134);
1162 final Vector3D acceleration = new Vector3D(-0.001295920501, -0.002233045187, -0.000349906292);
1163 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
1164 final Frame frame = FramesFactory.getEME2000();
1165 final double mu = Constants.EIGEN5C_EARTH_MU;
1166 final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, mu);
1167
1168 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
1169 orbit.getADot(),
1170 9.6e-8);
1171 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
1172 orbit.getEquinoctialExDot(),
1173 2.8e-16);
1174 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
1175 orbit.getEquinoctialEyDot(),
1176 3.6e-15);
1177 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
1178 orbit.getHxDot(),
1179 1.4e-15);
1180 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
1181 orbit.getHyDot(),
1182 9.4e-16);
1183 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
1184 orbit.getLvDot(),
1185 5.6e-16);
1186 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
1187 orbit.getLEDot(),
1188 9.0e-16);
1189 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
1190 orbit.getLMDot(),
1191 1.8e-15);
1192 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
1193 orbit.getEDot(),
1194 1.8e-15);
1195 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
1196 orbit.getIDot(),
1197 3.6e-15);
1198 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getPerigeeArgument()),
1199 orbit.getPerigeeArgumentDot(),
1200 9.4e-16);
1201 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getRightAscensionOfAscendingNode()),
1202 orbit.getRightAscensionOfAscendingNodeDot(),
1203 1.1e-15);
1204 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getTrueAnomaly()),
1205 orbit.getTrueAnomalyDot(),
1206 1.4e-15);
1207 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEccentricAnomaly()),
1208 orbit.getEccentricAnomalyDot(),
1209 9.2e-16);
1210 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getMeanAnomaly()),
1211 orbit.getMeanAnomalyDot(),
1212 1.4e-15);
1213 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.TRUE)),
1214 orbit.getAnomalyDot(PositionAngleType.TRUE),
1215 1.4e-15);
1216 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.ECCENTRIC)),
1217 orbit.getAnomalyDot(PositionAngleType.ECCENTRIC),
1218 9.2e-16);
1219 Assertions.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getAnomaly(PositionAngleType.MEAN)),
1220 orbit.getAnomalyDot(PositionAngleType.MEAN),
1221 1.4e-15);
1222
1223 }
1224
1225 private <S extends Function<KeplerianOrbit, Double>>
1226 double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
1227 final DSFactory factory = new DSFactory(1, 1);
1228 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
1229 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
1230 public double value(double dt) {
1231 return picker.apply(new KeplerianOrbit(pv.shiftedBy(dt), frame, mu));
1232 }
1233 });
1234 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
1235 }
1236
1237 @Test
1238 void testPositionAngleDerivatives() {
1239 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1240 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
1241 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
1242 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
1243 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
1244 final Frame frame = FramesFactory.getEME2000();
1245 final double mu = Constants.EIGEN5C_EARTH_MU;
1246 final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, mu);
1247
1248 for (PositionAngleType type : PositionAngleType.values()) {
1249 final KeplerianOrbit rebuilt = new KeplerianOrbit(orbit.getA(),
1250 orbit.getE(),
1251 orbit.getI(),
1252 orbit.getPerigeeArgument(),
1253 orbit.getRightAscensionOfAscendingNode(),
1254 orbit.getAnomaly(type),
1255 orbit.getADot(),
1256 orbit.getEDot(),
1257 orbit.getIDot(),
1258 orbit.getPerigeeArgumentDot(),
1259 orbit.getRightAscensionOfAscendingNodeDot(),
1260 orbit.getAnomalyDot(type),
1261 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1262 MatcherAssert.assertThat(rebuilt.getA(), relativelyCloseTo(orbit.getA(), 1));
1263 MatcherAssert.assertThat(rebuilt.getE(), relativelyCloseTo(orbit.getE(), 1));
1264 MatcherAssert.assertThat(rebuilt.getI(), relativelyCloseTo(orbit.getI(), 1));
1265 MatcherAssert.assertThat(rebuilt.getPerigeeArgument(), relativelyCloseTo(orbit.getPerigeeArgument(), 1));
1266 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNode(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNode(), 1));
1267 MatcherAssert.assertThat(rebuilt.getADot(), relativelyCloseTo(orbit.getADot(), 1));
1268 MatcherAssert.assertThat(rebuilt.getEDot(), relativelyCloseTo(orbit.getEDot(), 1));
1269 MatcherAssert.assertThat(rebuilt.getIDot(), relativelyCloseTo(orbit.getIDot(), 1));
1270 MatcherAssert.assertThat(rebuilt.getPerigeeArgumentDot(), relativelyCloseTo(orbit.getPerigeeArgumentDot(), 1));
1271 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNodeDot(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNodeDot(), 1));
1272 for (PositionAngleType type2 : PositionAngleType.values()) {
1273 MatcherAssert.assertThat(rebuilt.getAnomaly(type2), relativelyCloseTo(orbit.getAnomaly(type2), 1));
1274 MatcherAssert.assertThat(rebuilt.getAnomalyDot(type2), relativelyCloseTo(orbit.getAnomalyDot(type2), 1));
1275 }
1276 }
1277
1278 }
1279
1280 @Test
1281 void testPositionAngleHyperbolicDerivatives() {
1282 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1283 final Vector3D position = new Vector3D(224267911.905821, 290251613.109399, 45534292.777492);
1284 final Vector3D velocity = new Vector3D(-1494.068165293, 1124.771027677, 526.915286134);
1285 final Vector3D acceleration = new Vector3D(-0.001295920501, -0.002233045187, -0.000349906292);
1286 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
1287 final Frame frame = FramesFactory.getEME2000();
1288 final double mu = Constants.EIGEN5C_EARTH_MU;
1289 final KeplerianOrbit orbit = new KeplerianOrbit(pv, frame, mu);
1290
1291 for (PositionAngleType type : PositionAngleType.values()) {
1292 final KeplerianOrbit rebuilt = new KeplerianOrbit(orbit.getA(),
1293 orbit.getE(),
1294 orbit.getI(),
1295 orbit.getPerigeeArgument(),
1296 orbit.getRightAscensionOfAscendingNode(),
1297 orbit.getAnomaly(type),
1298 orbit.getADot(),
1299 orbit.getEDot(),
1300 orbit.getIDot(),
1301 orbit.getPerigeeArgumentDot(),
1302 orbit.getRightAscensionOfAscendingNodeDot(),
1303 orbit.getAnomalyDot(type),
1304 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1305 MatcherAssert.assertThat(rebuilt.getA(), relativelyCloseTo(orbit.getA(), 1));
1306 MatcherAssert.assertThat(rebuilt.getE(), relativelyCloseTo(orbit.getE(), 1));
1307 MatcherAssert.assertThat(rebuilt.getI(), relativelyCloseTo(orbit.getI(), 1));
1308 MatcherAssert.assertThat(rebuilt.getPerigeeArgument(), relativelyCloseTo(orbit.getPerigeeArgument(), 1));
1309 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNode(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNode(), 1));
1310 MatcherAssert.assertThat(rebuilt.getADot(), relativelyCloseTo(orbit.getADot(), 1));
1311 MatcherAssert.assertThat(rebuilt.getEDot(), relativelyCloseTo(orbit.getEDot(), 1));
1312 MatcherAssert.assertThat(rebuilt.getIDot(), relativelyCloseTo(orbit.getIDot(), 1));
1313 MatcherAssert.assertThat(rebuilt.getPerigeeArgumentDot(), relativelyCloseTo(orbit.getPerigeeArgumentDot(), 1));
1314 MatcherAssert.assertThat(rebuilt.getRightAscensionOfAscendingNodeDot(), relativelyCloseTo(orbit.getRightAscensionOfAscendingNodeDot(), 1));
1315 for (PositionAngleType type2 : PositionAngleType.values()) {
1316 MatcherAssert.assertThat(rebuilt.getAnomaly(type2), relativelyCloseTo(orbit.getAnomaly(type2), 3));
1317 MatcherAssert.assertThat(rebuilt.getAnomalyDot(type2), relativelyCloseTo(orbit.getAnomalyDot(type2), 4));
1318 }
1319 }
1320
1321 }
1322
1323 @Test
1324 void testEquatorialRetrograde() {
1325 Vector3D position = new Vector3D(10000000.0, 0.0, 0.0);
1326 Vector3D velocity = new Vector3D(0.0, -6500.0, 1.0e-10);
1327 double r2 = position.getNormSq();
1328 double r = FastMath.sqrt(r2);
1329 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
1330 1, new Vector3D(-0.1, 0.2, 0.3));
1331 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
1332 KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
1333 Assertions.assertEquals(10637829.465, orbit.getA(), 1.0e-3);
1334 Assertions.assertEquals(-738.145, orbit.getADot(), 1.0e-3);
1335 Assertions.assertEquals(0.05995861, orbit.getE(), 1.0e-8);
1336 Assertions.assertEquals(-6.523e-5, orbit.getEDot(), 1.0e-8);
1337 Assertions.assertEquals(FastMath.PI, orbit.getI(), 2.0e-14);
1338 Assertions.assertEquals(-4.615e-5, orbit.getIDot(), 1.0e-8);
1339 Assertions.assertTrue(Double.isNaN(orbit.getHx()));
1340 Assertions.assertTrue(Double.isNaN(orbit.getHxDot()));
1341 Assertions.assertTrue(Double.isNaN(orbit.getHy()));
1342 Assertions.assertTrue(Double.isNaN(orbit.getHyDot()));
1343 }
1344
1345 @Test
1346 void testDerivativesConversionSymmetry() {
1347 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
1348 Vector3D position = new Vector3D(6893443.400234382, 1886406.1073757345, -589265.1150359757);
1349 Vector3D velocity = new Vector3D(-281.1261461082365, -1231.6165642450928, -7348.756363469432);
1350 Vector3D acceleration = new Vector3D(-7.460341170581685, -2.0415957334584527, 0.6393322823627762);
1351 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
1352 KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(),
1353 date, Constants.EIGEN5C_EARTH_MU);
1354 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1355 double r2 = position.getNormSq();
1356 double r = FastMath.sqrt(r2);
1357 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
1358 Assertions.assertEquals(0.0101, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-4);
1359
1360 for (OrbitType type : OrbitType.values()) {
1361 Orbit converted = type.convertType(orbit);
1362 Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
1363 KeplerianOrbit rebuilt = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(converted);
1364 Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
1365 Assertions.assertEquals(orbit.getADot(), rebuilt.getADot(), 3.0e-13);
1366 Assertions.assertEquals(orbit.getEDot(), rebuilt.getEDot(), 1.0e-15);
1367 Assertions.assertEquals(orbit.getIDot(), rebuilt.getIDot(), 1.0e-15);
1368 Assertions.assertEquals(orbit.getPerigeeArgumentDot(), rebuilt.getPerigeeArgumentDot(), 1.0e-15);
1369 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNodeDot(), rebuilt.getRightAscensionOfAscendingNodeDot(), 1.0e-15);
1370 Assertions.assertEquals(orbit.getTrueAnomalyDot(), rebuilt.getTrueAnomalyDot(), 1.0e-15);
1371 }
1372
1373 }
1374
1375 @Test
1376 void testDerivativesConversionSymmetryHyperbolic() {
1377 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
1378 final Vector3D position = new Vector3D(224267911.905821, 290251613.109399, 45534292.777492);
1379 final Vector3D velocity = new Vector3D(-1494.068165293, 1124.771027677, 526.915286134);
1380 final Vector3D acceleration = new Vector3D(-0.001295920501, -0.002233045187, -0.000349906292);
1381 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
1382 KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(),
1383 date, Constants.EIGEN5C_EARTH_MU);
1384 Assertions.assertTrue(orbit.hasNonKeplerianAcceleration());
1385 double r2 = position.getNormSq();
1386 double r = FastMath.sqrt(r2);
1387 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
1388 Assertions.assertEquals(4.78e-4, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-6);
1389
1390 OrbitType type = OrbitType.CARTESIAN;
1391 Orbit converted = type.convertType(orbit);
1392 Assertions.assertTrue(converted.hasNonKeplerianAcceleration());
1393 KeplerianOrbit rebuilt = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(converted);
1394 Assertions.assertTrue(rebuilt.hasNonKeplerianAcceleration());
1395 Assertions.assertEquals(orbit.getADot(), rebuilt.getADot(), 3.0e-13);
1396 Assertions.assertEquals(orbit.getEDot(), rebuilt.getEDot(), 1.0e-15);
1397 Assertions.assertEquals(orbit.getIDot(), rebuilt.getIDot(), 1.0e-15);
1398 Assertions.assertEquals(orbit.getPerigeeArgumentDot(), rebuilt.getPerigeeArgumentDot(), 1.0e-15);
1399 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNodeDot(), rebuilt.getRightAscensionOfAscendingNodeDot(), 1.0e-15);
1400 Assertions.assertEquals(orbit.getTrueAnomalyDot(), rebuilt.getTrueAnomalyDot(), 1.0e-15);
1401
1402 }
1403
1404 @Test
1405 void testToString() {
1406 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
1407 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
1408 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
1409 KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
1410 Assertions.assertEquals("Keplerian parameters: {a: 4.225517000282565E7; e: 0.002146216321416967; i: 0.20189257051515358; pa: 13.949966363606599; raan: -87.91788415673473; v: -151.79096272977213;}",
1411 orbit.toString());
1412 }
1413
1414 @Test
1415 void testWithKeplerianRates() {
1416
1417 final Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
1418 final Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
1419 final PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
1420 final KeplerianOrbit orbit = new KeplerianOrbit(pvCoordinates, FramesFactory.getGCRF(), date, mu);
1421
1422 final KeplerianOrbit orbitWithoutRates = orbit.withKeplerianRates();
1423
1424 Assertions.assertFalse(orbitWithoutRates.hasNonKeplerianRates());
1425 Assertions.assertEquals(orbit.getMu(), orbitWithoutRates.getMu());
1426 Assertions.assertEquals(orbit.getDate(), orbitWithoutRates.getDate());
1427 Assertions.assertEquals(orbit.getFrame(), orbitWithoutRates.getFrame());
1428 Assertions.assertEquals(orbit.getA(), orbitWithoutRates.getA());
1429 Assertions.assertEquals(orbit.getE(), orbitWithoutRates.getE());
1430 Assertions.assertEquals(orbit.getI(), orbitWithoutRates.getI());
1431 Assertions.assertEquals(orbit.getRightAscensionOfAscendingNode(),
1432 orbitWithoutRates.getRightAscensionOfAscendingNode());
1433 Assertions.assertEquals(orbit.getPerigeeArgument(), orbitWithoutRates.getPerigeeArgument());
1434 Assertions.assertEquals(orbit.getTrueAnomaly(), orbitWithoutRates.getTrueAnomaly());
1435 }
1436
1437 @Test
1438 void testCopyNonKeplerianAcceleration() {
1439
1440 final Frame eme2000 = FramesFactory.getEME2000();
1441
1442
1443 final Vector3D position = new Vector3D(42164140, 0, 0);
1444
1445 final PVCoordinates pv = new PVCoordinates(position,
1446 new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
1447
1448 final Orbit orbit = new KeplerianOrbit(pv, eme2000, date, mu);
1449
1450
1451 final Orbit orbitCopy = new KeplerianOrbit(orbit);
1452
1453
1454 final Orbit shiftedOrbit = orbit.shiftedBy(10);
1455 final Orbit shiftedOrbitCopy = orbitCopy.shiftedBy(10);
1456
1457 Assertions.assertEquals(0.0,
1458 Vector3D.distance(shiftedOrbit.getPosition(),
1459 shiftedOrbitCopy.getPosition()),
1460 1.0e-10);
1461 Assertions.assertEquals(0.0,
1462 Vector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
1463 shiftedOrbitCopy.getPVCoordinates().getVelocity()),
1464 1.0e-10);
1465
1466 }
1467
1468 @Test
1469 void testIssue674() {
1470 try {
1471 new KeplerianOrbit(24464560.0, -0.7311, 0.122138, 3.10686, 1.00681,
1472 0.048363, PositionAngleType.MEAN,
1473 FramesFactory.getEME2000(), date, mu);
1474 Assertions.fail("an exception should have been thrown");
1475 } catch (OrekitException oe) {
1476 Assertions.assertEquals(OrekitMessages.INVALID_PARAMETER_RANGE, oe.getSpecifier());
1477 Assertions.assertEquals("eccentricity", oe.getParts()[0]);
1478 Assertions.assertEquals(-0.7311, ((Double) oe.getParts()[1]).doubleValue(), Double.MIN_VALUE);
1479 Assertions.assertEquals(0.0, ((Double) oe.getParts()[2]).doubleValue(), Double.MIN_VALUE);
1480 Assertions.assertEquals(Double.POSITIVE_INFINITY, ((Double) oe.getParts()[3]).doubleValue(), Double.MIN_VALUE);
1481 }
1482 }
1483
1484 @Test
1485 void testNormalize() {
1486 KeplerianOrbit withoutDerivatives =
1487 new KeplerianOrbit(42166712.0, 0.005, 1.6,
1488 -0.3, 1.25, 0.4, PositionAngleType.MEAN,
1489 FramesFactory.getEME2000(), date, mu);
1490 KeplerianOrbit ref =
1491 new KeplerianOrbit(24000000.0, 0.012, 1.9,
1492 -6.28, 6.28, 12.56, PositionAngleType.MEAN,
1493 FramesFactory.getEME2000(), date, mu);
1494
1495 KeplerianOrbit normalized1 = (KeplerianOrbit) OrbitType.KEPLERIAN.normalize(withoutDerivatives, ref);
1496 Assertions.assertFalse(normalized1.hasNonKeplerianAcceleration());
1497 Assertions.assertEquals(0.0, normalized1.getA() - withoutDerivatives.getA(), 1.0e-6);
1498 Assertions.assertEquals(0.0, normalized1.getE() - withoutDerivatives.getE(), 1.0e-10);
1499 Assertions.assertEquals(0.0, normalized1.getI() - withoutDerivatives.getI(), 1.0e-10);
1500 Assertions.assertEquals(-MathUtils.TWO_PI, normalized1.getPerigeeArgument() - withoutDerivatives.getPerigeeArgument(), 1.0e-10);
1501 Assertions.assertEquals(+MathUtils.TWO_PI, normalized1.getRightAscensionOfAscendingNode() - withoutDerivatives.getRightAscensionOfAscendingNode(), 1.0e-10);
1502 Assertions.assertEquals(2 * MathUtils.TWO_PI, normalized1.getTrueAnomaly() - withoutDerivatives.getTrueAnomaly(), 1.0e-10);
1503 Assertions.assertEquals(withoutDerivatives.getADot(), normalized1.getADot());
1504 Assertions.assertEquals(withoutDerivatives.getEDot(), normalized1.getEDot());
1505 Assertions.assertEquals(withoutDerivatives.getIDot(), normalized1.getIDot());
1506 Assertions.assertEquals(withoutDerivatives.getPerigeeArgumentDot(), normalized1.getPerigeeArgumentDot());
1507 Assertions.assertEquals(withoutDerivatives.getRightAscensionOfAscendingNodeDot(), normalized1.getRightAscensionOfAscendingNodeDot());
1508 Assertions.assertEquals(withoutDerivatives.getTrueAnomalyDot(), normalized1.getTrueAnomalyDot());
1509
1510 double[] p = new double[6];
1511 OrbitType.KEPLERIAN.mapOrbitToArray(withoutDerivatives, PositionAngleType.TRUE, p, null);
1512 KeplerianOrbit withDerivatives = (KeplerianOrbit) OrbitType.KEPLERIAN.mapArrayToOrbit(p,
1513 new double[] { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 },
1514 PositionAngleType.TRUE,
1515 withoutDerivatives.getDate(),
1516 withoutDerivatives.getMu(),
1517 withoutDerivatives.getFrame());
1518 KeplerianOrbit normalized2 = (KeplerianOrbit) OrbitType.KEPLERIAN.normalize(withDerivatives, ref);
1519 Assertions.assertTrue(normalized2.hasNonKeplerianAcceleration());
1520 Assertions.assertEquals(0.0, normalized2.getA() - withoutDerivatives.getA(), 1.0e-6);
1521 Assertions.assertEquals(0.0, normalized2.getE() - withoutDerivatives.getE(), 1.0e-10);
1522 Assertions.assertEquals(0.0, normalized2.getI() - withoutDerivatives.getI(), 1.0e-10);
1523 Assertions.assertEquals(-MathUtils.TWO_PI, normalized2.getPerigeeArgument() - withoutDerivatives.getPerigeeArgument(), 1.0e-10);
1524 Assertions.assertEquals(+MathUtils.TWO_PI, normalized2.getRightAscensionOfAscendingNode() - withoutDerivatives.getRightAscensionOfAscendingNode(), 1.0e-10);
1525 Assertions.assertEquals(2 * MathUtils.TWO_PI, normalized2.getTrueAnomaly() - withoutDerivatives.getTrueAnomaly(), 1.0e-10);
1526 Assertions.assertEquals(0.0, normalized2.getADot() - withDerivatives.getADot(), 1.0e-10);
1527 Assertions.assertEquals(0.0, normalized2.getEDot() - withDerivatives.getEDot(), 1.0e-10);
1528 Assertions.assertEquals(0.0, normalized2.getIDot() - withDerivatives.getIDot(), 1.0e-10);
1529 Assertions.assertEquals(0.0, normalized2.getPerigeeArgumentDot() - withDerivatives.getPerigeeArgumentDot(), 1.0e-10);
1530 Assertions.assertEquals(0.0, normalized2.getRightAscensionOfAscendingNodeDot() - withDerivatives.getRightAscensionOfAscendingNodeDot(), 1.0e-10);
1531 Assertions.assertEquals(0.0, normalized2.getTrueAnomalyDot() - withDerivatives.getTrueAnomalyDot(), 1.0e-10);
1532
1533 }
1534
1535 @Test
1536 void testKeplerianToPvToKeplerian() {
1537
1538 Frame eci = FramesFactory.getGCRF();
1539 AbsoluteDate date = AbsoluteDate.ARBITRARY_EPOCH;
1540 double mu = Constants.EGM96_EARTH_MU;
1541 KeplerianOrbit orbit = new KeplerianOrbit(6878e3, 0, 0, 0, 0, 0,
1542 PositionAngleType.TRUE, eci, date, mu);
1543
1544
1545 KeplerianOrbit pvOrbit = new KeplerianOrbit(orbit.getPVCoordinates(), eci, mu);
1546
1547
1548 Assertions.assertEquals(orbit.shiftedBy(1).getA(), pvOrbit.shiftedBy(1).getA(), 1e-6);
1549 Assertions.assertEquals(orbit.shiftedBy(1).getE(), pvOrbit.shiftedBy(1).getE(), 1e-14);
1550 Assertions.assertEquals(orbit.shiftedBy(1).getI(), pvOrbit.shiftedBy(1).getI(), 1e-10);
1551 Assertions.assertEquals(orbit.shiftedBy(1).getPerigeeArgument(), pvOrbit.shiftedBy(1).getPerigeeArgument(), 1e-10);
1552 Assertions.assertEquals(orbit.shiftedBy(1).getRightAscensionOfAscendingNode(), pvOrbit.shiftedBy(1).getRightAscensionOfAscendingNode(), 1e-10);
1553 Assertions.assertEquals(orbit.shiftedBy(1).getTrueAnomaly(), pvOrbit.shiftedBy(1).getTrueAnomaly(), 1e-10);
1554
1555 }
1556
1557 @Test
1558 void testCoverageCachedPositionAngleTypeElliptic() {
1559 testCoverageCachedPositionAngleType(1e4, 0.5);
1560 }
1561
1562 @Test
1563 void testCoverageCachedPositionAngleTypeHyperbolic() {
1564 testCoverageCachedPositionAngleType(-1e4, 2);
1565 }
1566
1567 private void testCoverageCachedPositionAngleType(final double a, final double e) {
1568
1569 final double expectedAnomaly = 0.;
1570
1571 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
1572 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1573 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(a, e, 0., 0., 0.,
1574 expectedAnomaly, inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1575 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getTrueAnomaly());
1576 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getEccentricAnomaly());
1577 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getMeanAnomaly());
1578 }
1579 }
1580 }
1581
1582 @Test
1583 void testCoverageCachedPositionAngleTypeWithRates() {
1584
1585 final double semiMajorAxis = 1e4;
1586 final double eccentricity = 0.;
1587 final double expectedAnomaly = 0.;
1588 final double expectedAnomalyDot = 0.;
1589
1590 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
1591 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1592 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(semiMajorAxis, eccentricity, 0., 0., 0.,
1593 expectedAnomaly, 0., 0., 0., 0., 0., expectedAnomalyDot,
1594 inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1595 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getTrueAnomaly());
1596 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getEccentricAnomaly());
1597 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getMeanAnomaly());
1598 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getTrueAnomalyDot());
1599 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getEccentricAnomalyDot());
1600 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getMeanAnomalyDot());
1601 }
1602 }
1603 }
1604
1605 @Test
1606 void testCoverageCachedPositionAngleTypeWithRatesHyperbolic() {
1607
1608 final double semiMajorAxis = -1e4;
1609 final double eccentricity = 2.;
1610 final double expectedAnomaly = 0.;
1611 final double expectedAnomalyDot = 0.;
1612
1613 for (final PositionAngleType inputPositionAngleType: PositionAngleType.values()) {
1614 for (final PositionAngleType cachedPositionAngleType: PositionAngleType.values()) {
1615 final KeplerianOrbit keplerianOrbit = new KeplerianOrbit(semiMajorAxis, eccentricity, 0., 0., 0.,
1616 expectedAnomaly, 0., 0., 0., 0., 0., expectedAnomalyDot,
1617 inputPositionAngleType, cachedPositionAngleType, FramesFactory.getGCRF(), date, mu);
1618 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getTrueAnomaly());
1619 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getEccentricAnomaly());
1620 Assertions.assertEquals(expectedAnomaly, keplerianOrbit.getMeanAnomaly());
1621 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getTrueAnomalyDot());
1622 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getEccentricAnomalyDot());
1623 Assertions.assertEquals(expectedAnomalyDot, keplerianOrbit.getMeanAnomalyDot());
1624 }
1625 }
1626 }
1627
1628 @BeforeEach
1629 public void setUp() {
1630
1631 Utils.setDataRoot("regular-data");
1632
1633
1634 date = AbsoluteDate.J2000_EPOCH;
1635
1636
1637 mu = 3.9860047e14;
1638 }
1639
1640 @AfterEach
1641 public void tearDown() {
1642 date = null;
1643 }
1644
1645 }