1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import static org.orekit.OrekitMatchers.relativelyCloseTo;
20
21 import java.io.ByteArrayInputStream;
22 import java.io.ByteArrayOutputStream;
23 import java.io.IOException;
24 import java.io.ObjectInputStream;
25 import java.io.ObjectOutputStream;
26 import java.util.ArrayList;
27 import java.util.List;
28 import java.util.function.Function;
29
30 import org.hamcrest.MatcherAssert;
31 import org.hipparchus.analysis.UnivariateFunction;
32 import org.hipparchus.analysis.differentiation.DSFactory;
33 import org.hipparchus.analysis.differentiation.FiniteDifferencesDifferentiator;
34 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
35 import org.hipparchus.geometry.euclidean.threed.Vector3D;
36 import org.hipparchus.linear.MatrixUtils;
37 import org.hipparchus.linear.RealMatrixPreservingVisitor;
38 import org.hipparchus.util.FastMath;
39 import org.hipparchus.util.MathUtils;
40 import org.junit.After;
41 import org.junit.Assert;
42 import org.junit.Before;
43 import org.junit.Test;
44 import org.orekit.Utils;
45 import org.orekit.frames.Frame;
46 import org.orekit.frames.FramesFactory;
47 import org.orekit.frames.Transform;
48 import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
49 import org.orekit.time.AbsoluteDate;
50 import org.orekit.time.TimeScalesFactory;
51 import org.orekit.utils.Constants;
52 import org.orekit.utils.PVCoordinates;
53 import org.orekit.utils.TimeStampedPVCoordinates;
54
55
56 public class EquinoctialOrbitTest {
57
58
59 private AbsoluteDate date;
60
61
62 private double mu;
63
64 @Test
65 public void testEquinoctialToEquinoctialEll() {
66
67 double ix = 1.200e-04;
68 double iy = -1.16e-04;
69 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
70 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
71 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
72
73
74 EquinoctialOrbit equi =
75 new EquinoctialOrbit(42166712.0, 0.5, -0.5, hx, hy,
76 5.300, PositionAngle.MEAN,
77 FramesFactory.getEME2000(), date, mu);
78 Vector3D pos = equi.getPVCoordinates().getPosition();
79 Vector3D vit = equi.getPVCoordinates().getVelocity();
80
81 PVCoordinates pvCoordinates = new PVCoordinates(pos, vit);
82
83 EquinoctialOrbit param = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
84 Assert.assertEquals(param.getA(), equi.getA(), Utils.epsilonTest * equi.getA());
85 Assert.assertEquals(param.getEquinoctialEx(), equi.getEquinoctialEx(),
86 Utils.epsilonE * FastMath.abs(equi.getE()));
87 Assert.assertEquals(param.getEquinoctialEy(), equi.getEquinoctialEy(),
88 Utils.epsilonE * FastMath.abs(equi.getE()));
89 Assert.assertEquals(param.getHx(), equi.getHx(), Utils.epsilonAngle
90 * FastMath.abs(equi.getI()));
91 Assert.assertEquals(param.getHy(), equi.getHy(), Utils.epsilonAngle
92 * FastMath.abs(equi.getI()));
93 Assert.assertEquals(MathUtils.normalizeAngle(param.getLv(), equi.getLv()), equi.getLv(),
94 Utils.epsilonAngle * FastMath.abs(equi.getLv()));
95
96 }
97
98 @Test
99 public void testEquinoctialToEquinoctialCirc() {
100
101 double ix = 1.200e-04;
102 double iy = -1.16e-04;
103 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
104 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
105 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
106
107
108 EquinoctialOrbit equiCir =
109 new EquinoctialOrbit(42166712.0, 0.1e-10, -0.1e-10, hx, hy,
110 5.300, PositionAngle.MEAN,
111 FramesFactory.getEME2000(), date, mu);
112 Vector3D posCir = equiCir.getPVCoordinates().getPosition();
113 Vector3D vitCir = equiCir.getPVCoordinates().getVelocity();
114
115 PVCoordinates pvCoordinates = new PVCoordinates(posCir, vitCir);
116
117 EquinoctialOrbit paramCir = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(),
118 date, mu);
119 Assert.assertEquals(paramCir.getA(), equiCir.getA(), Utils.epsilonTest
120 * equiCir.getA());
121 Assert.assertEquals(paramCir.getEquinoctialEx(), equiCir.getEquinoctialEx(),
122 Utils.epsilonEcir * FastMath.abs(equiCir.getE()));
123 Assert.assertEquals(paramCir.getEquinoctialEy(), equiCir.getEquinoctialEy(),
124 Utils.epsilonEcir * FastMath.abs(equiCir.getE()));
125 Assert.assertEquals(paramCir.getHx(), equiCir.getHx(), Utils.epsilonAngle
126 * FastMath.abs(equiCir.getI()));
127 Assert.assertEquals(paramCir.getHy(), equiCir.getHy(), Utils.epsilonAngle
128 * FastMath.abs(equiCir.getI()));
129 Assert.assertEquals(MathUtils.normalizeAngle(paramCir.getLv(), equiCir.getLv()), equiCir
130 .getLv(), Utils.epsilonAngle * FastMath.abs(equiCir.getLv()));
131
132 }
133
134 @Test
135 public void testEquinoctialToCartesian() {
136
137 double ix = 1.200e-04;
138 double iy = -1.16e-04;
139 double inc = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4.));
140 double hx = FastMath.tan(inc / 2.) * ix / (2 * FastMath.sin(inc / 2.));
141 double hy = FastMath.tan(inc / 2.) * iy / (2 * FastMath.sin(inc / 2.));
142
143 EquinoctialOrbit equi =
144 new EquinoctialOrbit(42166712.0, -7.900e-06, 1.100e-04, hx, hy,
145 5.300, PositionAngle.MEAN,
146 FramesFactory.getEME2000(), date, mu);
147 Vector3D pos = equi.getPVCoordinates().getPosition();
148 Vector3D vit = equi.getPVCoordinates().getVelocity();
149
150
151 double oneovera = (2. / pos.getNorm()) - vit.getNorm() * vit.getNorm() / mu;
152 Assert.assertEquals(oneovera, 1. / equi.getA(), 1.0e-7);
153
154 Assert.assertEquals( 0.233745668678733e+08, pos.getX(), Utils.epsilonTest * FastMath.abs(pos.getX()));
155 Assert.assertEquals(-0.350998914352669e+08, pos.getY(), Utils.epsilonTest * FastMath.abs(pos.getY()));
156 Assert.assertEquals(-0.150053723123334e+04, pos.getZ(), Utils.epsilonTest * FastMath.abs(pos.getZ()));
157
158 Assert.assertEquals(2558.7096558809967, vit.getX(), Utils.epsilonTest * FastMath.abs(vit.getX()));
159 Assert.assertEquals(1704.1586039092576, vit.getY(), Utils.epsilonTest * FastMath.abs(vit.getY()));
160 Assert.assertEquals( 0.5013093577879, vit.getZ(), Utils.epsilonTest * FastMath.abs(vit.getZ()));
161
162 }
163
164 @Test
165 public void testEquinoctialToKeplerian() {
166
167 double ix = 1.20e-4;
168 double iy = -1.16e-4;
169 double i = 2 * FastMath.asin(FastMath.sqrt((ix * ix + iy * iy) / 4));
170 double hx = FastMath.tan(i / 2) * ix / (2 * FastMath.sin(i / 2));
171 double hy = FastMath.tan(i / 2) * iy / (2 * FastMath.sin(i / 2));
172
173 EquinoctialOrbit equi =
174 new EquinoctialOrbit(42166712.0, -7.900e-6, 1.100e-4, hx, hy,
175 5.300, PositionAngle.MEAN,
176 FramesFactory.getEME2000(), date, mu);
177 KeplerianOrbit kep = new KeplerianOrbit(equi);
178
179 Assert.assertEquals(42166712.000, equi.getA(), Utils.epsilonTest * kep.getA());
180 Assert.assertEquals(0.110283316961361e-03, kep.getE(), Utils.epsilonE
181 * FastMath.abs(kep.getE()));
182 Assert.assertEquals(0.166901168553917e-03, kep.getI(), Utils.epsilonAngle
183 * FastMath.abs(kep.getI()));
184 Assert.assertEquals(MathUtils.normalizeAngle(-3.87224326008837, kep.getPerigeeArgument()),
185 kep.getPerigeeArgument(), Utils.epsilonTest
186 * FastMath.abs(kep.getPerigeeArgument()));
187 Assert.assertEquals(MathUtils.normalizeAngle(5.51473467358854, kep
188 .getRightAscensionOfAscendingNode()), kep
189 .getRightAscensionOfAscendingNode(), Utils.epsilonTest
190 * FastMath.abs(kep.getRightAscensionOfAscendingNode()));
191 Assert.assertEquals(MathUtils.normalizeAngle(3.65750858649982, kep.getMeanAnomaly()), kep
192 .getMeanAnomaly(), Utils.epsilonTest * FastMath.abs(kep.getMeanAnomaly()));
193
194 }
195
196 @Test(expected=IllegalArgumentException.class)
197 public void testHyperbolic() {
198 new EquinoctialOrbit(42166712.0, 0.9, 0.5, 0.01, -0.02, 5.300,
199 PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu);
200 }
201
202 @Test
203 public void testNumericalIssue25() {
204 Vector3D position = new Vector3D(3782116.14107698, 416663.11924914, 5875541.62103057);
205 Vector3D velocity = new Vector3D(-6349.7848910501, 288.4061811651, 4066.9366759691);
206 EquinoctialOrbit orbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
207 FramesFactory.getEME2000(),
208 new AbsoluteDate("2004-01-01T23:00:00.000",
209 TimeScalesFactory.getUTC()),
210 3.986004415E14);
211 Assert.assertEquals(0.0, orbit.getE(), 2.0e-14);
212 }
213
214
215 @Test
216 public void testAnomaly() {
217
218
219 Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
220 Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
221
222 EquinoctialOrbit p = new EquinoctialOrbit(new PVCoordinates(position, velocity), FramesFactory.getEME2000(), date, mu);
223 KeplerianOrbit kep = new KeplerianOrbit(p);
224
225 double e = p.getE();
226 double eRatio = FastMath.sqrt((1 - e) / (1 + e));
227 double paPraan = kep.getPerigeeArgument()
228 + kep.getRightAscensionOfAscendingNode();
229
230 double lv = 1.1;
231
232 double lE = 2 * FastMath.atan(eRatio * FastMath.tan((lv - paPraan) / 2)) + paPraan;
233 double lM = lE - e * FastMath.sin(lE - paPraan);
234
235 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
236 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngle.TRUE,
237 p.getFrame(), p.getDate(), p.getMu());
238 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
239 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
240 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
241 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
242 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngle.TRUE,
243 p.getFrame(), p.getDate(), p.getMu());
244
245 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
246 p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngle.ECCENTRIC,
247 p.getFrame(), p.getDate(), p.getMu());
248 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
249 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
250 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
251 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
252 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngle.TRUE,
253 p.getFrame(), p.getDate(), p.getMu());
254
255 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
256 p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngle.MEAN,
257 p.getFrame(), p.getDate(), p.getMu());
258 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
259 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
260 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
261
262
263 p = new EquinoctialOrbit(p.getA(), 0 ,
264 0, p.getHx(), p.getHy() , p.getLv() , PositionAngle.TRUE,
265 p.getFrame(), p.getDate(), p.getMu());
266
267 lE = lv;
268 lM = lE;
269
270 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
271 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngle.TRUE,
272 p.getFrame(), p.getDate(), p.getMu());
273 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
274 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
275 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
276 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
277 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngle.TRUE,
278 p.getFrame(), p.getDate(), p.getMu());
279
280 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
281 p.getEquinoctialEy() , p.getHx(), p.getHy() , lE , PositionAngle.ECCENTRIC,
282 p.getFrame(), p.getDate(), p.getMu());
283 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
284 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
285 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
286 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
287 p.getEquinoctialEy() , p.getHx(), p.getHy() , 0 , PositionAngle.TRUE,
288 p.getFrame(), p.getDate(), p.getMu());
289
290 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
291 p.getEquinoctialEy() , p.getHx(), p.getHy() , lM , PositionAngle.MEAN, p.getFrame(), p.getDate(), p.getMu());
292 Assert.assertEquals(p.getLv(), lv, Utils.epsilonAngle * FastMath.abs(lv));
293 Assert.assertEquals(p.getLE(), lE, Utils.epsilonAngle * FastMath.abs(lE));
294 Assert.assertEquals(p.getLM(), lM, Utils.epsilonAngle * FastMath.abs(lM));
295 }
296
297 @Test
298 public void testPositionVelocityNorms() {
299
300
301 EquinoctialOrbit p =
302 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
303 0.67, PositionAngle.TRUE,
304 FramesFactory.getEME2000(), date, mu);
305
306 double ex = p.getEquinoctialEx();
307 double ey = p.getEquinoctialEy();
308 double lv = p.getLv();
309 double ksi = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
310 double nu = ex * FastMath.sin(lv) - ey * FastMath.cos(lv);
311 double epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
312
313 double a = p.getA();
314 double na = FastMath.sqrt(p.getMu() / a);
315
316 Assert.assertEquals(a * epsilon * epsilon / ksi, p.getPVCoordinates().getPosition().getNorm(),
317 Utils.epsilonTest * FastMath.abs(p.getPVCoordinates().getPosition().getNorm()));
318 Assert.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon, p
319 .getPVCoordinates().getVelocity().getNorm(), Utils.epsilonTest
320 * FastMath.abs(p.getPVCoordinates().getVelocity().getNorm()));
321
322
323 EquinoctialOrbit pCirEqua =
324 new EquinoctialOrbit(42166712.0, 0.1e-8, 0.1e-8, 0.1e-8, 0.1e-8,
325 0.67, PositionAngle.TRUE,
326 FramesFactory.getEME2000(), date, mu);
327
328 ex = pCirEqua.getEquinoctialEx();
329 ey = pCirEqua.getEquinoctialEy();
330 lv = pCirEqua.getLv();
331 ksi = 1 + ex * FastMath.cos(lv) + ey * FastMath.sin(lv);
332 nu = ex * FastMath.sin(lv) - ey * FastMath.cos(lv);
333 epsilon = FastMath.sqrt(1 - ex * ex - ey * ey);
334
335 a = pCirEqua.getA();
336 na = FastMath.sqrt(pCirEqua.getMu() / a);
337
338 Assert.assertEquals(a * epsilon * epsilon / ksi, pCirEqua.getPVCoordinates().getPosition()
339 .getNorm(), Utils.epsilonTest
340 * FastMath.abs(pCirEqua.getPVCoordinates().getPosition().getNorm()));
341 Assert.assertEquals(na * FastMath.sqrt(ksi * ksi + nu * nu) / epsilon, pCirEqua
342 .getPVCoordinates().getVelocity().getNorm(), Utils.epsilonTest
343 * FastMath.abs(pCirEqua.getPVCoordinates().getVelocity().getNorm()));
344 }
345
346 @Test
347 public void testGeometry() {
348
349
350 EquinoctialOrbit p =
351 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
352 0.67, PositionAngle.TRUE,
353 FramesFactory.getEME2000(), date, mu);
354
355 Vector3D position = p.getPVCoordinates().getPosition();
356 Vector3D velocity = p.getPVCoordinates().getVelocity();
357 Vector3D momentum = p.getPVCoordinates().getMomentum().normalize();
358
359 double apogeeRadius = p.getA() * (1 + p.getE());
360 double perigeeRadius = p.getA() * (1 - p.getE());
361
362 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) {
363 p = new EquinoctialOrbit(p.getA(), p.getEquinoctialEx(),
364 p.getEquinoctialEy() , p.getHx(), p.getHy() , lv , PositionAngle.TRUE,
365 p.getFrame(), p.getDate(), p.getMu());
366 position = p.getPVCoordinates().getPosition();
367
368
369
370
371 Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest));
372 Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest));
373
374 position = position.normalize();
375 velocity = p.getPVCoordinates().getVelocity();
376 velocity = velocity.normalize();
377
378
379
380
381
382 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
383
384 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
385 }
386
387
388 EquinoctialOrbit pCirEqua =
389 new EquinoctialOrbit(42166712.0, 0.1e-8, 0.1e-8, 0.1e-8, 0.1e-8,
390 0.67, PositionAngle.TRUE,
391 FramesFactory.getEME2000(), date, mu);
392
393 position = pCirEqua.getPVCoordinates().getPosition();
394 velocity = pCirEqua.getPVCoordinates().getVelocity();
395
396 momentum = Vector3D.crossProduct(position, velocity).normalize();
397
398 apogeeRadius = pCirEqua.getA() * (1 + pCirEqua.getE());
399 perigeeRadius = pCirEqua.getA() * (1 - pCirEqua.getE());
400
401 Assert.assertEquals(perigeeRadius, apogeeRadius, 1.e+4 * Utils.epsilonTest
402 * apogeeRadius);
403
404 for (double lv = 0; lv <= 2 * FastMath.PI; lv += 2 * FastMath.PI / 100.) {
405 pCirEqua = new EquinoctialOrbit(pCirEqua.getA(), pCirEqua.getEquinoctialEx(),
406 pCirEqua.getEquinoctialEy() , pCirEqua.getHx(), pCirEqua.getHy() , lv , PositionAngle.TRUE,
407 pCirEqua.getFrame(), p.getDate(), p.getMu());
408 position = pCirEqua.getPVCoordinates().getPosition();
409
410
411
412 Assert.assertTrue((position.getNorm() - apogeeRadius) <= (apogeeRadius * Utils.epsilonTest));
413 Assert.assertTrue((position.getNorm() - perigeeRadius) >= (-perigeeRadius * Utils.epsilonTest));
414
415 position = position.normalize();
416 velocity = pCirEqua.getPVCoordinates().getVelocity();
417 velocity = velocity.normalize();
418
419
420
421
422
423 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(position, momentum)) < Utils.epsilonTest);
424
425 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(velocity, momentum)) < Utils.epsilonTest);
426 }
427 }
428
429
430 @Test
431 public void testRadiusOfCurvature() {
432
433
434 EquinoctialOrbit p =
435 new EquinoctialOrbit(42166712.0, 0.5, -0.5, 1.200, 2.1,
436 0.67, PositionAngle.TRUE,
437 FramesFactory.getEME2000(), date, mu);
438
439
440 Vector3D u = p.getPVCoordinates().getMomentum().orthogonal();
441 Vector3D v = Vector3D.crossProduct(p.getPVCoordinates().getMomentum(), u).normalize();
442
443
444 double xDot = Vector3D.dotProduct(p.getPVCoordinates().getVelocity(), u);
445 double yDot = Vector3D.dotProduct(p.getPVCoordinates().getVelocity(), v);
446 double xDotDot = Vector3D.dotProduct(p.getPVCoordinates().getAcceleration(), u);
447 double yDotDot = Vector3D.dotProduct(p.getPVCoordinates().getAcceleration(), v);
448 double dot2 = xDot * xDot + yDot * yDot;
449 double rCart = dot2 * FastMath.sqrt(dot2) /
450 FastMath.abs(xDot * yDotDot - yDot * xDotDot);
451
452
453 double ex = p.getEquinoctialEx();
454 double ey = p.getEquinoctialEy();
455 double f = ex * FastMath.cos(p.getLE()) + ey * FastMath.sin(p.getLE());
456 double oMf2 = 1 - f * f;
457 double rOrb = p.getA() * oMf2 * FastMath.sqrt(oMf2 / (1 - (ex * ex + ey * ey)));
458
459
460 Assert.assertEquals(rCart, rOrb, 1.0e-15 * p.getA());
461
462
463
464 Assert.assertEquals(0.8477 * p.getA(), rCart, 1.0e-4 * p.getA());
465
466 }
467
468 @Test
469 public void testSymmetry() {
470
471
472 Vector3D position = new Vector3D(4512.9, 18260., -5127.);
473 Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6);
474
475 EquinoctialOrbit p = new EquinoctialOrbit(new PVCoordinates(position, velocity),
476 FramesFactory.getEME2000(), date, mu);
477
478 Vector3D positionOffset = p.getPVCoordinates().getPosition().subtract(position);
479 Vector3D velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
480
481 Assert.assertTrue(positionOffset.getNorm() < Utils.epsilonTest);
482 Assert.assertTrue(velocityOffset.getNorm() < Utils.epsilonTest);
483
484
485 position = new Vector3D(33051.2, 26184.9, -1.3E-5);
486 velocity = new Vector3D(-60376.2, 76208., 2.7E-4);
487
488 p = new EquinoctialOrbit(new PVCoordinates(position, velocity),
489 FramesFactory.getEME2000(), date, mu);
490
491 positionOffset = p.getPVCoordinates().getPosition().subtract(position);
492 velocityOffset = p.getPVCoordinates().getVelocity().subtract(velocity);
493
494 Assert.assertTrue(positionOffset.getNorm() < Utils.epsilonTest);
495 Assert.assertTrue(velocityOffset.getNorm() < Utils.epsilonTest);
496 }
497
498 @Test(expected=IllegalArgumentException.class)
499 public void testNonInertialFrame() throws IllegalArgumentException {
500
501 Vector3D position = new Vector3D(4512.9, 18260., -5127.);
502 Vector3D velocity = new Vector3D(134664.6, 90066.8, 72047.6);
503 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
504 new EquinoctialOrbit(pvCoordinates,
505 new Frame(FramesFactory.getEME2000(), Transform.IDENTITY, "non-inertial", false),
506 date, mu);
507 }
508
509 @Test
510 public void testJacobianReference() {
511
512 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
513 double mu = 3.986004415e+14;
514 EquinoctialOrbit orbEqu = new EquinoctialOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1,
515 FastMath.toRadians(40.), PositionAngle.MEAN,
516 FramesFactory.getEME2000(), dateTca, mu);
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587 Vector3D pRef = new Vector3D(2004367.298657628707588, 6575317.978060320019722, -1518024.843913963763043);
588 Vector3D vRef = new Vector3D(5574.048661495634406, -368.839015744295409, 5009.529487849066754);
589 double[][] jRef = {
590 { 0.56305379787310628, 1.8470954710993663, -0.42643364527246025, 1370.4369387322224, -90.682848736736688 , 1231.6441195141242 },
591 { 9.52434720041122055E-008, 9.49704503778007296E-008, 4.46607520107935678E-008, 1.69704446323098610E-004, 7.05603505855828105E-005, 1.14825140460141970E-004 },
592 { -5.41784097802642701E-008, 9.54903765833015538E-008, -8.95815777332234450E-008, 1.01864980963344096E-004, -1.03194262242761416E-004, 1.40668700715197768E-004 },
593 { 1.96680305426455816E-007, -1.12388745957974467E-007, -2.27118924123407353E-007, 2.06472886488132167E-004, -1.17984506564646906E-004, -2.38427023682723818E-004 },
594 { -2.24382495052235118E-007, 1.28218568601277626E-007, 2.59108357381747656E-007, 1.89034327703662092E-004, -1.08019615830663994E-004, -2.18289640324466583E-004 },
595 { -3.04001022071876804E-007, 1.22214683774559989E-007, 1.35141804810132761E-007, -1.34034616931480536E-004, -2.14283975204169379E-004, 1.29018773893081404E-004 }
596 };
597
598 PVCoordinates pv = orbEqu.getPVCoordinates();
599 Assert.assertEquals(0, pv.getPosition().subtract(pRef).getNorm(), 2.0e-16 * pRef.getNorm());
600 Assert.assertEquals(0, pv.getVelocity().subtract(vRef).getNorm(), 2.0e-16 * vRef.getNorm());
601
602 double[][] jacobian = new double[6][6];
603 orbEqu.getJacobianWrtCartesian(PositionAngle.MEAN, jacobian);
604
605 for (int i = 0; i < jacobian.length; i++) {
606 double[] row = jacobian[i];
607 double[] rowRef = jRef[i];
608 for (int j = 0; j < row.length; j++) {
609 Assert.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 4.0e-15);
610 }
611 }
612
613 }
614
615 @Test
616 public void testJacobianFinitedifferences() {
617
618 AbsoluteDate dateTca = new AbsoluteDate(2000, 04, 01, 0, 0, 0.000, TimeScalesFactory.getUTC());
619 double mu = 3.986004415e+14;
620 EquinoctialOrbit orbEqu = new EquinoctialOrbit(7000000.0, 0.01, -0.02, 1.2, 2.1,
621 FastMath.toRadians(40.), PositionAngle.MEAN,
622 FramesFactory.getEME2000(), dateTca, mu);
623
624 for (PositionAngle type : PositionAngle.values()) {
625 double hP = 2.0;
626 double[][] finiteDiffJacobian = finiteDifferencesJacobian(type, orbEqu, hP);
627 double[][] jacobian = new double[6][6];
628 orbEqu.getJacobianWrtCartesian(type, jacobian);
629
630 for (int i = 0; i < jacobian.length; i++) {
631 double[] row = jacobian[i];
632 double[] rowRef = finiteDiffJacobian[i];
633 for (int j = 0; j < row.length; j++) {
634 Assert.assertEquals(0, (row[j] - rowRef[j]) / rowRef[j], 4.0e-9);
635 }
636 }
637
638 double[][] invJacobian = new double[6][6];
639 orbEqu.getJacobianWrtParameters(type, invJacobian);
640 MatrixUtils.createRealMatrix(jacobian).
641 multiply(MatrixUtils.createRealMatrix(invJacobian)).
642 walkInRowOrder(new RealMatrixPreservingVisitor() {
643 public void start(int rows, int columns,
644 int startRow, int endRow, int startColumn, int endColumn) {
645 }
646
647 public void visit(int row, int column, double value) {
648 Assert.assertEquals(row == column ? 1.0 : 0.0, value, 7.0e-9);
649 }
650
651 public double end() {
652 return Double.NaN;
653 }
654 });
655
656 }
657
658 }
659
660 private double[][] finiteDifferencesJacobian(PositionAngle type, EquinoctialOrbit orbit, double hP)
661 {
662 double[][] jacobian = new double[6][6];
663 for (int i = 0; i < 6; ++i) {
664 fillColumn(type, i, orbit, hP, jacobian);
665 }
666 return jacobian;
667 }
668
669 private void fillColumn(PositionAngle type, int i, EquinoctialOrbit orbit, double hP, double[][] jacobian) {
670
671
672
673 Vector3D p = orbit.getPVCoordinates().getPosition();
674 Vector3D v = orbit.getPVCoordinates().getVelocity();
675 double hV = orbit.getMu() * hP / (v.getNorm() * p.getNormSq());
676
677 double h;
678 Vector3D dP = Vector3D.ZERO;
679 Vector3D dV = Vector3D.ZERO;
680 switch (i) {
681 case 0:
682 h = hP;
683 dP = new Vector3D(hP, 0, 0);
684 break;
685 case 1:
686 h = hP;
687 dP = new Vector3D(0, hP, 0);
688 break;
689 case 2:
690 h = hP;
691 dP = new Vector3D(0, 0, hP);
692 break;
693 case 3:
694 h = hV;
695 dV = new Vector3D(hV, 0, 0);
696 break;
697 case 4:
698 h = hV;
699 dV = new Vector3D(0, hV, 0);
700 break;
701 default:
702 h = hV;
703 dV = new Vector3D(0, 0, hV);
704 break;
705 }
706
707 EquinoctialOrbit oM4h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -4, dP), new Vector3D(1, v, -4, dV)),
708 orbit.getFrame(), orbit.getDate(), orbit.getMu());
709 EquinoctialOrbit oM3h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -3, dP), new Vector3D(1, v, -3, dV)),
710 orbit.getFrame(), orbit.getDate(), orbit.getMu());
711 EquinoctialOrbit oM2h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -2, dP), new Vector3D(1, v, -2, dV)),
712 orbit.getFrame(), orbit.getDate(), orbit.getMu());
713 EquinoctialOrbit oM1h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, -1, dP), new Vector3D(1, v, -1, dV)),
714 orbit.getFrame(), orbit.getDate(), orbit.getMu());
715 EquinoctialOrbit oP1h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +1, dP), new Vector3D(1, v, +1, dV)),
716 orbit.getFrame(), orbit.getDate(), orbit.getMu());
717 EquinoctialOrbit oP2h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +2, dP), new Vector3D(1, v, +2, dV)),
718 orbit.getFrame(), orbit.getDate(), orbit.getMu());
719 EquinoctialOrbit oP3h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +3, dP), new Vector3D(1, v, +3, dV)),
720 orbit.getFrame(), orbit.getDate(), orbit.getMu());
721 EquinoctialOrbit oP4h = new EquinoctialOrbit(new PVCoordinates(new Vector3D(1, p, +4, dP), new Vector3D(1, v, +4, dV)),
722 orbit.getFrame(), orbit.getDate(), orbit.getMu());
723
724 jacobian[0][i] = (-3 * (oP4h.getA() - oM4h.getA()) +
725 32 * (oP3h.getA() - oM3h.getA()) -
726 168 * (oP2h.getA() - oM2h.getA()) +
727 672 * (oP1h.getA() - oM1h.getA())) / (840 * h);
728 jacobian[1][i] = (-3 * (oP4h.getEquinoctialEx() - oM4h.getEquinoctialEx()) +
729 32 * (oP3h.getEquinoctialEx() - oM3h.getEquinoctialEx()) -
730 168 * (oP2h.getEquinoctialEx() - oM2h.getEquinoctialEx()) +
731 672 * (oP1h.getEquinoctialEx() - oM1h.getEquinoctialEx())) / (840 * h);
732 jacobian[2][i] = (-3 * (oP4h.getEquinoctialEy() - oM4h.getEquinoctialEy()) +
733 32 * (oP3h.getEquinoctialEy() - oM3h.getEquinoctialEy()) -
734 168 * (oP2h.getEquinoctialEy() - oM2h.getEquinoctialEy()) +
735 672 * (oP1h.getEquinoctialEy() - oM1h.getEquinoctialEy())) / (840 * h);
736 jacobian[3][i] = (-3 * (oP4h.getHx() - oM4h.getHx()) +
737 32 * (oP3h.getHx() - oM3h.getHx()) -
738 168 * (oP2h.getHx() - oM2h.getHx()) +
739 672 * (oP1h.getHx() - oM1h.getHx())) / (840 * h);
740 jacobian[4][i] = (-3 * (oP4h.getHy() - oM4h.getHy()) +
741 32 * (oP3h.getHy() - oM3h.getHy()) -
742 168 * (oP2h.getHy() - oM2h.getHy()) +
743 672 * (oP1h.getHy() - oM1h.getHy())) / (840 * h);
744 jacobian[5][i] = (-3 * (oP4h.getL(type) - oM4h.getL(type)) +
745 32 * (oP3h.getL(type) - oM3h.getL(type)) -
746 168 * (oP2h.getL(type) - oM2h.getL(type)) +
747 672 * (oP1h.getL(type) - oM1h.getL(type))) / (840 * h);
748
749 }
750
751 @Test
752 public void testInterpolationWithDerivatives() {
753 doTestInterpolation(true,
754 397, 1.17e-8,
755 610, 4.48e-6,
756 4870, 115);
757 }
758
759 @Test
760 public void testInterpolationWithoutDerivatives() {
761 doTestInterpolation(false,
762 397, 0.0372,
763 610.0, 1.23,
764 4879, 8871);
765 }
766
767 private void doTestInterpolation(boolean useDerivatives,
768 double shiftErrorWithin, double interpolationErrorWithin,
769 double shiftErrorSlightlyPast, double interpolationErrorSlightlyPast,
770 double shiftErrorFarPast, double interpolationErrorFarPast)
771 {
772
773 final double ehMu = 3.9860047e14;
774 final double ae = 6.378137e6;
775 final double c20 = -1.08263e-3;
776 final double c30 = 2.54e-6;
777 final double c40 = 1.62e-6;
778 final double c50 = 2.3e-7;
779 final double c60 = -5.5e-7;
780
781 final AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
782 final Vector3D position = new Vector3D(3220103., 69623., 6449822.);
783 final Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
784 final EquinoctialOrbit initialOrbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
785 FramesFactory.getEME2000(), date, ehMu);
786
787 EcksteinHechlerPropagator propagator =
788 new EcksteinHechlerPropagator(initialOrbit, ae, ehMu, c20, c30, c40, c50, c60);
789
790
791 List<Orbit> sample = new ArrayList<Orbit>();
792 for (double dt = 0; dt < 300.0; dt += 60.0) {
793 Orbit orbit = propagator.propagate(date.shiftedBy(dt)).getOrbit();
794 if (!useDerivatives) {
795
796 double[] stateVector = new double[6];
797 orbit.getType().mapOrbitToArray(orbit, PositionAngle.TRUE, stateVector, null);
798 orbit = orbit.getType().mapArrayToOrbit(stateVector, null, PositionAngle.TRUE,
799 orbit.getDate(), orbit.getMu(), orbit.getFrame());
800 }
801 sample.add(orbit);
802 }
803
804
805 double maxShiftError = 0;
806 double maxInterpolationError = 0;
807 for (double dt = 0; dt < 241.0; dt += 1.0) {
808 AbsoluteDate t = initialOrbit.getDate().shiftedBy(dt);
809 Vector3D shifted = initialOrbit.shiftedBy(dt).getPVCoordinates().getPosition();
810 Vector3D interpolated = initialOrbit.interpolate(t, sample).getPVCoordinates().getPosition();
811 Vector3D propagated = propagator.propagate(t).getPVCoordinates().getPosition();
812 maxShiftError = FastMath.max(maxShiftError, shifted.subtract(propagated).getNorm());
813 maxInterpolationError = FastMath.max(maxInterpolationError, interpolated.subtract(propagated).getNorm());
814 }
815 Assert.assertEquals(shiftErrorWithin, maxShiftError, 0.01 * shiftErrorWithin);
816 Assert.assertEquals(interpolationErrorWithin, maxInterpolationError, 0.01 * interpolationErrorWithin);
817
818
819 maxShiftError = 0;
820 maxInterpolationError = 0;
821 for (double dt = 240; dt < 300.0; dt += 1.0) {
822 AbsoluteDate t = initialOrbit.getDate().shiftedBy(dt);
823 Vector3D shifted = initialOrbit.shiftedBy(dt).getPVCoordinates().getPosition();
824 Vector3D interpolated = initialOrbit.interpolate(t, sample).getPVCoordinates().getPosition();
825 Vector3D propagated = propagator.propagate(t).getPVCoordinates().getPosition();
826 maxShiftError = FastMath.max(maxShiftError, shifted.subtract(propagated).getNorm());
827 maxInterpolationError = FastMath.max(maxInterpolationError, interpolated.subtract(propagated).getNorm());
828 }
829 Assert.assertEquals(shiftErrorSlightlyPast, maxShiftError, 0.01 * shiftErrorSlightlyPast);
830 Assert.assertEquals(interpolationErrorSlightlyPast, maxInterpolationError, 0.01 * interpolationErrorSlightlyPast);
831
832
833
834 maxShiftError = 0;
835 maxInterpolationError = 0;
836 for (double dt = 300; dt < 1000; dt += 1.0) {
837 AbsoluteDate t = initialOrbit.getDate().shiftedBy(dt);
838 Vector3D shifted = initialOrbit.shiftedBy(dt).getPVCoordinates().getPosition();
839 Vector3D interpolated = initialOrbit.interpolate(t, sample).getPVCoordinates().getPosition();
840 Vector3D propagated = propagator.propagate(t).getPVCoordinates().getPosition();
841 maxShiftError = FastMath.max(maxShiftError, shifted.subtract(propagated).getNorm());
842 maxInterpolationError = FastMath.max(maxInterpolationError, interpolated.subtract(propagated).getNorm());
843 }
844 Assert.assertEquals(shiftErrorFarPast, maxShiftError, 0.01 * shiftErrorFarPast);
845 Assert.assertEquals(interpolationErrorFarPast, maxInterpolationError, 0.01 * interpolationErrorFarPast);
846
847 }
848
849 @Test
850 public void testSerialization()
851 throws IOException, ClassNotFoundException {
852 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
853 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
854 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity);
855 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
856 Assert.assertEquals(42255170.003, orbit.getA(), 1.0e-3);
857
858 ByteArrayOutputStream bos = new ByteArrayOutputStream();
859 ObjectOutputStream oos = new ObjectOutputStream(bos);
860 oos.writeObject(orbit);
861
862 Assert.assertTrue(bos.size() > 280);
863 Assert.assertTrue(bos.size() < 330);
864
865 ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
866 ObjectInputStream ois = new ObjectInputStream(bis);
867 EquinoctialOrbit deserialized = (EquinoctialOrbit) ois.readObject();
868 Assert.assertEquals(orbit.getA(), deserialized.getA(), 1.0e-10);
869 Assert.assertEquals(orbit.getEquinoctialEx(), deserialized.getEquinoctialEx(), 1.0e-10);
870 Assert.assertEquals(orbit.getEquinoctialEy(), deserialized.getEquinoctialEy(), 1.0e-10);
871 Assert.assertEquals(orbit.getHx(), deserialized.getHx(), 1.0e-10);
872 Assert.assertEquals(orbit.getHy(), deserialized.getHy(), 1.0e-10);
873 Assert.assertEquals(orbit.getLv(), deserialized.getLv(), 1.0e-10);
874 Assert.assertTrue(Double.isNaN(orbit.getADot()) && Double.isNaN(deserialized.getADot()));
875 Assert.assertTrue(Double.isNaN(orbit.getEquinoctialExDot()) && Double.isNaN(deserialized.getEquinoctialExDot()));
876 Assert.assertTrue(Double.isNaN(orbit.getEquinoctialEyDot()) && Double.isNaN(deserialized.getEquinoctialEyDot()));
877 Assert.assertTrue(Double.isNaN(orbit.getHxDot()) && Double.isNaN(deserialized.getHxDot()));
878 Assert.assertTrue(Double.isNaN(orbit.getHyDot()) && Double.isNaN(deserialized.getHyDot()));
879 Assert.assertTrue(Double.isNaN(orbit.getLvDot()) && Double.isNaN(deserialized.getLvDot()));
880 Assert.assertEquals(orbit.getDate(), deserialized.getDate());
881 Assert.assertEquals(orbit.getMu(), deserialized.getMu(), 1.0e-10);
882 Assert.assertEquals(orbit.getFrame().getName(), deserialized.getFrame().getName());
883
884 }
885
886 @Test
887 public void testSerializationWithDerivatives()
888 throws IOException, ClassNotFoundException {
889 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
890 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
891 double r2 = position.getNormSq();
892 double r = FastMath.sqrt(r2);
893 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
894 1, new Vector3D(-0.1, 0.2, 0.3));
895 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
896 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
897 Assert.assertEquals(42255170.003, orbit.getA(), 1.0e-3);
898
899 ByteArrayOutputStream bos = new ByteArrayOutputStream();
900 ObjectOutputStream oos = new ObjectOutputStream(bos);
901 oos.writeObject(orbit);
902
903 Assert.assertTrue(bos.size() > 330);
904 Assert.assertTrue(bos.size() < 380);
905
906 ByteArrayInputStream bis = new ByteArrayInputStream(bos.toByteArray());
907 ObjectInputStream ois = new ObjectInputStream(bis);
908 EquinoctialOrbit deserialized = (EquinoctialOrbit) ois.readObject();
909 Assert.assertEquals(orbit.getA(), deserialized.getA(), 1.0e-10);
910 Assert.assertEquals(orbit.getEquinoctialEx(), deserialized.getEquinoctialEx(), 1.0e-10);
911 Assert.assertEquals(orbit.getEquinoctialEy(), deserialized.getEquinoctialEy(), 1.0e-10);
912 Assert.assertEquals(orbit.getHx(), deserialized.getHx(), 1.0e-10);
913 Assert.assertEquals(orbit.getHy(), deserialized.getHy(), 1.0e-10);
914 Assert.assertEquals(orbit.getLv(), deserialized.getLv(), 1.0e-10);
915 Assert.assertEquals(orbit.getADot(), deserialized.getADot(), 1.0e-10);
916 Assert.assertEquals(orbit.getEquinoctialExDot(), deserialized.getEquinoctialExDot(), 1.0e-10);
917 Assert.assertEquals(orbit.getEquinoctialEyDot(), deserialized.getEquinoctialEyDot(), 1.0e-10);
918 Assert.assertEquals(orbit.getHxDot(), deserialized.getHxDot(), 1.0e-10);
919 Assert.assertEquals(orbit.getHyDot(), deserialized.getHyDot(), 1.0e-10);
920 Assert.assertEquals(orbit.getLvDot(), deserialized.getLvDot(), 1.0e-10);
921 Assert.assertEquals(orbit.getDate(), deserialized.getDate());
922 Assert.assertEquals(orbit.getMu(), deserialized.getMu(), 1.0e-10);
923 Assert.assertEquals(orbit.getFrame().getName(), deserialized.getFrame().getName());
924
925 }
926
927 @Test
928 public void testNonKeplerianDerivatives() {
929 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
930 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
931 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
932 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
933 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
934 final Frame frame = FramesFactory.getEME2000();
935 final double mu = Constants.EIGEN5C_EARTH_MU;
936 final EquinoctialOrbit orbit = new EquinoctialOrbit(pv, frame, mu);
937
938 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getA()),
939 orbit.getADot(),
940 4.3e-8);
941 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEx()),
942 orbit.getEquinoctialExDot(),
943 2.1e-15);
944 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getEquinoctialEy()),
945 orbit.getEquinoctialEyDot(),
946 5.3e-16);
947 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHx()),
948 orbit.getHxDot(),
949 4.4e-15);
950 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getHy()),
951 orbit.getHyDot(),
952 1.5e-15);
953 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLv()),
954 orbit.getLvDot(),
955 1.2e-15);
956 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLE()),
957 orbit.getLEDot(),
958 7.7e-16);
959 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getLM()),
960 orbit.getLMDot(),
961 8.8e-16);
962 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getE()),
963 orbit.getEDot(),
964 6.9e-16);
965 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getI()),
966 orbit.getIDot(),
967 3.5e-15);
968 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngle.TRUE)),
969 orbit.getLDot(PositionAngle.TRUE),
970 1.2e-15);
971 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngle.ECCENTRIC)),
972 orbit.getLDot(PositionAngle.ECCENTRIC),
973 7.7e-16);
974 Assert.assertEquals(differentiate(pv, frame, mu, shifted -> shifted.getL(PositionAngle.MEAN)),
975 orbit.getLDot(PositionAngle.MEAN),
976 8.8e-16);
977
978 }
979
980 private <S extends Function<EquinoctialOrbit, Double>>
981 double differentiate(TimeStampedPVCoordinates pv, Frame frame, double mu, S picker) {
982 final DSFactory factory = new DSFactory(1, 1);
983 FiniteDifferencesDifferentiator differentiator = new FiniteDifferencesDifferentiator(8, 0.1);
984 UnivariateDifferentiableFunction diff = differentiator.differentiate(new UnivariateFunction() {
985 public double value(double dt) {
986 return picker.apply(new EquinoctialOrbit(pv.shiftedBy(dt), frame, mu));
987 }
988 });
989 return diff.value(factory.variable(0, 0.0)).getPartialDerivative(1);
990 }
991
992 @Test
993 public void testPositionAngleDerivatives() {
994 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:00:20.000", TimeScalesFactory.getUTC());
995 final Vector3D position = new Vector3D(6896874.444705, 1956581.072644, -147476.245054);
996 final Vector3D velocity = new Vector3D(166.816407662, -1106.783301861, -7372.745712770);
997 final Vector3D acceleration = new Vector3D(-7.466182457944, -2.118153357345, 0.160004048437);
998 final TimeStampedPVCoordinates pv = new TimeStampedPVCoordinates(date, position, velocity, acceleration);
999 final Frame frame = FramesFactory.getEME2000();
1000 final double mu = Constants.EIGEN5C_EARTH_MU;
1001 final EquinoctialOrbit orbit = new EquinoctialOrbit(pv, frame, mu);
1002
1003 for (PositionAngle type : PositionAngle.values()) {
1004 final EquinoctialOrbit rebuilt = new EquinoctialOrbit(orbit.getA(),
1005 orbit.getEquinoctialEx(),
1006 orbit.getEquinoctialEy(),
1007 orbit.getHx(),
1008 orbit.getHy(),
1009 orbit.getL(type),
1010 orbit.getADot(),
1011 orbit.getEquinoctialExDot(),
1012 orbit.getEquinoctialEyDot(),
1013 orbit.getHxDot(),
1014 orbit.getHyDot(),
1015 orbit.getLDot(type),
1016 type, orbit.getFrame(), orbit.getDate(), orbit.getMu());
1017 MatcherAssert.assertThat(rebuilt.getA(), relativelyCloseTo(orbit.getA(), 1));
1018 MatcherAssert.assertThat(rebuilt.getEquinoctialEx(), relativelyCloseTo(orbit.getEquinoctialEx(), 1));
1019 MatcherAssert.assertThat(rebuilt.getEquinoctialEy(), relativelyCloseTo(orbit.getEquinoctialEy(), 1));
1020 MatcherAssert.assertThat(rebuilt.getHx(), relativelyCloseTo(orbit.getHx(), 1));
1021 MatcherAssert.assertThat(rebuilt.getHy(), relativelyCloseTo(orbit.getHy(), 1));
1022 MatcherAssert.assertThat(rebuilt.getADot(), relativelyCloseTo(orbit.getADot(), 1));
1023 MatcherAssert.assertThat(rebuilt.getEquinoctialExDot(), relativelyCloseTo(orbit.getEquinoctialExDot(), 1));
1024 MatcherAssert.assertThat(rebuilt.getEquinoctialEyDot(), relativelyCloseTo(orbit.getEquinoctialEyDot(), 1));
1025 MatcherAssert.assertThat(rebuilt.getHxDot(), relativelyCloseTo(orbit.getHxDot(), 1));
1026 MatcherAssert.assertThat(rebuilt.getHyDot(), relativelyCloseTo(orbit.getHyDot(), 1));
1027 for (PositionAngle type2 : PositionAngle.values()) {
1028 MatcherAssert.assertThat(rebuilt.getL(type2), relativelyCloseTo(orbit.getL(type2), 1));
1029 MatcherAssert.assertThat(rebuilt.getLDot(type2), relativelyCloseTo(orbit.getLDot(type2), 1));
1030 }
1031 }
1032
1033 }
1034
1035 @Test
1036 public void testEquatorialRetrograde() {
1037 Vector3D position = new Vector3D(10000000.0, 0.0, 0.0);
1038 Vector3D velocity = new Vector3D(0.0, -6500.0, 0.0);
1039 double r2 = position.getNormSq();
1040 double r = FastMath.sqrt(r2);
1041 Vector3D acceleration = new Vector3D(-mu / (r * r2), position,
1042 1, new Vector3D(-0.1, 0.2, 0.3));
1043 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity, acceleration);
1044
1045
1046
1047 EquinoctialOrbit orbit = new EquinoctialOrbit(new KeplerianOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu));
1048 Assert.assertEquals(10637829.465, orbit.getA(), 1.0e-3);
1049 Assert.assertEquals(-738.145, orbit.getADot(), 1.0e-3);
1050 Assert.assertEquals(0.05995861, orbit.getE(), 1.0e-8);
1051 Assert.assertEquals(-6.523e-5, orbit.getEDot(), 1.0e-8);
1052 Assert.assertTrue(Double.isNaN(orbit.getI()));
1053 Assert.assertTrue(Double.isNaN(orbit.getIDot()));
1054 Assert.assertTrue(Double.isNaN(orbit.getHx()));
1055 Assert.assertTrue(Double.isNaN(orbit.getHxDot()));
1056 Assert.assertTrue(Double.isNaN(orbit.getHy()));
1057 Assert.assertTrue(Double.isNaN(orbit.getHyDot()));
1058 }
1059
1060 @Test
1061 public void testDerivativesConversionSymmetry() {
1062 final AbsoluteDate date = new AbsoluteDate("2003-05-01T00:01:20.000", TimeScalesFactory.getUTC());
1063 Vector3D position = new Vector3D(6893443.400234382, 1886406.1073757345, -589265.1150359757);
1064 Vector3D velocity = new Vector3D(-281.1261461082365, -1231.6165642450928, -7348.756363469432);
1065 Vector3D acceleration = new Vector3D(-7.460341170581685, -2.0415957334584527, 0.6393322823627762);
1066 PVCoordinates pvCoordinates = new PVCoordinates( position, velocity, acceleration);
1067 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(),
1068 date, Constants.EIGEN5C_EARTH_MU);
1069 Assert.assertTrue(orbit.hasDerivatives());
1070 double r2 = position.getNormSq();
1071 double r = FastMath.sqrt(r2);
1072 Vector3D keplerianAcceleration = new Vector3D(-orbit.getMu() / (r2 * r), position);
1073 Assert.assertEquals(0.0101, Vector3D.distance(keplerianAcceleration, acceleration), 1.0e-4);
1074
1075 for (OrbitType type : OrbitType.values()) {
1076 Orbit converted = type.convertType(orbit);
1077 Assert.assertTrue(converted.hasDerivatives());
1078 EquinoctialOrbit rebuilt = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(converted);
1079 Assert.assertTrue(rebuilt.hasDerivatives());
1080 Assert.assertEquals(orbit.getADot(), rebuilt.getADot(), 3.0e-13);
1081 Assert.assertEquals(orbit.getEquinoctialExDot(), rebuilt.getEquinoctialExDot(), 1.0e-15);
1082 Assert.assertEquals(orbit.getEquinoctialEyDot(), rebuilt.getEquinoctialEyDot(), 1.0e-15);
1083 Assert.assertEquals(orbit.getHxDot(), rebuilt.getHxDot(), 1.0e-15);
1084 Assert.assertEquals(orbit.getHyDot(), rebuilt.getHyDot(), 1.0e-15);
1085 Assert.assertEquals(orbit.getLvDot(), rebuilt.getLvDot(), 1.0e-15);
1086 }
1087
1088 }
1089
1090 @Test
1091 public void testToString() {
1092 Vector3D position = new Vector3D(-29536113.0, 30329259.0, -100125.0);
1093 Vector3D velocity = new Vector3D(-2194.0, -2141.0, -8.0);
1094 PVCoordinates pvCoordinates = new PVCoordinates(position, velocity);
1095 EquinoctialOrbit orbit = new EquinoctialOrbit(pvCoordinates, FramesFactory.getEME2000(), date, mu);
1096 Assert.assertEquals("equinoctial parameters: {a: 4.225517000282565E7; ex: 5.927324978565528E-4; ey: -0.002062743969643666; hx: 6.401103130239252E-5; hy: -0.0017606836670756732; lv: 134.24111947709974;}",
1097 orbit.toString());
1098 }
1099
1100 @Test
1101 public void testCopyNonKeplerianAcceleration() {
1102
1103 final Frame eme2000 = FramesFactory.getEME2000();
1104
1105
1106 final Vector3D position = new Vector3D(42164140, 0, 0);
1107
1108 final PVCoordinates pv = new PVCoordinates(position,
1109 new Vector3D(0, FastMath.sqrt(mu / position.getNorm()), 0));
1110
1111 final Orbit orbit = new EquinoctialOrbit(pv, eme2000, date, mu);
1112
1113
1114 final Orbit orbitCopy = new EquinoctialOrbit(orbit);
1115
1116
1117 final Orbit shiftedOrbit = orbit.shiftedBy(10);
1118 final Orbit shiftedOrbitCopy = orbitCopy.shiftedBy(10);
1119
1120 Assert.assertEquals(0.0,
1121 Vector3D.distance(shiftedOrbit.getPVCoordinates().getPosition(),
1122 shiftedOrbitCopy.getPVCoordinates().getPosition()),
1123 1.0e-10);
1124 Assert.assertEquals(0.0,
1125 Vector3D.distance(shiftedOrbit.getPVCoordinates().getVelocity(),
1126 shiftedOrbitCopy.getPVCoordinates().getVelocity()),
1127 1.0e-10);
1128
1129 }
1130
1131 @Test
1132 public void testNormalize() {
1133 EquinoctialOrbit withoutDerivatives =
1134 new EquinoctialOrbit(42166712.0, 0.005, -0.025, 0.17, 0.34,
1135 0.4, PositionAngle.MEAN,
1136 FramesFactory.getEME2000(), date, mu);
1137 EquinoctialOrbit ref =
1138 new EquinoctialOrbit(24000000.0, -0.012, 0.01, 0.2, 0.1,
1139 -6.28, PositionAngle.MEAN,
1140 FramesFactory.getEME2000(), date, mu);
1141
1142 EquinoctialOrbit normalized1 = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.normalize(withoutDerivatives, ref);
1143 Assert.assertFalse(normalized1.hasDerivatives());
1144 Assert.assertEquals(0.0, normalized1.getA() - withoutDerivatives.getA(), 1.0e-6);
1145 Assert.assertEquals(0.0, normalized1.getEquinoctialEx() - withoutDerivatives.getEquinoctialEx(), 1.0e-10);
1146 Assert.assertEquals(0.0, normalized1.getEquinoctialEy() - withoutDerivatives.getEquinoctialEy(), 1.0e-10);
1147 Assert.assertEquals(0.0, normalized1.getHx() - withoutDerivatives.getHx(), 1.0e-10);
1148 Assert.assertEquals(0.0, normalized1.getHy() - withoutDerivatives.getHy(), 1.0e-10);
1149 Assert.assertEquals(-MathUtils.TWO_PI, normalized1.getLv() - withoutDerivatives.getLv(), 1.0e-10);
1150 Assert.assertTrue(Double.isNaN(normalized1.getADot()));
1151 Assert.assertTrue(Double.isNaN(normalized1.getEquinoctialExDot()));
1152 Assert.assertTrue(Double.isNaN(normalized1.getEquinoctialEyDot()));
1153 Assert.assertTrue(Double.isNaN(normalized1.getHxDot()));
1154 Assert.assertTrue(Double.isNaN(normalized1.getHyDot()));
1155 Assert.assertTrue(Double.isNaN(normalized1.getLvDot()));
1156
1157 double[] p = new double[6];
1158 OrbitType.EQUINOCTIAL.mapOrbitToArray(withoutDerivatives, PositionAngle.TRUE, p, null);
1159 EquinoctialOrbit withDerivatives = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.mapArrayToOrbit(p,
1160 new double[] { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 },
1161 PositionAngle.TRUE,
1162 withoutDerivatives.getDate(),
1163 withoutDerivatives.getMu(),
1164 withoutDerivatives.getFrame());
1165 EquinoctialOrbit normalized2 = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.normalize(withDerivatives, ref);
1166 Assert.assertTrue(normalized2.hasDerivatives());
1167 Assert.assertFalse(normalized1.hasDerivatives());
1168 Assert.assertEquals(0.0, normalized1.getA() - withoutDerivatives.getA(), 1.0e-6);
1169 Assert.assertEquals(0.0, normalized1.getEquinoctialEx() - withoutDerivatives.getEquinoctialEx(), 1.0e-10);
1170 Assert.assertEquals(0.0, normalized1.getEquinoctialEy() - withoutDerivatives.getEquinoctialEy(), 1.0e-10);
1171 Assert.assertEquals(0.0, normalized1.getHx() - withoutDerivatives.getHx(), 1.0e-10);
1172 Assert.assertEquals(0.0, normalized1.getHy() - withoutDerivatives.getHy(), 1.0e-10);
1173 Assert.assertEquals(-MathUtils.TWO_PI, normalized1.getLv() - withoutDerivatives.getLv(), 1.0e-10);
1174 Assert.assertEquals(0.0, normalized2.getADot() - withDerivatives.getADot(), 1.0e-10);
1175 Assert.assertEquals(0.0, normalized2.getEquinoctialExDot() - withDerivatives.getEquinoctialExDot(), 1.0e-10);
1176 Assert.assertEquals(0.0, normalized2.getEquinoctialEyDot() - withDerivatives.getEquinoctialEyDot(), 1.0e-10);
1177 Assert.assertEquals(0.0, normalized2.getHxDot() - withDerivatives.getHxDot(), 1.0e-10);
1178 Assert.assertEquals(0.0, normalized2.getHyDot() - withDerivatives.getHyDot(), 1.0e-10);
1179 Assert.assertEquals(0.0, normalized2.getLvDot() - withDerivatives.getLvDot(), 1.0e-10);
1180
1181 }
1182
1183 @Before
1184 public void setUp() {
1185
1186 Utils.setDataRoot("regular-data");
1187
1188
1189 date = AbsoluteDate.J2000_EPOCH;
1190
1191
1192 mu = 3.9860047e14;
1193 }
1194
1195 @After
1196 public void tearDown() {
1197 date = null;
1198 }
1199
1200 }