1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.polynomials.PolynomialFunction;
22 import org.hipparchus.complex.Complex;
23 import org.hipparchus.complex.ComplexField;
24 import org.hipparchus.exception.LocalizedCoreFormats;
25 import org.hipparchus.exception.MathIllegalStateException;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Rotation;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.ode.FieldODEIntegrator;
31 import org.hipparchus.ode.events.Action;
32 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
33 import org.hipparchus.util.Binary64Field;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.MathArrays;
36 import org.hipparchus.util.Precision;
37 import org.junit.jupiter.api.Assertions;
38 import org.junit.jupiter.api.BeforeEach;
39 import org.junit.jupiter.api.Test;
40 import org.orekit.Utils;
41 import org.orekit.attitudes.BodyCenterPointing;
42 import org.orekit.attitudes.FieldAttitude;
43 import org.orekit.bodies.OneAxisEllipsoid;
44 import org.orekit.errors.OrekitException;
45 import org.orekit.errors.OrekitIllegalArgumentException;
46 import org.orekit.errors.OrekitMessages;
47 import org.orekit.frames.FieldStaticTransform;
48 import org.orekit.frames.FieldTransform;
49 import org.orekit.frames.FramesFactory;
50 import org.orekit.frames.Transform;
51 import org.orekit.orbits.FieldCartesianOrbit;
52 import org.orekit.orbits.FieldKeplerianOrbit;
53 import org.orekit.orbits.FieldOrbit;
54 import org.orekit.orbits.KeplerianOrbit;
55 import org.orekit.orbits.Orbit;
56 import org.orekit.orbits.PositionAngleType;
57 import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator;
58 import org.orekit.propagation.analytical.FieldKeplerianPropagator;
59 import org.orekit.propagation.events.FieldDateDetector;
60 import org.orekit.propagation.events.FieldEventDetector;
61 import org.orekit.propagation.events.handlers.FieldEventHandler;
62 import org.orekit.propagation.numerical.FieldNumericalPropagator;
63 import org.orekit.time.AbsoluteDate;
64 import org.orekit.time.DateComponents;
65 import org.orekit.time.FieldAbsoluteDate;
66 import org.orekit.time.TimeComponents;
67 import org.orekit.time.TimeScalesFactory;
68 import org.orekit.utils.Constants;
69 import org.orekit.utils.FieldAbsolutePVCoordinates;
70 import org.orekit.utils.FieldArrayDictionary;
71 import org.orekit.utils.FieldPVCoordinates;
72 import org.orekit.utils.IERSConventions;
73 import org.orekit.utils.PVCoordinates;
74 import org.orekit.utils.TimeStampedAngularCoordinates;
75 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
76
77
78 class FieldSpacecraftStateTest {
79
80 @Test
81 void testFieldVSReal() {
82 doTestFieldVsReal(Binary64Field.getInstance());
83 }
84
85 @Test
86 void testShiftVsEcksteinHechlerError() {
87 doTestShiftVsEcksteinHechlerError(Binary64Field.getInstance());
88 }
89
90 @Test
91 void testDatesConsistency() {
92 Assertions.assertThrows(IllegalArgumentException.class, () -> {
93 doTestDatesConsistency(Binary64Field.getInstance());
94 });
95 }
96
97 @Test
98 void testDateConsistencyClose() {
99 doTestDateConsistencyClose(Binary64Field.getInstance());
100 }
101
102 @Test
103 void testFramesConsistency() {
104 Assertions.assertThrows(IllegalArgumentException.class, () -> {
105 doTestFramesConsistency(Binary64Field.getInstance());
106 });
107 }
108
109 @Test
110 void testTransform() {
111 doTestTransform(Binary64Field.getInstance());
112 }
113
114 @Test
115 void testAdditionalStates() {
116 doTestAdditionalStates(Binary64Field.getInstance());
117 }
118
119 @Test
120 void testAdditionalStatesDerivatives() {
121 doTestAdditionalStatesDerivatives(Binary64Field.getInstance());
122 }
123
124 @Test
125 void testFieldVSRealAbsPV() {
126 doTestFieldVsRealAbsPV(Binary64Field.getInstance());
127 }
128
129 @Test
130 void testDateConsistencyCloseAbsPV() {
131 doTestDateConsistencyCloseAbsPV(Binary64Field.getInstance());
132 }
133
134 @Test
135 void testFramesConsistencyAbsPV() {
136 Assertions.assertThrows(IllegalArgumentException.class, () -> {
137 doTestFramesConsistencyAbsPV(Binary64Field.getInstance());
138 });
139 }
140
141 @Test
142 void testAdditionalStatesAbsPV() {
143 doTestAdditionalStatesAbsPV(Binary64Field.getInstance());
144 }
145
146 @Test
147 void testAdditionalStatesDerivativesAbsPV() {
148 doTestAdditionalStatesDerivativesAbsPV(Binary64Field.getInstance());
149 }
150
151 @Test
152 void testResetOnEventAnalytical() {
153 doTestAdditionalTestResetOnEventAnalytical(Binary64Field.getInstance());
154 }
155
156 @Test
157 void testResetOnEventNumerical() {
158 doTestAdditionalTestResetOnEventNumerical(Binary64Field.getInstance());
159 }
160
161 @Test
162 void testShiftAdditionalDerivativesDouble() {
163 doTestShiftAdditionalDerivativesDouble(Binary64Field.getInstance());
164 }
165
166 @Test
167 void testShiftAdditionalDerivativesField() {
168 doTestShiftAdditionalDerivativesField(Binary64Field.getInstance());
169 }
170
171 @Test
172 void testToTransform() {
173
174 final ComplexField field = ComplexField.getInstance();
175 final FieldOrbit<Complex> orbit = new FieldCartesianOrbit<>(field, rOrbit);
176 final TimeStampedAngularCoordinates angularCoordinates = new TimeStampedAngularCoordinates(
177 orbit.getDate().toAbsoluteDate(), Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
178 final FieldAttitude<Complex> attitude = new FieldAttitude<>(orbit.getFrame(),
179 new TimeStampedFieldAngularCoordinates<>(field, angularCoordinates));
180 final FieldSpacecraftState<Complex> state = new FieldSpacecraftState<>(orbit, attitude);
181
182 final FieldTransform<Complex> fieldTransform = state.toTransform();
183
184 final Transform expectedTransform = state.toSpacecraftState().toTransform();
185 Assertions.assertEquals(expectedTransform.getDate(), fieldTransform.getDate());
186 final double tolerance = 1e-10;
187 Assertions.assertEquals(expectedTransform.getTranslation().getX(),
188 fieldTransform.getTranslation().getX().getReal(), tolerance);
189 Assertions.assertEquals(expectedTransform.getTranslation().getY(),
190 fieldTransform.getTranslation().getY().getReal(), tolerance);
191 Assertions.assertEquals(expectedTransform.getTranslation().getZ(),
192 fieldTransform.getTranslation().getZ().getReal(), tolerance);
193 Assertions.assertEquals(0., Rotation.distance(expectedTransform.getRotation(),
194 fieldTransform.getRotation().toRotation()));
195 }
196
197 @Test
198 void testToStaticTransform() {
199
200 final ComplexField field = ComplexField.getInstance();
201 final FieldOrbit<Complex> orbit = new FieldCartesianOrbit<>(field, rOrbit);
202 final TimeStampedAngularCoordinates angularCoordinates = new TimeStampedAngularCoordinates(
203 orbit.getDate().toAbsoluteDate(), Rotation.IDENTITY, Vector3D.ZERO, Vector3D.ZERO);
204 final FieldAttitude<Complex> attitude = new FieldAttitude<>(orbit.getFrame(),
205 new TimeStampedFieldAngularCoordinates<>(field, angularCoordinates));
206 final FieldSpacecraftState<Complex> state = new FieldSpacecraftState<>(orbit, attitude);
207
208 final FieldStaticTransform<Complex> actualStaticTransform = state.toStaticTransform();
209
210 final FieldStaticTransform<Complex> expectedStaticTransform = state.toTransform();
211 Assertions.assertEquals(expectedStaticTransform.getDate(), actualStaticTransform.getDate());
212 final double tolerance = 1e-10;
213 Assertions.assertEquals(expectedStaticTransform.getTranslation().getX().getReal(),
214 actualStaticTransform.getTranslation().getX().getReal(), tolerance);
215 Assertions.assertEquals(expectedStaticTransform.getTranslation().getY().getReal(),
216 actualStaticTransform.getTranslation().getY().getReal(), tolerance);
217 Assertions.assertEquals(expectedStaticTransform.getTranslation().getZ().getReal(),
218 actualStaticTransform.getTranslation().getZ().getReal(), tolerance);
219 Assertions.assertEquals(0., Rotation.distance(expectedStaticTransform.getRotation().toRotation(),
220 actualStaticTransform.getRotation().toRotation()));
221 }
222
223 private <T extends CalculusFieldElement<T>> void doTestFieldVsReal(final Field<T> field) {
224 T zero = field.getZero();
225
226 double mu = 3.9860047e14;
227
228 T a_f = zero.add(150000);
229 T e_f = zero.add( 0);
230 T i_f = zero.add( 0);
231 T pa_f = zero.add( 0);
232 T raan_f = zero.add( 0);
233 T m_f = zero.add( 0);
234
235 FieldAbsoluteDate<T> t_f = new FieldAbsoluteDate<>(field);
236
237 double a_r = a_f.getReal();
238 double e_r = e_f.getReal();
239 double i_r = i_f.getReal();
240 double pa_r = pa_f.getReal();
241 double raan_r = raan_f.getReal();
242 double m_r = m_f.getReal();
243
244 AbsoluteDate t_r = t_f.toAbsoluteDate();
245
246
247 KeplerianOrbit kep_r = new KeplerianOrbit(a_r, e_r, i_r, pa_r, raan_r, m_r, PositionAngleType.ECCENTRIC, FramesFactory.getEME2000(), t_r, mu);
248 FieldKeplerianOrbit<T> kep_f = new FieldKeplerianOrbit<>(a_f, e_f, i_f, pa_f, raan_f, m_f, PositionAngleType.ECCENTRIC, FramesFactory.getEME2000(), t_f, zero.add(mu));
249
250 SpacecraftState ScS_r = new SpacecraftState(kep_r);
251 FieldSpacecraftState<T> ScS_f = new FieldSpacecraftState<>(kep_f);
252
253 for (double dt = 0; dt < 500; dt+=100){
254 SpacecraftState control_r = ScS_r.shiftedBy(dt);
255 FieldSpacecraftState<T> control_f = ScS_f.shiftedBy(zero.add(dt));
256
257
258 Assertions.assertEquals(control_r.getA(), control_f.getA().getReal(), 1e-10);
259 Assertions.assertEquals(control_r.getE(), control_f.getE().getReal(), 1e-10);
260 Assertions.assertEquals(control_r.getEquinoctialEx(), control_f.getEquinoctialEx().getReal(), 1e-10);
261 Assertions.assertEquals(control_r.getEquinoctialEy(), control_f.getEquinoctialEy().getReal(), 1e-10);
262 Assertions.assertEquals(control_r.getPosition().getX(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getX(), 1e-10);
263 Assertions.assertEquals(control_r.getPosition().getY(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getY(), 1e-10);
264 Assertions.assertEquals(control_r.getPosition().getZ(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getZ(), 1e-10);
265 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getX(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getX(), 1e-10);
266 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getY(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getY(), 1e-10);
267 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getZ(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getZ(), 1e-10);
268 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getX(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getX(), 1e-10);
269 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getY(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getY(), 1e-10);
270 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getZ(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getZ(), 1e-10);
271 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ0(), control_f.getAttitude().getOrientation().getRotation().getQ0().getReal(), 1e-10);
272 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ1(), control_f.getAttitude().getOrientation().getRotation().getQ1().getReal(), 1e-10);
273 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ2(), control_f.getAttitude().getOrientation().getRotation().getQ2().getReal(), 1e-10);
274 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ3(), control_f.getAttitude().getOrientation().getRotation().getQ3().getReal(), 1e-10);
275
276 Assertions.assertEquals(control_r.getAttitude().getSpin().getAlpha(), control_f.getAttitude().getSpin().getAlpha().getReal(), 1e-10);
277 Assertions.assertEquals(control_r.getAttitude().getSpin().getDelta(), control_f.getAttitude().getSpin().getDelta().getReal(), 1e-10);
278 Assertions.assertEquals(control_r.getAttitude().getSpin().getNorm(), control_f.getAttitude().getSpin().getNorm().getReal(), 1e-10);
279
280 Assertions.assertEquals(control_r.getAttitude().getReferenceFrame().isPseudoInertial(), control_f.getAttitude().getReferenceFrame().isPseudoInertial());
281 Assertions.assertEquals(control_r.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH), control_f.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH).getReal(), 1e-10);
282
283
284 }
285
286 }
287
288 private <T extends CalculusFieldElement<T>> void doTestShiftVsEcksteinHechlerError(final Field<T> field)
289 {
290
291 T zero = field.getZero();
292 T mass = zero.add(2500.);
293 T a = zero.add(rOrbit.getA());
294 T e = zero.add(rOrbit.getE());
295 T i = zero.add(rOrbit.getI());
296 T pa = zero.add(1.9674147913622104);
297 T raan = zero.add(FastMath.toRadians(261));
298 T lv = zero.add(0);
299 final double ae = 6.378137e6;
300 final double c20 = -1.08263e-3;
301 final double c30 = 2.54e-6;
302 final double c40 = 1.62e-6;
303 final double c50 = 2.3e-7;
304 final double c60 = -5.5e-7;
305
306
307
308
309
310
311
312
313
314
315
316 PolynomialFunction pModel = new PolynomialFunction(new double[] {
317 1.5664070631933846e-01, 7.5504722733047560e-03, -8.2460562451009510e-05,
318 6.9546332080305580e-06, -1.7045365367533077e-09, -4.2187860791066264e-13
319 });
320 PolynomialFunction vModel = new PolynomialFunction(new double[] {
321 -3.5472364019908720e-04, 1.6568103861124980e-05, 1.9637913327830596e-05,
322 -3.4248792843039766e-09, -5.6565135131014254e-12, 1.4730170946808630e-15
323 });
324 PolynomialFunction aModel = new PolynomialFunction(new double[] {
325 3.0731707577766896e-06, 3.9770746399850350e-05, 1.9779039254538660e-09,
326 8.0263328220724900e-12, -1.5600835252366078e-14, 1.1785257001549687e-18
327 });
328 PolynomialFunction rModel = new PolynomialFunction(new double[] {
329 -2.7689062063188115e-06, 1.7406542538258334e-07, 2.5109795349592287e-09,
330 2.0399322661074575e-11, 9.9126348912426750e-15, -3.5015638905729510e-18
331 });
332
333 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
334 TimeComponents.H00,
335 TimeScalesFactory.getUTC());
336
337 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
338 FramesFactory.getEME2000(), date, zero.add(mu));
339
340 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
341
342 FieldPropagator<T> propagator =
343 new FieldEcksteinHechlerPropagator<>(orbit, attitudeLaw, mass,
344 ae, zero.add(mu), c20, c30, c40, c50, c60);
345
346 FieldAbsoluteDate<T> centerDate = orbit.getDate().shiftedBy(100.0);
347
348 FieldSpacecraftState<T> centerState = propagator.propagate(centerDate);
349
350 double maxResidualP = 0;
351 double maxResidualV = 0;
352 double maxResidualA = 0;
353 double maxResidualR = 0;
354 for (T dt = field.getZero(); dt.getReal() < 900.0; dt = dt.add(5)) {
355
356 FieldSpacecraftState<T> shifted = centerState.shiftedBy(dt);
357 FieldSpacecraftState<T> propagated = propagator.propagate(centerDate.shiftedBy(dt));
358 FieldPVCoordinates<T> dpv = new FieldPVCoordinates<>(propagated.getPVCoordinates(), shifted.getPVCoordinates());
359
360
361 double residualP = pModel.value(dt.getReal()) - dpv.getPosition().getNorm().getReal();
362 double residualV = vModel.value(dt.getReal()) - dpv.getVelocity().getNorm().getReal();
363 double residualA = aModel.value(dt.getReal()) - dpv.getAcceleration().getNorm().getReal();
364 double residualR = rModel.value(dt.getReal()) -
365 FastMath.toDegrees(FieldRotation.distance(shifted.getAttitude().getRotation(),
366 propagated.getAttitude().getRotation()).getReal());
367 maxResidualP = FastMath.max(maxResidualP, FastMath.abs(residualP));
368 maxResidualV = FastMath.max(maxResidualV, FastMath.abs(residualV));
369 maxResidualA = FastMath.max(maxResidualA, FastMath.abs(residualA));
370 maxResidualR = FastMath.max(maxResidualR, FastMath.abs(residualR));
371
372 }
373
374 Assertions.assertEquals(0.40, maxResidualP, 0.01);
375 Assertions.assertEquals(4.9e-4, maxResidualV, 1.0e-5);
376 Assertions.assertEquals(2.8e-6, maxResidualR, 1.0e-1);
377
378 }
379
380 private <T extends CalculusFieldElement<T>> void doTestDatesConsistency(final Field<T> field) {
381
382 T zero = field.getZero();
383 T a = zero.add(rOrbit.getA());
384 T e = zero.add(rOrbit.getE());
385 T i = zero.add(rOrbit.getI());
386 T pa = zero.add(1.9674147913622104);
387 T raan = zero.add(FastMath.toRadians(261));
388 T lv = zero.add(0);
389
390
391 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
392 TimeComponents.H00,
393 TimeScalesFactory.getUTC());
394
395 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
396 FramesFactory.getEME2000(), date, zero.add(mu));
397 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
398
399 new FieldSpacecraftState<>(orbit, attitudeLaw.getAttitude(orbit.shiftedBy(zero.add(10.0)),
400 orbit.getDate().shiftedBy(10.0),
401 orbit.getFrame()));
402 }
403
404
405
406
407
408 private <T extends CalculusFieldElement<T>> void doTestDateConsistencyClose(final Field<T> field) {
409
410
411
412 T zero = field.getZero();
413 T one = field.getOne();
414 T a = zero.add(rOrbit.getA());
415 T e = zero.add(rOrbit.getE());
416 T i = zero.add(rOrbit.getI());
417 T pa = zero.add(1.9674147913622104);
418 T raan = zero.add(FastMath.toRadians(261));
419 T lv = zero.add(0);
420
421 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
422 TimeComponents.H00,
423 TimeScalesFactory.getUTC());
424 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
425 FramesFactory.getEME2000(), date, zero.add(mu));
426
427 FieldKeplerianOrbit<T> orbit10Shifts = orbit;
428 for (int ii = 0; ii < 10; ii++) {
429 orbit10Shifts = orbit10Shifts.shiftedBy(zero.add(0.1));
430 }
431 final FieldOrbit<T> orbit1Shift = orbit.shiftedBy(one);
432
433 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
434
435 FieldAttitude<T> shiftedAttitude = attitudeLaw
436 .getAttitude(orbit1Shift, orbit1Shift.getDate(), orbit.getFrame());
437
438
439 Assertions.assertNotEquals(shiftedAttitude.getDate(), orbit10Shifts.getDate());
440 Assertions.assertEquals(
441 shiftedAttitude.getDate().durationFrom(orbit10Shifts.getDate()).getReal(),
442 0, Precision.EPSILON);
443
444
445 new FieldSpacecraftState<>(orbit10Shifts, shiftedAttitude);
446 }
447
448
449 private <T extends CalculusFieldElement<T>> void doTestFramesConsistency(final Field<T> field) {
450
451 T zero = field.getZero();
452 T a = zero.add(rOrbit.getA());
453 T e = zero.add(rOrbit.getE());
454 T i = zero.add(rOrbit.getI());
455 T pa = zero.add(1.9674147913622104);
456 T raan = zero.add(FastMath.toRadians(261));
457 T lv = zero.add(0);
458
459 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
460 TimeComponents.H00,
461 TimeScalesFactory.getUTC());
462 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
463 FramesFactory.getEME2000(), date, zero.add(mu));
464
465 new FieldSpacecraftState<>(orbit,
466 new FieldAttitude<>(orbit.getDate(),
467 FramesFactory.getGCRF(),
468 Rotation.IDENTITY,
469 Vector3D.ZERO,
470 Vector3D.ZERO,
471 field));
472 }
473
474 private <T extends CalculusFieldElement<T>> void doTestTransform(final Field<T> field) {
475
476 T zero = field.getZero();
477 T a = zero.add(rOrbit.getA());
478 T e = zero.add(rOrbit.getE());
479 T i = zero.add(rOrbit.getI());
480 T pa = zero.add(1.9674147913622104);
481 T raan = zero.add(FastMath.toRadians(261));
482 T lv = zero.add(0);
483 T mass = zero.add(2500);
484
485 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
486 TimeComponents.H00,
487 TimeScalesFactory.getUTC());
488
489 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
490 FramesFactory.getEME2000(), date, zero.add(mu));
491
492 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
493
494 FieldKeplerianPropagator<T> propagator =
495 new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
496
497 double maxDP = 0;
498 double maxDV = 0;
499 double maxDA = 0;
500 for (double t = 0; t < orbit.getKeplerianPeriod().getReal(); t += 60) {
501 final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(zero.add(t)));
502 final Transform transform = state.toSpacecraftState().toTransform().getInverse();
503 PVCoordinates pv = transform.transformPVCoordinates(PVCoordinates.ZERO);
504 PVCoordinates dPV = new PVCoordinates(pv, state.getPVCoordinates().toPVCoordinates());
505 Vector3D mZDirection = transform.transformVector(Vector3D.MINUS_K);
506 double alpha = Vector3D.angle(mZDirection, state.getPosition().toVector3D());
507 maxDP = FastMath.max(maxDP, dPV.getPosition().getNorm());
508 maxDV = FastMath.max(maxDV, dPV.getVelocity().getNorm());
509 maxDA = FastMath.max(maxDA, FastMath.toDegrees(alpha));
510 }
511 Assertions.assertEquals(0.0, maxDP, 1.0e-6);
512 Assertions.assertEquals(0.0, maxDV, 1.0e-9);
513 Assertions.assertEquals(0.0, maxDA, 8.1e-10);
514
515 }
516
517 private <T extends CalculusFieldElement<T>> void doTestAdditionalStates(final Field<T> field) {
518
519 T zero = field.getZero();
520 T a = zero.add(rOrbit.getA());
521 T e = zero.add(rOrbit.getE());
522 T i = zero.add(rOrbit.getI());
523 T pa = zero.add(1.9674147913622104);
524 T raan = zero.add(FastMath.toRadians(261));
525 T lv = zero.add(0);
526 T mass = zero.add(2500);
527
528 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
529 TimeComponents.H00,
530 TimeScalesFactory.getUTC());
531
532 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
533 FramesFactory.getEME2000(), date, zero.add(mu));
534
535 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
536
537 FieldKeplerianPropagator<T> propagator =
538 new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
539
540
541
542
543 final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(60));
544 T[] add = MathArrays.buildArray(field, 2);
545 add[0] = zero.add(1.);
546 add[1] = zero.add(2.);
547 final FieldSpacecraftState<T> extended =
548 state.
549 addAdditionalState("test-1", add).
550 addAdditionalState("test-2", zero.add(42.0));
551 Assertions.assertEquals(0, state.getAdditionalStatesValues().size());
552 Assertions.assertFalse(state.hasAdditionalState("test-1"));
553 try {
554 state.getAdditionalState("test-1");
555 Assertions.fail("an exception should have been thrown");
556 } catch (OrekitException oe) {
557 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
558 Assertions.assertEquals(oe.getParts()[0], "test-1");
559 }
560 try {
561 state.ensureCompatibleAdditionalStates(extended);
562 Assertions.fail("an exception should have been thrown");
563 } catch (OrekitException oe) {
564 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
565 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
566 }
567 try {
568 extended.ensureCompatibleAdditionalStates(state);
569 Assertions.fail("an exception should have been thrown");
570 } catch (OrekitException oe) {
571 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
572 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
573 }
574 try {
575 T[] kk = MathArrays.buildArray(field, 7);
576 extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk));
577 Assertions.fail("an exception should have been thrown");
578 } catch (MathIllegalStateException mise) {
579 Assertions.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
580 Assertions.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
581 }
582 Assertions.assertEquals(2, extended.getAdditionalStatesValues().size());
583 Assertions.assertTrue(extended.hasAdditionalState("test-1"));
584 Assertions.assertTrue(extended.hasAdditionalState("test-2"));
585 Assertions.assertEquals( 1.0, extended.getAdditionalState("test-1")[0].getReal(), 1.0e-15);
586 Assertions.assertEquals( 2.0, extended.getAdditionalState("test-1")[1].getReal(), 1.0e-15);
587 Assertions.assertEquals(42.0, extended.getAdditionalState("test-2")[0].getReal(), 1.0e-15);
588
589
590 T[] dd = MathArrays.buildArray(field, 1);
591 dd[0] = zero.add(-6.0);
592 FieldArrayDictionary<T> dictionary = new FieldArrayDictionary<>(field);
593 dictionary.put("test-3", dd);
594 FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getOrbit(), dictionary);
595 Assertions.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
596 FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), dictionary);
597 Assertions.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
598 FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getOrbit(), state.getMass(), dictionary);
599 Assertions.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
600 FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), state.getMass(), dictionary);
601 Assertions.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
602 FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
603 Assertions.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
604
605 }
606
607 private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesDerivatives(final Field<T> field) {
608
609 T zero = field.getZero();
610 T a = zero.add(rOrbit.getA());
611 T e = zero.add(rOrbit.getE());
612 T i = zero.add(rOrbit.getI());
613 T pa = zero.add(1.9674147913622104);
614 T raan = zero.add(FastMath.toRadians(261));
615 T lv = zero.add(0);
616 T mass = zero.add(2500);
617
618 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
619 TimeComponents.H00,
620 TimeScalesFactory.getUTC());
621
622 FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngleType.TRUE,
623 FramesFactory.getEME2000(), date, zero.add(mu));
624
625 BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
626
627 FieldKeplerianPropagator<T> propagator =
628 new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
629
630
631
632
633 final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(60));
634 T[] add = MathArrays.buildArray(field, 2);
635 add[0] = zero.add(1.);
636 add[1] = zero.add(2.);
637 final FieldSpacecraftState<T> extended =
638 state.
639 addAdditionalStateDerivative("test-1", add).
640 addAdditionalStateDerivative("test-2", zero.add(42.0));
641 Assertions.assertEquals(0, state.getAdditionalStatesDerivatives().size());
642 Assertions.assertFalse(state.hasAdditionalStateDerivative("test-1"));
643 try {
644 state.getAdditionalStateDerivative("test-1");
645 Assertions.fail("an exception should have been thrown");
646 } catch (OrekitException oe) {
647 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
648 Assertions.assertEquals(oe.getParts()[0], "test-1");
649 }
650 try {
651 state.ensureCompatibleAdditionalStates(extended);
652 Assertions.fail("an exception should have been thrown");
653 } catch (OrekitException oe) {
654 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
655 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
656 }
657 try {
658 extended.ensureCompatibleAdditionalStates(state);
659 Assertions.fail("an exception should have been thrown");
660 } catch (OrekitException oe) {
661 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
662 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
663 }
664 try {
665 T[] kk = MathArrays.buildArray(field, 7);
666 extended.ensureCompatibleAdditionalStates(extended.addAdditionalStateDerivative("test-2", kk));
667 Assertions.fail("an exception should have been thrown");
668 } catch (MathIllegalStateException mise) {
669 Assertions.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
670 Assertions.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
671 }
672 Assertions.assertEquals(2, extended.getAdditionalStatesDerivatives().size());
673 Assertions.assertTrue(extended.hasAdditionalStateDerivative("test-1"));
674 Assertions.assertTrue(extended.hasAdditionalStateDerivative("test-2"));
675 Assertions.assertEquals( 1.0, extended.getAdditionalStateDerivative("test-1")[0].getReal(), 1.0e-15);
676 Assertions.assertEquals( 2.0, extended.getAdditionalStateDerivative("test-1")[1].getReal(), 1.0e-15);
677 Assertions.assertEquals(42.0, extended.getAdditionalStateDerivative("test-2")[0].getReal(), 1.0e-15);
678
679
680 T[] dd = MathArrays.buildArray(field, 1);
681 dd[0] = zero.add(-6.0);
682 FieldArrayDictionary<T> dict = new FieldArrayDictionary<>(field);
683 dict.put("test-3", dd);
684 FieldSpacecraftState<T> s = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), state.getMass(), null, dict);
685 Assertions.assertFalse(s.hasAdditionalState("test-3"));
686 Assertions.assertEquals(-6.0, s.getAdditionalStateDerivative("test-3")[0].getReal(), 1.0e-15);
687
688
689 FieldSpacecraftState<T> rebuilt = new FieldSpacecraftState<>(field, s.toSpacecraftState());
690 rebuilt.ensureCompatibleAdditionalStates(s);
691
692 }
693
694 private <T extends CalculusFieldElement<T>> void doTestFieldVsRealAbsPV(final Field<T> field) {
695 T zero = field.getZero();
696
697 T x_f = zero.add(0.8);
698 T y_f = zero.add(0.2);
699 T z_f = zero;
700 T vx_f = zero;
701 T vy_f = zero;
702 T vz_f = zero.add(0.1);
703
704 FieldAbsoluteDate<T> t_f = new FieldAbsoluteDate<>(field);
705
706 FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
707
708 FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), t_f, pva_f);
709
710 FieldSpacecraftState<T> ScS_f = new FieldSpacecraftState<>(absPV_f);
711 FieldSpacecraftState<T> withM = new FieldSpacecraftState<>(absPV_f, zero.newInstance(1234.5));
712 Assertions.assertEquals(1234.5, withM.getMass().getReal(), 1.0e-10);
713 SpacecraftState ScS_r = ScS_f.toSpacecraftState();
714
715 for (double dt = 0; dt < 500; dt+=100){
716 SpacecraftState control_r = ScS_r.shiftedBy(dt);
717 FieldSpacecraftState<T> control_f = ScS_f.shiftedBy(zero.add(dt));
718
719
720 Assertions.assertEquals(control_r.getMu(), control_f.getMu().getReal(), 1e-10);
721 Assertions.assertEquals(control_r.getI(), control_f.getI().getReal(), 1e-10);
722 Assertions.assertEquals(control_r.getHx(), control_f.getHx().getReal(), 1e-10);
723 Assertions.assertEquals(control_r.getHy(), control_f.getHy().getReal(), 1e-10);
724 Assertions.assertEquals(control_r.getLv(), control_f.getLv().getReal(), 1e-10);
725 Assertions.assertEquals(control_r.getLE(), control_f.getLE().getReal(), 1e-10);
726 Assertions.assertEquals(control_r.getLM(), control_f.getLM().getReal(), 1e-10);
727 Assertions.assertEquals(control_r.getKeplerianMeanMotion(), control_f.getKeplerianMeanMotion().getReal(), 1e-10);
728 Assertions.assertEquals(control_r.getKeplerianPeriod(), control_f.getKeplerianPeriod().getReal(), 1e-10);
729 Assertions.assertEquals(control_r.getPosition().getX(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getX(), 1e-10);
730 Assertions.assertEquals(control_r.getPosition().getY(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getY(), 1e-10);
731 Assertions.assertEquals(control_r.getPosition().getZ(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getZ(), 1e-10);
732 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getX(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getX(), 1e-10);
733 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getY(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getY(), 1e-10);
734 Assertions.assertEquals(control_r.getPVCoordinates().getVelocity().getZ(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getZ(), 1e-10);
735 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getX(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getX(), 1e-10);
736 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getY(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getY(), 1e-10);
737 Assertions.assertEquals(control_r.getPVCoordinates().getAcceleration().getZ(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getZ(), 1e-10);
738 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ0(), control_f.getAttitude().getOrientation().getRotation().getQ0().getReal(), 1e-10);
739 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ1(), control_f.getAttitude().getOrientation().getRotation().getQ1().getReal(), 1e-10);
740 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ2(), control_f.getAttitude().getOrientation().getRotation().getQ2().getReal(), 1e-10);
741 Assertions.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ3(), control_f.getAttitude().getOrientation().getRotation().getQ3().getReal(), 1e-10);
742
743 Assertions.assertEquals(control_r.getAttitude().getSpin().getAlpha(), control_f.getAttitude().getSpin().getAlpha().getReal(), 1e-10);
744 Assertions.assertEquals(control_r.getAttitude().getSpin().getDelta(), control_f.getAttitude().getSpin().getDelta().getReal(), 1e-10);
745 Assertions.assertEquals(control_r.getAttitude().getSpin().getNorm(), control_f.getAttitude().getSpin().getNorm().getReal(), 1e-10);
746
747 Assertions.assertEquals(control_r.getAttitude().getReferenceFrame().isPseudoInertial(), control_f.getAttitude().getReferenceFrame().isPseudoInertial());
748 Assertions.assertEquals(control_r.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH), control_f.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH).getReal(), 1e-10);
749
750
751 }
752 }
753
754
755
756
757
758
759 private <T extends CalculusFieldElement<T>> void doTestDateConsistencyCloseAbsPV(final Field<T> field) {
760
761
762
763 T zero = field.getZero();
764 T one = field.getOne();
765 T x_f = zero.add(0.8);
766 T y_f = zero.add(0.2);
767 T z_f = zero;
768 T vx_f = zero;
769 T vy_f = zero;
770 T vz_f = zero.add(0.1);
771 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
772 TimeComponents.H00,
773 TimeScalesFactory.getUTC());
774 FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
775
776 FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
777
778 FieldAbsolutePVCoordinates<T> AbsolutePVCoordinates10Shifts = absPV_f;
779 for (int ii = 0; ii < 10; ii++) {
780 AbsolutePVCoordinates10Shifts = AbsolutePVCoordinates10Shifts.shiftedBy(zero.add(0.1));
781 }
782 final FieldAbsolutePVCoordinates<T> AbsolutePVCoordinates1Shift = absPV_f.shiftedBy(one);
783
784 BodyCenterPointing attitudeLaw = new BodyCenterPointing(absPV_f.getFrame(), earth);
785
786 FieldAttitude<T> shiftedAttitude = attitudeLaw
787 .getAttitude(AbsolutePVCoordinates1Shift, AbsolutePVCoordinates1Shift.getDate(), absPV_f.getFrame());
788
789
790 Assertions.assertNotEquals(shiftedAttitude.getDate(), AbsolutePVCoordinates10Shifts.getDate());
791 Assertions.assertEquals(
792 shiftedAttitude.getDate().durationFrom(AbsolutePVCoordinates10Shifts.getDate()).getReal(),
793 0, Precision.EPSILON);
794
795
796 FieldSpacecraftState<T> s1 = new FieldSpacecraftState<>(AbsolutePVCoordinates10Shifts, shiftedAttitude);
797 FieldSpacecraftState<T> s2 = s1.shiftedBy(0.001);
798
799 try {
800
801 new FieldSpacecraftState<>(AbsolutePVCoordinates10Shifts, s2.getAttitude());
802 Assertions.fail("an exception should have been thrown");
803 } catch (OrekitIllegalArgumentException oiae) {
804 Assertions.assertEquals(OrekitMessages.ORBIT_AND_ATTITUDE_DATES_MISMATCH,
805 oiae.getSpecifier());
806 }
807 }
808
809
810
811 private <T extends CalculusFieldElement<T>> void doTestFramesConsistencyAbsPV(final Field<T> field) {
812
813 T zero = field.getZero();
814
815 T x_f = zero.add(0.8);
816 T y_f = zero.add(0.2);
817 T z_f = zero;
818 T vx_f = zero;
819 T vy_f = zero;
820 T vz_f = zero.add(0.1);
821
822
823 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
824 TimeComponents.H00,
825 TimeScalesFactory.getUTC());
826
827 FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
828
829 FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
830
831 new FieldSpacecraftState<>(absPV_f,
832 new FieldAttitude<>(absPV_f.getDate(),
833 FramesFactory.getGCRF(),
834 FieldRotation.getIdentity(field),
835 FieldVector3D.getZero(field),
836 FieldVector3D.getZero(field)));
837 }
838
839 private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesAbsPV(final Field<T> field) {
840
841 T zero = field.getZero();
842 T x_f = zero.add(0.8);
843 T y_f = zero.add(0.2);
844 T z_f = zero;
845 T vx_f = zero;
846 T vy_f = zero;
847 T vz_f = zero.add(0.1);
848
849 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
850 TimeComponents.H00,
851 TimeScalesFactory.getUTC());
852
853 FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
854
855 FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
856
857 FieldNumericalPropagator<T> prop = new FieldNumericalPropagator<>(field,
858 new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001));
859 prop.setOrbitType(null);
860
861 final FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(absPV_f);
862
863 prop.resetInitialState(initialState);
864
865 final FieldSpacecraftState<T> state = prop.propagate(absPV_f.getDate().shiftedBy(60));
866 T[] add = MathArrays.buildArray(field, 2);
867 add[0] = zero.add(1.);
868 add[1] = zero.add(2.);
869 final FieldSpacecraftState<T> extended =
870 state.
871 addAdditionalState("test-1", add).
872 addAdditionalState("test-2", zero.add(42.0));
873 Assertions.assertEquals(0, state.getAdditionalStatesValues().size());
874 Assertions.assertFalse(state.hasAdditionalState("test-1"));
875 try {
876 state.getAdditionalState("test-1");
877 Assertions.fail("an exception should have been thrown");
878 } catch (OrekitException oe) {
879 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
880 Assertions.assertEquals(oe.getParts()[0], "test-1");
881 }
882 try {
883 state.ensureCompatibleAdditionalStates(extended);
884 Assertions.fail("an exception should have been thrown");
885 } catch (OrekitException oe) {
886 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
887 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
888 }
889 try {
890 extended.ensureCompatibleAdditionalStates(state);
891 Assertions.fail("an exception should have been thrown");
892 } catch (OrekitException oe) {
893 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
894 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
895 }
896 try {
897 T[] kk = MathArrays.buildArray(field, 7);
898 extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk));
899 Assertions.fail("an exception should have been thrown");
900 } catch (MathIllegalStateException mise) {
901 Assertions.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
902 Assertions.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
903 }
904 Assertions.assertEquals(2, extended.getAdditionalStatesValues().size());
905 Assertions.assertTrue(extended.hasAdditionalState("test-1"));
906 Assertions.assertTrue(extended.hasAdditionalState("test-2"));
907 Assertions.assertEquals( 1.0, extended.getAdditionalState("test-1")[0].getReal(), 1.0e-15);
908 Assertions.assertEquals( 2.0, extended.getAdditionalState("test-1")[1].getReal(), 1.0e-15);
909 Assertions.assertEquals(42.0, extended.getAdditionalState("test-2")[0].getReal(), 1.0e-15);
910
911
912 T[] dd = MathArrays.buildArray(field, 1);
913 dd[0] = zero.add(-6.0);
914 FieldArrayDictionary<T> map = new FieldArrayDictionary<>(field);
915 map.put("test-3", dd);
916 FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getAbsPVA(), map);
917 Assertions.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
918 FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), map);
919 Assertions.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
920 FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getMass(), map);
921 Assertions.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
922 FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), map);
923 Assertions.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
924 FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
925 Assertions.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
926
927 }
928
929 private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesDerivativesAbsPV(final Field<T> field) {
930
931 T zero = field.getZero();
932 T x_f = zero.add(0.8);
933 T y_f = zero.add(0.2);
934 T z_f = zero;
935 T vx_f = zero;
936 T vy_f = zero;
937 T vz_f = zero.add(0.1);
938
939 FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
940 TimeComponents.H00,
941 TimeScalesFactory.getUTC());
942
943 FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
944
945 FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
946
947 FieldNumericalPropagator<T> prop = new FieldNumericalPropagator<>(field,
948 new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001));
949 prop.setOrbitType(null);
950
951 final FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(absPV_f);
952
953 prop.resetInitialState(initialState);
954
955 final FieldSpacecraftState<T> state = prop.propagate(absPV_f.getDate().shiftedBy(60));
956 T[] add = MathArrays.buildArray(field, 2);
957 add[0] = zero.add(1.);
958 add[1] = zero.add(2.);
959 final FieldSpacecraftState<T> extended =
960 state.
961 addAdditionalStateDerivative("test-1", add).
962 addAdditionalStateDerivative("test-2", zero.add(42.0));
963 Assertions.assertEquals(0, state.getAdditionalStatesDerivatives().size());
964 Assertions.assertFalse(state.hasAdditionalStateDerivative("test-1"));
965 try {
966 state.getAdditionalStateDerivative("test-1");
967 Assertions.fail("an exception should have been thrown");
968 } catch (OrekitException oe) {
969 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
970 Assertions.assertEquals(oe.getParts()[0], "test-1");
971 }
972 try {
973 state.ensureCompatibleAdditionalStates(extended);
974 Assertions.fail("an exception should have been thrown");
975 } catch (OrekitException oe) {
976 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
977 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
978 }
979 try {
980 extended.ensureCompatibleAdditionalStates(state);
981 Assertions.fail("an exception should have been thrown");
982 } catch (OrekitException oe) {
983 Assertions.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
984 Assertions.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
985 }
986 try {
987 T[] kk = MathArrays.buildArray(field, 7);
988 extended.ensureCompatibleAdditionalStates(extended.addAdditionalStateDerivative("test-2", kk));
989 Assertions.fail("an exception should have been thrown");
990 } catch (MathIllegalStateException mise) {
991 Assertions.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
992 Assertions.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
993 }
994 Assertions.assertEquals(2, extended.getAdditionalStatesDerivatives().size());
995 Assertions.assertTrue(extended.hasAdditionalStateDerivative("test-1"));
996 Assertions.assertTrue(extended.hasAdditionalStateDerivative("test-2"));
997 Assertions.assertEquals( 1.0, extended.getAdditionalStateDerivative("test-1")[0].getReal(), 1.0e-15);
998 Assertions.assertEquals( 2.0, extended.getAdditionalStateDerivative("test-1")[1].getReal(), 1.0e-15);
999 Assertions.assertEquals(42.0, extended.getAdditionalStateDerivative("test-2")[0].getReal(), 1.0e-15);
1000
1001
1002 T[] dd = MathArrays.buildArray(field, 1);
1003 dd[0] = zero.add(-6.0);
1004 FieldArrayDictionary<T> dictionary = new FieldArrayDictionary<T>(field);
1005 dictionary.put("test-3", dd);
1006 FieldSpacecraftState<T> s = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), null, dictionary);
1007 Assertions.assertFalse(s.hasAdditionalState("test-3"));
1008 Assertions.assertEquals(-6.0, s.getAdditionalStateDerivative("test-3")[0].getReal(), 1.0e-15);
1009
1010 }
1011
1012 private <T extends CalculusFieldElement<T>> void doTestAdditionalTestResetOnEventAnalytical(final Field<T> field) {
1013
1014 T zero = field.getZero();
1015
1016
1017 FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1018 FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1019 PositionAngleType.TRUE, FramesFactory.getGCRF(), date0,
1020 zero.add(Constants.WGS84_EARTH_MU));
1021
1022
1023 FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1024
1025
1026 final String name = "A";
1027 FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(orbit).
1028 addAdditionalState(name, zero.add(-1));
1029
1030 propagator.resetInitialState(initialState);
1031
1032
1033 FieldAbsoluteDate<T> changeDate = date0.shiftedBy(3);
1034 FieldDateDetector<T> dateDetector = new FieldDateDetector<>(field, changeDate).
1035 withHandler(new FieldEventHandler<T>() {
1036
1037 @Override
1038 public Action eventOccurred(FieldSpacecraftState<T> s, FieldEventDetector<T> detector, boolean increasing) {
1039 return Action.RESET_STATE;
1040 }
1041
1042 @Override
1043 public FieldSpacecraftState<T> resetState(FieldEventDetector<T> detector, FieldSpacecraftState<T> oldState) {
1044 return oldState.addAdditionalState(name, zero.add(+1));
1045 }
1046
1047 });
1048
1049 propagator.addEventDetector(dateDetector);
1050 propagator.setStepHandler(zero.add(0.125), s -> {
1051 if (s.getDate().durationFrom(changeDate).getReal() < -0.001) {
1052 Assertions.assertEquals(-1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1053 } else if (s.getDate().durationFrom(changeDate).getReal() > +0.001) {
1054 Assertions.assertEquals(+1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1055 }
1056 });
1057 FieldSpacecraftState<T> finalState = propagator.propagate(date0, date0.shiftedBy(5));
1058 Assertions.assertEquals(+1, finalState.getAdditionalState(name)[0].getReal(), 1.0e-15);
1059
1060 }
1061
1062 private <T extends CalculusFieldElement<T>> void doTestAdditionalTestResetOnEventNumerical(final Field<T> field) {
1063
1064 T zero = field.getZero();
1065
1066
1067 FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1068 FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1069 PositionAngleType.TRUE, FramesFactory.getGCRF(), date0,
1070 zero.add(Constants.WGS84_EARTH_MU));
1071
1072
1073 FieldODEIntegrator<T> odeIntegrator = new DormandPrince853FieldIntegrator<>(field, 1E-3, 1E3, 1E-6, 1E-6);
1074 FieldNumericalPropagator<T> propagator = new FieldNumericalPropagator<>(field, odeIntegrator);
1075
1076
1077 final String name = "A";
1078 FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(orbit).
1079 addAdditionalState(name, zero.add(-1));
1080
1081 propagator.resetInitialState(initialState);
1082
1083
1084 FieldAbsoluteDate<T> changeDate = date0.shiftedBy(3);
1085 FieldDateDetector<T> dateDetector = new FieldDateDetector<>(field, changeDate).
1086 withHandler(new FieldEventHandler<T>() {
1087
1088 @Override
1089 public Action eventOccurred(FieldSpacecraftState<T> s, FieldEventDetector<T> detector, boolean increasing) {
1090 return Action.RESET_STATE;
1091 }
1092
1093 @Override
1094 public FieldSpacecraftState<T> resetState(FieldEventDetector<T> detector, FieldSpacecraftState<T> oldState) {
1095 return oldState.addAdditionalState(name, zero.add(+1));
1096 }
1097
1098 });
1099
1100 propagator.addEventDetector(dateDetector);
1101 propagator.setStepHandler(zero.add(0.125), s -> {
1102 if (s.getDate().durationFrom(changeDate).getReal() < -0.001) {
1103 Assertions.assertEquals(-1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1104 } else if (s.getDate().durationFrom(changeDate).getReal() > +0.001) {
1105 Assertions.assertEquals(+1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1106 }
1107 });
1108 FieldSpacecraftState<T> finalState = propagator.propagate(date0, date0.shiftedBy(5));
1109 Assertions.assertEquals(+1, finalState.getAdditionalState(name)[0].getReal(), 1.0e-15);
1110
1111 }
1112
1113 private <T extends CalculusFieldElement<T>> void doTestShiftAdditionalDerivativesDouble(final Field<T> field) {
1114
1115 T zero = field.getZero();
1116
1117
1118 FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1119 FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1120 PositionAngleType.TRUE, FramesFactory.getGCRF(), date0,
1121 zero.add(Constants.WGS84_EARTH_MU));
1122
1123
1124 FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1125
1126 final String valueAndDerivative = "value-and-derivative";
1127 final String valueOnly = "value-only";
1128 final String derivativeOnly = "derivative-only";
1129 final FieldSpacecraftState<T> s0 = propagator.getInitialState().
1130 addAdditionalState(valueAndDerivative, convert(field, new double[] { 1.0, 2.0 })).
1131 addAdditionalStateDerivative(valueAndDerivative, convert(field, new double[] { 3.0, 2.0 })).
1132 addAdditionalState(valueOnly, convert(field, new double[] { 5.0, 4.0 })).
1133 addAdditionalStateDerivative(derivativeOnly, convert(field, new double[] { 1.0, -1.0 }));
1134 Assertions.assertEquals( 1.0, s0.getAdditionalState(valueAndDerivative)[0].getReal(), 1.0e-15);
1135 Assertions.assertEquals( 2.0, s0.getAdditionalState(valueAndDerivative)[1].getReal(), 1.0e-15);
1136 Assertions.assertEquals( 3.0, s0.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1137 Assertions.assertEquals( 2.0, s0.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1138 Assertions.assertEquals( 5.0, s0.getAdditionalState(valueOnly)[0].getReal(), 1.0e-15);
1139 Assertions.assertEquals( 4.0, s0.getAdditionalState(valueOnly)[1].getReal(), 1.0e-15);
1140 Assertions.assertEquals( 1.0, s0.getAdditionalStateDerivative(derivativeOnly)[0].getReal(), 1.0e-15);
1141 Assertions.assertEquals(-1.0, s0.getAdditionalStateDerivative(derivativeOnly)[1].getReal(), 1.0e-15);
1142 final FieldSpacecraftState<T> s1 = s0.shiftedBy(-2.0);
1143 Assertions.assertEquals(-5.0, s1.getAdditionalState(valueAndDerivative)[0].getReal(), 1.0e-15);
1144 Assertions.assertEquals(-2.0, s1.getAdditionalState(valueAndDerivative)[1].getReal(), 1.0e-15);
1145 Assertions.assertEquals( 3.0, s1.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1146 Assertions.assertEquals( 2.0, s1.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1147 Assertions.assertEquals( 5.0, s1.getAdditionalState(valueOnly)[0].getReal(), 1.0e-15);
1148 Assertions.assertEquals( 4.0, s1.getAdditionalState(valueOnly)[1].getReal(), 1.0e-15);
1149 Assertions.assertEquals( 1.0, s1.getAdditionalStateDerivative(derivativeOnly)[0].getReal(), 1.0e-15);
1150 Assertions.assertEquals(-1.0, s1.getAdditionalStateDerivative(derivativeOnly)[1].getReal(), 1.0e-15);
1151
1152 }
1153
1154 private <T extends CalculusFieldElement<T>> void doTestShiftAdditionalDerivativesField(final Field<T> field) {
1155
1156 T zero = field.getZero();
1157
1158
1159 FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1160 FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1161 PositionAngleType.TRUE, FramesFactory.getGCRF(), date0,
1162 zero.add(Constants.WGS84_EARTH_MU));
1163
1164
1165 FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1166
1167 final String valueAndDerivative = "value-and-derivative";
1168 final String valueOnly = "value-only";
1169 final String derivativeOnly = "derivative-only";
1170 final FieldSpacecraftState<T> s0 = propagator.getInitialState().
1171 addAdditionalState(valueAndDerivative, convert(field, new double[] { 1.0, 2.0 })).
1172 addAdditionalStateDerivative(valueAndDerivative, convert(field, new double[] { 3.0, 2.0 })).
1173 addAdditionalState(valueOnly, convert(field, new double[] { 5.0, 4.0 })).
1174 addAdditionalStateDerivative(derivativeOnly, convert(field, new double[] { 1.0, -1.0 }));
1175 Assertions.assertEquals( 1.0, s0.getAdditionalState(valueAndDerivative)[0].getReal(), 1.0e-15);
1176 Assertions.assertEquals( 2.0, s0.getAdditionalState(valueAndDerivative)[1].getReal(), 1.0e-15);
1177 Assertions.assertEquals( 3.0, s0.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1178 Assertions.assertEquals( 2.0, s0.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1179 Assertions.assertEquals( 5.0, s0.getAdditionalState(valueOnly)[0].getReal(), 1.0e-15);
1180 Assertions.assertEquals( 4.0, s0.getAdditionalState(valueOnly)[1].getReal(), 1.0e-15);
1181 Assertions.assertEquals( 1.0, s0.getAdditionalStateDerivative(derivativeOnly)[0].getReal(), 1.0e-15);
1182 Assertions.assertEquals(-1.0, s0.getAdditionalStateDerivative(derivativeOnly)[1].getReal(), 1.0e-15);
1183 final FieldSpacecraftState<T> s1 = s0.shiftedBy(field.getZero().newInstance(-2.0));
1184 Assertions.assertEquals(-5.0, s1.getAdditionalState(valueAndDerivative)[0].getReal(), 1.0e-15);
1185 Assertions.assertEquals(-2.0, s1.getAdditionalState(valueAndDerivative)[1].getReal(), 1.0e-15);
1186 Assertions.assertEquals( 3.0, s1.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1187 Assertions.assertEquals( 2.0, s1.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1188 Assertions.assertEquals( 5.0, s1.getAdditionalState(valueOnly)[0].getReal(), 1.0e-15);
1189 Assertions.assertEquals( 4.0, s1.getAdditionalState(valueOnly)[1].getReal(), 1.0e-15);
1190 Assertions.assertEquals( 1.0, s1.getAdditionalStateDerivative(derivativeOnly)[0].getReal(), 1.0e-15);
1191 Assertions.assertEquals(-1.0, s1.getAdditionalStateDerivative(derivativeOnly)[1].getReal(), 1.0e-15);
1192
1193 }
1194
1195 @BeforeEach
1196 public void setUp(){
1197 try {
1198
1199 Utils.setDataRoot("regular-data");
1200 mu = 3.9860047e14;
1201
1202 double a = 7187990.1979844316;
1203 double e = 0.5e-4;
1204 double i = 1.7105407051081795;
1205 double omega = 1.9674147913622104;
1206 double OMEGA = FastMath.toRadians(261);
1207 double lv = 0;
1208
1209 rDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
1210 TimeComponents.H00,
1211 TimeScalesFactory.getUTC());
1212 rOrbit = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngleType.TRUE,
1213 FramesFactory.getEME2000(), rDate, mu);
1214 earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1215 Constants.WGS84_EARTH_FLATTENING,
1216 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
1217 } catch (OrekitException oe) {
1218
1219 Assertions.fail(oe.getLocalizedMessage());
1220 }
1221 }
1222
1223 private <T extends CalculusFieldElement<T>> T[] convert(final Field<T> field, final double[] a) {
1224 final T[] converted = MathArrays.buildArray(field, a.length);
1225 for (int i = 0; i < a.length; ++i) {
1226 converted[i] = field.getZero().newInstance(a[i]);
1227 }
1228 return converted;
1229 }
1230
1231 private AbsoluteDate rDate;
1232 private double mu;
1233 private Orbit rOrbit;
1234 private OneAxisEllipsoid earth;
1235
1236 }
1237