1   /* Copyright 2002-2022 CS GROUP
2    * Licensed to CS GROUP (CS) under one or more
3    * contributor license agreements.  See the NOTICE file distributed with
4    * this work for additional information regarding copyright ownership.
5    * CS licenses this file to You under the Apache License, Version 2.0
6    * (the "License"); you may not use this file except in compliance with
7    * the License.  You may obtain a copy of the License at
8    *
9    *   http://www.apache.org/licenses/LICENSE-2.0
10   *
11   * Unless required by applicable law or agreed to in writing, software
12   * distributed under the License is distributed on an "AS IS" BASIS,
13   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14   * See the License for the specific language governing permissions and
15   * limitations under the License.
16   */
17  package org.orekit.propagation;
18  
19  
20  import java.text.ParseException;
21  import java.util.ArrayList;
22  import java.util.HashMap;
23  import java.util.List;
24  import java.util.Map;
25  
26  import org.hipparchus.Field;
27  import org.hipparchus.CalculusFieldElement;
28  import org.hipparchus.analysis.polynomials.PolynomialFunction;
29  import org.hipparchus.exception.LocalizedCoreFormats;
30  import org.hipparchus.exception.MathIllegalStateException;
31  import org.hipparchus.geometry.euclidean.threed.FieldRotation;
32  import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
33  import org.hipparchus.geometry.euclidean.threed.Rotation;
34  import org.hipparchus.geometry.euclidean.threed.Vector3D;
35  import org.hipparchus.ode.FieldODEIntegrator;
36  import org.hipparchus.ode.events.Action;
37  import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
38  import org.hipparchus.util.Decimal64Field;
39  import org.hipparchus.util.FastMath;
40  import org.hipparchus.util.MathArrays;
41  import org.hipparchus.util.Precision;
42  import org.junit.Assert;
43  import org.junit.Before;
44  import org.junit.Test;
45  import org.orekit.Utils;
46  import org.orekit.attitudes.BodyCenterPointing;
47  import org.orekit.attitudes.FieldAttitude;
48  import org.orekit.bodies.OneAxisEllipsoid;
49  import org.orekit.errors.OrekitException;
50  import org.orekit.errors.OrekitIllegalArgumentException;
51  import org.orekit.errors.OrekitMessages;
52  import org.orekit.frames.Frame;
53  import org.orekit.frames.FramesFactory;
54  import org.orekit.frames.Transform;
55  import org.orekit.orbits.FieldKeplerianOrbit;
56  import org.orekit.orbits.FieldOrbit;
57  import org.orekit.orbits.KeplerianOrbit;
58  import org.orekit.orbits.Orbit;
59  import org.orekit.orbits.PositionAngle;
60  import org.orekit.propagation.analytical.FieldEcksteinHechlerPropagator;
61  import org.orekit.propagation.analytical.FieldKeplerianPropagator;
62  import org.orekit.propagation.events.FieldDateDetector;
63  import org.orekit.propagation.events.handlers.FieldEventHandler;
64  import org.orekit.propagation.numerical.FieldNumericalPropagator;
65  import org.orekit.time.AbsoluteDate;
66  import org.orekit.time.DateComponents;
67  import org.orekit.time.FieldAbsoluteDate;
68  import org.orekit.time.TimeComponents;
69  import org.orekit.time.TimeScalesFactory;
70  import org.orekit.utils.Constants;
71  import org.orekit.utils.FieldAbsolutePVCoordinates;
72  import org.orekit.utils.FieldArrayDictionary;
73  import org.orekit.utils.FieldPVCoordinates;
74  import org.orekit.utils.IERSConventions;
75  import org.orekit.utils.PVCoordinates;
76  import org.orekit.utils.TimeStampedFieldPVCoordinates;
77  
78  
79  public class FieldSpacecraftStateTest {
80  
81      @Test
82      public void testFieldVSReal() {
83          doTestFieldVsReal(Decimal64Field.getInstance());
84      }
85  
86      @Test
87      public void testShiftVsEcksteinHechlerError() {
88          doTestShiftVsEcksteinHechlerError(Decimal64Field.getInstance());
89      }
90  
91      @Test(expected=IllegalArgumentException.class)
92      public void testDatesConsistency() {
93          doTestDatesConsistency(Decimal64Field.getInstance());
94      }
95  
96      @Test
97      public void testDateConsistencyClose() {
98          doTestDateConsistencyClose(Decimal64Field.getInstance());
99      }
100 
101     @Test(expected=IllegalArgumentException.class)
102     public void testFramesConsistency() {
103         doTestFramesConsistency(Decimal64Field.getInstance());
104     }
105 
106     @Test
107     public void testTransform() {
108         doTestTransform(Decimal64Field.getInstance());
109     }
110 
111     @Test
112     public void testAdditionalStates() {
113         doTestAdditionalStates(Decimal64Field.getInstance());
114     }
115 
116     @Deprecated
117     @Test
118     public void testDeprecatedOrbit() {
119         doTestDeprecatedOrbit(Decimal64Field.getInstance());
120     }
121 
122         @Test
123     public void testAdditionalStatesDerivatives() {
124         doTestAdditionalStatesDerivatives(Decimal64Field.getInstance());
125     }
126 
127     @Test
128     public void testInterpolation() throws ParseException {
129         doTestInterpolation(Decimal64Field.getInstance());
130     }
131 
132     @Test
133     public void testFieldVSRealAbsPV() {
134         doTestFieldVsRealAbsPV(Decimal64Field.getInstance());
135     }
136 
137     @Test
138     public void testDateConsistencyCloseAbsPV() {
139         doTestDateConsistencyCloseAbsPV(Decimal64Field.getInstance());
140     }
141 
142     @Test(expected=IllegalArgumentException.class)
143     public void testFramesConsistencyAbsPV() {
144         doTestFramesConsistencyAbsPV(Decimal64Field.getInstance());
145     }
146 
147     @Test
148     public void testAdditionalStatesAbsPV() {
149         doTestAdditionalStatesAbsPV(Decimal64Field.getInstance());
150     }
151 
152     @Deprecated
153     @Test
154     public void testDeprecatedAbsPV() {
155         doTestDeprecatedAbsPV(Decimal64Field.getInstance());
156     }
157 
158     @Test
159     public void testAdditionalStatesDerivativesAbsPV() {
160         doTestAdditionalStatesDerivativesAbsPV(Decimal64Field.getInstance());
161     }
162 
163     @Test
164     public void testResetOnEventAnalytical() {
165         doTestAdditionalTestResetOnEventAnalytical(Decimal64Field.getInstance());
166     }
167 
168     @Test
169     public void testResetOnEventNumerical() {
170         doTestAdditionalTestResetOnEventNumerical(Decimal64Field.getInstance());
171     }
172     
173     @Test
174     public void testIssue775() {
175         doTestIssue775(Decimal64Field.getInstance());
176     }
177 
178     @Test
179     public void testShiftAdditionalDerivativesDouble() {
180         doTestShiftAdditionalDerivativesDouble(Decimal64Field.getInstance());
181     }
182 
183     @Test
184     public void testShiftAdditionalDerivativesField() {
185         doTestShiftAdditionalDerivativesField(Decimal64Field.getInstance());
186     }
187 
188     private <T extends CalculusFieldElement<T>> void doTestFieldVsReal(final Field<T> field) {
189         T zero = field.getZero();
190 
191         double mu = 3.9860047e14;
192 
193         T a_f     = zero.add(150000);
194         T e_f     = zero.add(     0);
195         T i_f     = zero.add(     0);
196         T pa_f    = zero.add(     0);
197         T raan_f  = zero.add(     0);
198         T m_f     = zero.add(     0);
199 
200         FieldAbsoluteDate<T> t_f = new FieldAbsoluteDate<>(field);
201 
202         double a_r = a_f.getReal();
203         double e_r = e_f.getReal();
204         double i_r = i_f.getReal();
205         double pa_r = pa_f.getReal();
206         double raan_r = raan_f.getReal();
207         double m_r = m_f.getReal();
208 
209         AbsoluteDate t_r = t_f.toAbsoluteDate();
210 
211 
212         KeplerianOrbit      kep_r = new KeplerianOrbit(a_r, e_r, i_r, pa_r, raan_r, m_r, PositionAngle.ECCENTRIC, FramesFactory.getEME2000(), t_r, mu);
213         FieldKeplerianOrbit<T> kep_f = new FieldKeplerianOrbit<>(a_f, e_f, i_f, pa_f, raan_f, m_f, PositionAngle.ECCENTRIC, FramesFactory.getEME2000(), t_f, zero.add(mu));
214 
215         SpacecraftState ScS_r = new SpacecraftState(kep_r);
216         FieldSpacecraftState<T> ScS_f = new FieldSpacecraftState<>(kep_f);
217 
218         for (double dt = 0; dt < 500; dt+=100){
219             SpacecraftState control_r = ScS_r.shiftedBy(dt);
220             FieldSpacecraftState<T> control_f = ScS_f.shiftedBy(zero.add(dt));
221 
222 
223             Assert.assertEquals(control_r.getA(), control_f.getA().getReal(), 1e-10);
224             Assert.assertEquals(control_r.getE(), control_f.getE().getReal(), 1e-10);
225             Assert.assertEquals(control_r.getEquinoctialEx(), control_f.getEquinoctialEx().getReal(), 1e-10);
226             Assert.assertEquals(control_r.getEquinoctialEy(), control_f.getEquinoctialEy().getReal(), 1e-10);
227             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getX(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getX(), 1e-10);
228             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getY(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getY(), 1e-10);
229             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getZ(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getZ(), 1e-10);
230             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getX(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getX(), 1e-10);
231             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getY(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getY(), 1e-10);
232             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getZ(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getZ(), 1e-10);
233             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getX(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getX(), 1e-10);
234             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getY(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getY(), 1e-10);
235             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getZ(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getZ(), 1e-10);
236             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ0(), control_f.getAttitude().getOrientation().getRotation().getQ0().getReal(), 1e-10);
237             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ1(), control_f.getAttitude().getOrientation().getRotation().getQ1().getReal(), 1e-10);
238             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ2(), control_f.getAttitude().getOrientation().getRotation().getQ2().getReal(), 1e-10);
239             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ3(), control_f.getAttitude().getOrientation().getRotation().getQ3().getReal(), 1e-10);
240 
241             Assert.assertEquals(control_r.getAttitude().getSpin().getAlpha(), control_f.getAttitude().getSpin().getAlpha().getReal(), 1e-10);
242             Assert.assertEquals(control_r.getAttitude().getSpin().getDelta(), control_f.getAttitude().getSpin().getDelta().getReal(), 1e-10);
243             Assert.assertEquals(control_r.getAttitude().getSpin().getNorm(), control_f.getAttitude().getSpin().getNorm().getReal(), 1e-10);
244 
245             Assert.assertEquals(control_r.getAttitude().getReferenceFrame().isPseudoInertial(), control_f.getAttitude().getReferenceFrame().isPseudoInertial());
246             Assert.assertEquals(control_r.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH), control_f.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH).getReal(), 1e-10);
247 
248 
249         }
250 
251     }
252 
253     private <T extends CalculusFieldElement<T>>  void doTestShiftVsEcksteinHechlerError(final Field<T> field)
254         {
255 
256         T zero = field.getZero();
257         T mass = zero.add(2500.);
258         T a = zero.add(rOrbit.getA());
259         T e = zero.add(rOrbit.getE());
260         T i = zero.add(rOrbit.getI());
261         T pa = zero.add(1.9674147913622104);
262         T raan = zero.add(FastMath.toRadians(261));
263         T lv = zero.add(0);
264         final double ae  = 6.378137e6;
265         final double c20 = -1.08263e-3;
266         final double c30 =  2.54e-6;
267         final double c40 =  1.62e-6;
268         final double c50 =  2.3e-7;
269         final double c60 =  -5.5e-7;
270 
271 
272         // polynomial models for interpolation error in position, velocity, acceleration and attitude
273         // these models grow as follows
274         //   interpolation time (s)    position error (m)   velocity error (m/s)   acceleration error (m/s²)  attitude error (°)
275         //           60                        2                    0.07                  0.002               0.00002
276         //          120                       12                    0.3                   0.005               0.00009
277         //          300                      170                    1.6                   0.012               0.0009
278         //          600                     1200                    5.7                   0.024               0.006
279         //          900                     3600                   10.6                   0.034               0.02
280         // the expected maximum residuals with respect to these models are about 0.4m, 0.5mm/s, 8μm/s² and 3e-6°
281         PolynomialFunction pModel = new PolynomialFunction(new double[] {
282             1.5664070631933846e-01,  7.5504722733047560e-03, -8.2460562451009510e-05,
283             6.9546332080305580e-06, -1.7045365367533077e-09, -4.2187860791066264e-13
284         });
285         PolynomialFunction vModel = new PolynomialFunction(new double[] {
286            -3.5472364019908720e-04,  1.6568103861124980e-05,  1.9637913327830596e-05,
287            -3.4248792843039766e-09, -5.6565135131014254e-12,  1.4730170946808630e-15
288         });
289         PolynomialFunction aModel = new PolynomialFunction(new double[] {
290             3.0731707577766896e-06,  3.9770746399850350e-05,  1.9779039254538660e-09,
291             8.0263328220724900e-12, -1.5600835252366078e-14,  1.1785257001549687e-18
292         });
293         PolynomialFunction rModel = new PolynomialFunction(new double[] {
294            -2.7689062063188115e-06,  1.7406542538258334e-07,  2.5109795349592287e-09,
295             2.0399322661074575e-11,  9.9126348912426750e-15, -3.5015638905729510e-18
296         });
297 
298         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
299                                                             TimeComponents.H00,
300                                                             TimeScalesFactory.getUTC());
301 
302         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
303                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
304 
305         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
306 
307         FieldPropagator<T> propagator =
308             new FieldEcksteinHechlerPropagator<>(orbit, attitudeLaw, mass,
309                                                  ae, zero.add(mu), c20, c30, c40, c50, c60);
310 
311         FieldAbsoluteDate<T> centerDate = orbit.getDate().shiftedBy(100.0);
312 
313         FieldSpacecraftState<T> centerState = propagator.propagate(centerDate);
314 
315         double maxResidualP = 0;
316         double maxResidualV = 0;
317         double maxResidualA = 0;
318         double maxResidualR = 0;
319         for (T dt = field.getZero(); dt.getReal() < 900.0; dt = dt.add(5)) {
320 
321             FieldSpacecraftState<T> shifted = centerState.shiftedBy(dt);
322             FieldSpacecraftState<T> propagated = propagator.propagate(centerDate.shiftedBy(dt));
323             FieldPVCoordinates<T> dpv = new FieldPVCoordinates<>(propagated.getPVCoordinates(), shifted.getPVCoordinates());
324 
325 
326             double residualP = pModel.value(dt.getReal()) - dpv.getPosition().getNorm().getReal();
327             double residualV = vModel.value(dt.getReal()) - dpv.getVelocity().getNorm().getReal();
328             double residualA = aModel.value(dt.getReal()) - dpv.getAcceleration().getNorm().getReal();
329             double residualR = rModel.value(dt.getReal()) -
330                                FastMath.toDegrees(FieldRotation.distance(shifted.getAttitude().getRotation(),
331                                                                          propagated.getAttitude().getRotation()).getReal());
332             maxResidualP = FastMath.max(maxResidualP, FastMath.abs(residualP));
333             maxResidualV = FastMath.max(maxResidualV, FastMath.abs(residualV));
334             maxResidualA = FastMath.max(maxResidualA, FastMath.abs(residualA));
335             maxResidualR = FastMath.max(maxResidualR, FastMath.abs(residualR));
336 
337         }
338 
339         Assert.assertEquals(0.40,   maxResidualP, 0.01);
340         Assert.assertEquals(4.9e-4, maxResidualV, 1.0e-5);
341         Assert.assertEquals(2.8e-6, maxResidualR, 1.0e-1);
342 
343     }
344 
345     private <T extends CalculusFieldElement<T>> void doTestDatesConsistency(final Field<T> field) {
346 
347         T zero = field.getZero();
348         T a = zero.add(rOrbit.getA());
349         T e = zero.add(rOrbit.getE());
350         T i = zero.add(rOrbit.getI());
351         T pa = zero.add(1.9674147913622104);
352         T raan = zero.add(FastMath.toRadians(261));
353         T lv = zero.add(0);
354 
355 
356         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
357                                                             TimeComponents.H00,
358                                                             TimeScalesFactory.getUTC());
359 
360         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
361                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
362         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
363 
364         new FieldSpacecraftState<>(orbit, attitudeLaw.getAttitude(orbit.shiftedBy(zero.add(10.0)),
365                                                                   orbit.getDate().shiftedBy(10.0),
366                                                                   orbit.getFrame()));
367     }
368 
369     /**
370      * Check orbit and attitude dates can be off by a few ulps. I see this when using
371      * FixedRate attitude provider.
372      */
373     private <T extends CalculusFieldElement<T>> void doTestDateConsistencyClose(final Field<T> field) {
374 
375 
376         //setup
377         T zero = field.getZero();
378         T one  = field.getOne();
379         T a = zero.add(rOrbit.getA());
380         T e = zero.add(rOrbit.getE());
381         T i = zero.add(rOrbit.getI());
382         T pa = zero.add(1.9674147913622104);
383         T raan = zero.add(FastMath.toRadians(261));
384         T lv = zero.add(0);
385 
386         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
387                                                             TimeComponents.H00,
388                                                             TimeScalesFactory.getUTC());
389         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
390                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
391 
392         FieldKeplerianOrbit<T> orbit10Shifts = orbit;
393         for (int ii = 0; ii < 10; ii++) {
394             orbit10Shifts = orbit10Shifts.shiftedBy(zero.add(0.1));
395         }
396         final FieldOrbit<T> orbit1Shift = orbit.shiftedBy(one);
397 
398         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
399 
400         FieldAttitude<T> shiftedAttitude = attitudeLaw
401                 .getAttitude(orbit1Shift, orbit1Shift.getDate(), orbit.getFrame());
402 
403         //verify dates are very close, but not equal
404         Assert.assertNotEquals(shiftedAttitude.getDate(), orbit10Shifts.getDate());
405         Assert.assertEquals(
406                 shiftedAttitude.getDate().durationFrom(orbit10Shifts.getDate()).getReal(),
407                 0, Precision.EPSILON);
408 
409         //action + verify no exception is thrown
410         new FieldSpacecraftState<>(orbit10Shifts, shiftedAttitude);
411     }
412 
413     // (expected=IllegalArgumentException.class)
414     private <T extends CalculusFieldElement<T>> void doTestFramesConsistency(final Field<T> field) {
415 
416         T zero = field.getZero();
417         T a = zero.add(rOrbit.getA());
418         T e = zero.add(rOrbit.getE());
419         T i = zero.add(rOrbit.getI());
420         T pa = zero.add(1.9674147913622104);
421         T raan = zero.add(FastMath.toRadians(261));
422         T lv = zero.add(0);
423 
424         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
425                                                             TimeComponents.H00,
426                                                             TimeScalesFactory.getUTC());
427         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
428                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
429 
430         new FieldSpacecraftState<>(orbit,
431                             new FieldAttitude<>(orbit.getDate(),
432                                                 FramesFactory.getGCRF(),
433                                                 Rotation.IDENTITY,
434                                                 Vector3D.ZERO,
435                                                 Vector3D.ZERO,
436                                                 field));
437     }
438 
439     private <T extends CalculusFieldElement<T>> void doTestTransform(final Field<T> field)
440         {
441 
442         T zero = field.getZero();
443         T a = zero.add(rOrbit.getA());
444         T e = zero.add(rOrbit.getE());
445         T i = zero.add(rOrbit.getI());
446         T pa = zero.add(1.9674147913622104);
447         T raan = zero.add(FastMath.toRadians(261));
448         T lv = zero.add(0);
449         T mass = zero.add(2500);
450 
451         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
452                         TimeComponents.H00,
453                         TimeScalesFactory.getUTC());
454 
455         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
456                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
457 
458         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
459 
460         FieldKeplerianPropagator<T> propagator =
461                         new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
462 
463         double maxDP = 0;
464         double maxDV = 0;
465         double maxDA = 0;
466         for (double t = 0; t < orbit.getKeplerianPeriod().getReal(); t += 60) {
467             final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(zero.add(t)));
468             final Transform transform = state.toSpacecraftState().toTransform().getInverse();
469             PVCoordinates pv = transform.transformPVCoordinates(PVCoordinates.ZERO);
470             PVCoordinates dPV = new PVCoordinates(pv, state.getPVCoordinates().toPVCoordinates());
471             Vector3D mZDirection = transform.transformVector(Vector3D.MINUS_K);
472             double alpha = Vector3D.angle(mZDirection, state.getPVCoordinates().toPVCoordinates().getPosition());
473             maxDP = FastMath.max(maxDP, dPV.getPosition().getNorm());
474             maxDV = FastMath.max(maxDV, dPV.getVelocity().getNorm());
475             maxDA = FastMath.max(maxDA, FastMath.toDegrees(alpha));
476         }
477         Assert.assertEquals(0.0, maxDP, 1.0e-6);
478         Assert.assertEquals(0.0, maxDV, 1.0e-9);
479         Assert.assertEquals(0.0, maxDA, 1.0e-12);
480 
481     }
482 
483     private <T extends CalculusFieldElement<T>> void doTestAdditionalStates(final Field<T> field) {
484 
485         T zero = field.getZero();
486         T a = zero.add(rOrbit.getA());
487         T e = zero.add(rOrbit.getE());
488         T i = zero.add(rOrbit.getI());
489         T pa = zero.add(1.9674147913622104);
490         T raan = zero.add(FastMath.toRadians(261));
491         T lv = zero.add(0);
492         T mass = zero.add(2500);
493 
494         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
495                                                             TimeComponents.H00,
496                                                             TimeScalesFactory.getUTC());
497 
498         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
499                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
500 
501         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
502 
503         FieldKeplerianPropagator<T> propagator =
504                         new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
505 
506 
507 
508 
509         final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(60));
510         T[] add = MathArrays.buildArray(field, 2);
511         add[0] = zero.add(1.);
512         add[1] = zero.add(2.);
513         final FieldSpacecraftState<T> extended =
514                 state.
515                  addAdditionalState("test-1", add).
516                   addAdditionalState("test-2", zero.add(42.0));
517         Assert.assertEquals(0, state.getAdditionalStatesValues().size());
518         Assert.assertFalse(state.hasAdditionalState("test-1"));
519         try {
520             state.getAdditionalState("test-1");
521             Assert.fail("an exception should have been thrown");
522         } catch (OrekitException oe) {
523             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
524             Assert.assertEquals(oe.getParts()[0], "test-1");
525         }
526         try {
527             state.ensureCompatibleAdditionalStates(extended);
528             Assert.fail("an exception should have been thrown");
529         } catch (OrekitException oe) {
530             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
531             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
532         }
533         try {
534             extended.ensureCompatibleAdditionalStates(state);
535             Assert.fail("an exception should have been thrown");
536         } catch (OrekitException oe) {
537             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
538             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
539         }
540         try {
541             T[] kk = MathArrays.buildArray(field, 7);
542             extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk));
543             Assert.fail("an exception should have been thrown");
544         } catch (MathIllegalStateException mise) {
545             Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
546             Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
547         }
548         Assert.assertEquals(2, extended.getAdditionalStatesValues().size());
549         Assert.assertTrue(extended.hasAdditionalState("test-1"));
550         Assert.assertTrue(extended.hasAdditionalState("test-2"));
551         Assert.assertEquals( 1.0, extended.getAdditionalState("test-1")[0].getReal(), 1.0e-15);
552         Assert.assertEquals( 2.0, extended.getAdditionalState("test-1")[1].getReal(), 1.0e-15);
553         Assert.assertEquals(42.0, extended.getAdditionalState("test-2")[0].getReal(), 1.0e-15);
554 
555         // test various constructors
556         T[] dd = MathArrays.buildArray(field, 1);
557         dd[0] = zero.add(-6.0);
558         FieldArrayDictionary<T> dictionary = new FieldArrayDictionary<>(field);
559         dictionary.put("test-3", dd);
560         FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getOrbit(), dictionary);
561         Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
562         FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), dictionary);
563         Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
564         FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getOrbit(), state.getMass(), dictionary);
565         Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
566         FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), state.getMass(), dictionary);
567         Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
568         FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
569         Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
570 
571     }
572 
573     @Deprecated
574     private <T extends CalculusFieldElement<T>> void doTestDeprecatedOrbit(final Field<T> field) {
575         T zero = field.getZero();
576         T a = zero.add(rOrbit.getA());
577         T e = zero.add(rOrbit.getE());
578         T i = zero.add(rOrbit.getI());
579         T pa = zero.add(1.9674147913622104);
580         T raan = zero.add(FastMath.toRadians(261));
581         T lv = zero.add(0);
582         T mass = zero.add(2500);
583 
584         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
585                                                             TimeComponents.H00,
586                                                             TimeScalesFactory.getUTC());
587 
588         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
589                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
590 
591         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
592 
593         FieldKeplerianPropagator<T> propagator =
594                         new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
595 
596 
597 
598 
599         final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(60));
600         // test various constructors
601         T[] dd = MathArrays.buildArray(field, 1);
602         dd[0] = zero.add(-6.0);
603         Map<String, T[]> map = new HashMap<String, T[]>();
604         map.put("test-3", dd);
605         FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getOrbit(), map);
606         Assert.assertEquals(1, sO.getAdditionalStates().size());
607         Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
608         FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), map);
609         Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
610         FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getOrbit(), state.getMass(), map);
611         Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
612         FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), state.getMass(), map);
613         Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
614         FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
615         Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
616 
617     }
618 
619     private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesDerivatives(final Field<T> field) {
620 
621         T zero = field.getZero();
622         T a = zero.add(rOrbit.getA());
623         T e = zero.add(rOrbit.getE());
624         T i = zero.add(rOrbit.getI());
625         T pa = zero.add(1.9674147913622104);
626         T raan = zero.add(FastMath.toRadians(261));
627         T lv = zero.add(0);
628         T mass = zero.add(2500);
629 
630         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
631                                                             TimeComponents.H00,
632                                                             TimeScalesFactory.getUTC());
633 
634         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, pa, raan, lv, PositionAngle.TRUE,
635                                                                  FramesFactory.getEME2000(), date, zero.add(mu));
636 
637         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
638 
639         FieldKeplerianPropagator<T> propagator =
640                         new FieldKeplerianPropagator<>(orbit, attitudeLaw, zero.add(mu), mass);
641 
642 
643 
644 
645         final FieldSpacecraftState<T> state = propagator.propagate(orbit.getDate().shiftedBy(60));
646         T[] add = MathArrays.buildArray(field, 2);
647         add[0] = zero.add(1.);
648         add[1] = zero.add(2.);
649         final FieldSpacecraftState<T> extended =
650                 state.
651                  addAdditionalStateDerivative("test-1", add).
652                   addAdditionalStateDerivative("test-2", zero.add(42.0));
653         Assert.assertEquals(0, state.getAdditionalStatesDerivatives().size());
654         Assert.assertFalse(state.hasAdditionalStateDerivative("test-1"));
655         try {
656             state.getAdditionalStateDerivative("test-1");
657             Assert.fail("an exception should have been thrown");
658         } catch (OrekitException oe) {
659             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
660             Assert.assertEquals(oe.getParts()[0], "test-1");
661         }
662         try {
663             state.ensureCompatibleAdditionalStates(extended);
664             Assert.fail("an exception should have been thrown");
665         } catch (OrekitException oe) {
666             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
667             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
668         }
669         try {
670             extended.ensureCompatibleAdditionalStates(state);
671             Assert.fail("an exception should have been thrown");
672         } catch (OrekitException oe) {
673             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
674             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
675         }
676         try {
677             T[] kk = MathArrays.buildArray(field, 7);
678             extended.ensureCompatibleAdditionalStates(extended.addAdditionalStateDerivative("test-2", kk));
679             Assert.fail("an exception should have been thrown");
680         } catch (MathIllegalStateException mise) {
681             Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
682             Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
683         }
684         Assert.assertEquals(2, extended.getAdditionalStatesDerivatives().size());
685         Assert.assertTrue(extended.hasAdditionalStateDerivative("test-1"));
686         Assert.assertTrue(extended.hasAdditionalStateDerivative("test-2"));
687         Assert.assertEquals( 1.0, extended.getAdditionalStateDerivative("test-1")[0].getReal(), 1.0e-15);
688         Assert.assertEquals( 2.0, extended.getAdditionalStateDerivative("test-1")[1].getReal(), 1.0e-15);
689         Assert.assertEquals(42.0, extended.getAdditionalStateDerivative("test-2")[0].getReal(), 1.0e-15);
690 
691         // test most complete constructor
692         T[] dd = MathArrays.buildArray(field, 1);
693         dd[0] = zero.add(-6.0);
694         FieldArrayDictionary<T> dict = new FieldArrayDictionary<>(field);
695         dict.put("test-3", dd);
696         FieldSpacecraftState<T> s = new FieldSpacecraftState<>(state.getOrbit(), state.getAttitude(), state.getMass(), null, dict);
697         Assert.assertFalse(s.hasAdditionalState("test-3"));
698         Assert.assertEquals(-6.0, s.getAdditionalStateDerivative("test-3")[0].getReal(), 1.0e-15);
699 
700         // test conversion
701         FieldSpacecraftState<T> rebuilt = new FieldSpacecraftState<>(field, s.toSpacecraftState());
702         rebuilt.ensureCompatibleAdditionalStates(s);
703 
704     }
705 
706     private <T extends CalculusFieldElement<T>> void doTestInterpolation(Field<T> field)
707         throws ParseException, OrekitException {
708         checkInterpolationError( 2,  106.46533, 0.40709287, 169847806.33e-9, 0.0, 450 * 450, 450 * 450, field);
709         checkInterpolationError( 3,    0.00353, 0.00003250,    189886.01e-9, 0.0, 0.0, 0.0, field);
710         checkInterpolationError( 4,    0.00002, 0.00000023,       232.25e-9, 0.0, 0.0, 0.0, field);
711     }
712 
713     private <T extends CalculusFieldElement<T>> void checkInterpolationError(int n, double expectedErrorP, double expectedErrorV,
714                                          double expectedErrorA, double expectedErrorM,
715                                          double expectedErrorQ, double expectedErrorD,
716                                          Field<T> field) {
717 
718         T zero = field.getZero();
719         T mu  = zero.add(3.9860047e14);
720         double ae  = 6.378137e6;
721         double c20 = -1.08263e-3;
722         double c30 = 2.54e-6;
723         double c40 = 1.62e-6;
724         double c50 = 2.3e-7;
725         double c60 = -5.5e-7;
726 
727 
728         T mass = zero.add(2500);
729         T a = zero.add(7187990.1979844316);
730         T e = zero.add(0.5e-4);
731         T i = zero.add(1.7105407051081795);
732         T omega = zero.add(1.9674147913622104);
733         T OMEGA = zero.add(FastMath.toRadians(261));
734         T lv = zero;
735 
736         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
737                         TimeComponents.H00,
738                         TimeScalesFactory.getUTC());
739 
740         FieldKeplerianOrbit<T> orbit = new FieldKeplerianOrbit<>(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
741                         FramesFactory.getEME2000(), date, zero.add(mu));
742 
743         BodyCenterPointing attitudeLaw = new BodyCenterPointing(orbit.getFrame(), earth);
744 
745         FieldEcksteinHechlerPropagator<T> propagator = new FieldEcksteinHechlerPropagator<>(orbit, attitudeLaw, mass,
746                                                                                ae, mu, c20, c30, c40, c50, c60);
747         FieldAbsoluteDate<T> centerDate = orbit.getDate().shiftedBy(100.0);
748         FieldSpacecraftState<T> centerState = propagator.propagate(centerDate).
749                                               addAdditionalState("quadratic", zero).
750                                               addAdditionalStateDerivative("quadratic-dot", zero);
751         List<FieldSpacecraftState<T>> sample = new ArrayList<FieldSpacecraftState<T>>();
752         for (int ii = 0; ii < n; ++ii) {
753             double dt = ii * 900.0 / (n - 1);
754             FieldSpacecraftState<T> state = propagator.propagate(centerDate.shiftedBy(dt));
755             state = state.
756                     addAdditionalState("quadratic", zero.add(dt * dt)).
757                     addAdditionalStateDerivative("quadratic-dot", zero.add(dt * dt));
758             sample.add(state);
759         }
760         double maxErrorP  = 0;
761         double maxErrorV  = 0;
762         double maxErrorA  = 0;
763         double maxErrorM  = 0;
764         double maxErrorQ  = 0;
765         double maxErrorD = 0;
766         for (double dt = 0; dt < 900.0; dt += 5) {
767             FieldSpacecraftState<T> interpolated = centerState.interpolate(centerDate.shiftedBy(dt), sample);
768             FieldSpacecraftState<T> propagated = propagator.propagate(centerDate.shiftedBy(dt));
769             FieldPVCoordinates<T> dpv = new FieldPVCoordinates<>(propagated.getPVCoordinates(), interpolated.getPVCoordinates());
770             maxErrorP = FastMath.max(maxErrorP, dpv.getPosition().getNorm().getReal());
771             maxErrorV = FastMath.max(maxErrorV, dpv.getVelocity().getNorm().getReal());
772             maxErrorA = FastMath.max(maxErrorA, FastMath.toDegrees(FieldRotation.distance(interpolated.getAttitude().getRotation(),
773                                                                                                   propagated.getAttitude().getRotation()).getReal()));
774             maxErrorM = FastMath.max(maxErrorM, FastMath.abs(interpolated.getMass().getReal() - propagated.getMass().getReal()));
775             maxErrorQ = FastMath.max(maxErrorQ, FastMath.abs(interpolated.getAdditionalState("quadratic")[0].getReal() - dt * dt));
776             maxErrorD = FastMath.max(maxErrorD, FastMath.abs(interpolated.getAdditionalStateDerivative("quadratic-dot")[0].getReal() - dt * dt));
777         }
778         Assert.assertEquals(expectedErrorP, maxErrorP, 1.0e-3);
779         Assert.assertEquals(expectedErrorV, maxErrorV, 1.0e-6);
780         Assert.assertEquals(expectedErrorA, maxErrorA, 4.0e-10);
781         Assert.assertEquals(expectedErrorM, maxErrorM, 1.0e-15);
782         Assert.assertEquals(expectedErrorQ, maxErrorQ, 2.0e-10);
783         Assert.assertEquals(expectedErrorD, maxErrorD, 2.0e-10);
784     }
785 
786     private <T extends CalculusFieldElement<T>> void doTestFieldVsRealAbsPV(final Field<T> field) {
787         T zero = field.getZero();
788 
789         T x_f     = zero.add(0.8);
790         T y_f     = zero.add(0.2);
791         T z_f     = zero;
792         T vx_f    = zero;
793         T vy_f    = zero;
794         T vz_f    = zero.add(0.1);
795 
796         FieldAbsoluteDate<T> t_f = new FieldAbsoluteDate<>(field);
797 
798         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
799 
800         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), t_f, pva_f);
801 
802         FieldSpacecraftState<T> ScS_f = new FieldSpacecraftState<>(absPV_f);
803         FieldSpacecraftState<T> withM = new FieldSpacecraftState<>(absPV_f, zero.newInstance(1234.5));
804         Assert.assertEquals(1234.5, withM.getMass().getReal(), 1.0e-10);
805         SpacecraftState ScS_r = ScS_f.toSpacecraftState();
806 
807         for (double dt = 0; dt < 500; dt+=100){
808             SpacecraftState control_r = ScS_r.shiftedBy(dt);
809             FieldSpacecraftState<T> control_f = ScS_f.shiftedBy(zero.add(dt));
810 
811 
812             Assert.assertEquals(control_r.getMu(), control_f.getMu().getReal(), 1e-10);
813             Assert.assertEquals(control_r.getI(), control_f.getI().getReal(), 1e-10);
814             Assert.assertEquals(control_r.getHx(), control_f.getHx().getReal(), 1e-10);
815             Assert.assertEquals(control_r.getHy(), control_f.getHy().getReal(), 1e-10);
816             Assert.assertEquals(control_r.getLv(), control_f.getLv().getReal(), 1e-10);
817             Assert.assertEquals(control_r.getLE(), control_f.getLE().getReal(), 1e-10);
818             Assert.assertEquals(control_r.getLM(), control_f.getLM().getReal(), 1e-10);
819             Assert.assertEquals(control_r.getKeplerianMeanMotion(), control_f.getKeplerianMeanMotion().getReal(), 1e-10);
820             Assert.assertEquals(control_r.getKeplerianPeriod(), control_f.getKeplerianPeriod().getReal(), 1e-10);
821             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getX(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getX(), 1e-10);
822             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getY(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getY(), 1e-10);
823             Assert.assertEquals(control_r.getPVCoordinates().getPosition().getZ(), control_f.getPVCoordinates().toPVCoordinates().getPosition().getZ(), 1e-10);
824             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getX(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getX(), 1e-10);
825             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getY(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getY(), 1e-10);
826             Assert.assertEquals(control_r.getPVCoordinates().getVelocity().getZ(), control_f.getPVCoordinates().toPVCoordinates().getVelocity().getZ(), 1e-10);
827             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getX(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getX(), 1e-10);
828             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getY(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getY(), 1e-10);
829             Assert.assertEquals(control_r.getPVCoordinates().getAcceleration().getZ(), control_f.getPVCoordinates().toPVCoordinates().getAcceleration().getZ(), 1e-10);
830             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ0(), control_f.getAttitude().getOrientation().getRotation().getQ0().getReal(), 1e-10);
831             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ1(), control_f.getAttitude().getOrientation().getRotation().getQ1().getReal(), 1e-10);
832             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ2(), control_f.getAttitude().getOrientation().getRotation().getQ2().getReal(), 1e-10);
833             Assert.assertEquals(control_r.getAttitude().getOrientation().getRotation().getQ3(), control_f.getAttitude().getOrientation().getRotation().getQ3().getReal(), 1e-10);
834 
835             Assert.assertEquals(control_r.getAttitude().getSpin().getAlpha(), control_f.getAttitude().getSpin().getAlpha().getReal(), 1e-10);
836             Assert.assertEquals(control_r.getAttitude().getSpin().getDelta(), control_f.getAttitude().getSpin().getDelta().getReal(), 1e-10);
837             Assert.assertEquals(control_r.getAttitude().getSpin().getNorm(), control_f.getAttitude().getSpin().getNorm().getReal(), 1e-10);
838 
839             Assert.assertEquals(control_r.getAttitude().getReferenceFrame().isPseudoInertial(), control_f.getAttitude().getReferenceFrame().isPseudoInertial());
840             Assert.assertEquals(control_r.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH), control_f.getAttitude().getDate().durationFrom(AbsoluteDate.J2000_EPOCH).getReal(), 1e-10);
841 
842 
843         }
844     }
845 
846 
847     /**
848      * Check orbit and attitude dates can be off by a few ulps. I see this when using
849      * FixedRate attitude provider.
850      */
851     private <T extends CalculusFieldElement<T>> void doTestDateConsistencyCloseAbsPV(final Field<T> field) {
852 
853 
854         //setup
855         T zero = field.getZero();
856         T one  = field.getOne();
857         T x_f     = zero.add(0.8);
858         T y_f     = zero.add(0.2);
859         T z_f     = zero;
860         T vx_f    = zero;
861         T vy_f    = zero;
862         T vz_f    = zero.add(0.1);
863         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
864                                                             TimeComponents.H00,
865                                                             TimeScalesFactory.getUTC());
866         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
867 
868         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
869 
870         FieldAbsolutePVCoordinates<T> AbsolutePVCoordinates10Shifts = absPV_f;
871         for (int ii = 0; ii < 10; ii++) {
872             AbsolutePVCoordinates10Shifts = AbsolutePVCoordinates10Shifts.shiftedBy(zero.add(0.1));
873         }
874         final FieldAbsolutePVCoordinates<T> AbsolutePVCoordinates1Shift = absPV_f.shiftedBy(one);
875 
876         BodyCenterPointing attitudeLaw = new BodyCenterPointing(absPV_f.getFrame(), earth);
877 
878         FieldAttitude<T> shiftedAttitude = attitudeLaw
879                 .getAttitude(AbsolutePVCoordinates1Shift, AbsolutePVCoordinates1Shift.getDate(), absPV_f.getFrame());
880 
881         //verify dates are very close, but not equal
882         Assert.assertNotEquals(shiftedAttitude.getDate(), AbsolutePVCoordinates10Shifts.getDate());
883         Assert.assertEquals(
884                 shiftedAttitude.getDate().durationFrom(AbsolutePVCoordinates10Shifts.getDate()).getReal(),
885                 0, Precision.EPSILON);
886 
887         //action + verify no exception is thrown
888         FieldSpacecraftState<T> s1 = new FieldSpacecraftState<>(AbsolutePVCoordinates10Shifts, shiftedAttitude);
889         FieldSpacecraftState<T> s2 = s1.shiftedBy(0.001);
890 
891         try {
892             // but here, the time difference is too great
893             new FieldSpacecraftState<>(AbsolutePVCoordinates10Shifts, s2.getAttitude());
894             Assert.fail("an exception should have been thrown");
895         } catch (OrekitIllegalArgumentException oiae) {
896             Assert.assertEquals(OrekitMessages.ORBIT_AND_ATTITUDE_DATES_MISMATCH,
897                                 oiae.getSpecifier());
898         }
899     }
900 
901 
902     // (expected=IllegalArgumentException.class)
903     private <T extends CalculusFieldElement<T>> void doTestFramesConsistencyAbsPV(final Field<T> field) {
904 
905         T zero = field.getZero();
906 
907         T x_f     = zero.add(0.8);
908         T y_f     = zero.add(0.2);
909         T z_f     = zero;
910         T vx_f    = zero;
911         T vy_f    = zero;
912         T vz_f    = zero.add(0.1);
913 
914 
915         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
916                         TimeComponents.H00,
917                         TimeScalesFactory.getUTC());
918 
919         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
920 
921         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
922 
923         new FieldSpacecraftState<>(absPV_f,
924                         new FieldAttitude<>(absPV_f.getDate(),
925                                         FramesFactory.getGCRF(),
926                                         FieldRotation.getIdentity(field),
927                                         FieldVector3D.getZero(field),
928                                         FieldVector3D.getZero(field)));
929     }
930 
931     private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesAbsPV(final Field<T> field) {
932 
933         T zero = field.getZero();
934         T x_f     = zero.add(0.8);
935         T y_f     = zero.add(0.2);
936         T z_f     = zero;
937         T vx_f    = zero;
938         T vy_f    = zero;
939         T vz_f    = zero.add(0.1);
940 
941         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
942                                                             TimeComponents.H00,
943                                                             TimeScalesFactory.getUTC());
944 
945         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
946 
947         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
948 
949         FieldNumericalPropagator<T> prop = new FieldNumericalPropagator<>(field,
950                         new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001));
951         prop.setOrbitType(null);
952 
953         final FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(absPV_f);
954 
955         prop.resetInitialState(initialState);
956 
957         final FieldSpacecraftState<T> state = prop.propagate(absPV_f.getDate().shiftedBy(60));
958         T[] add = MathArrays.buildArray(field, 2);
959         add[0] = zero.add(1.);
960         add[1] = zero.add(2.);
961         final FieldSpacecraftState<T> extended =
962                 state.
963                  addAdditionalState("test-1", add).
964                   addAdditionalState("test-2", zero.add(42.0));
965         Assert.assertEquals(0, state.getAdditionalStatesValues().size());
966         Assert.assertFalse(state.hasAdditionalState("test-1"));
967         try {
968             state.getAdditionalState("test-1");
969             Assert.fail("an exception should have been thrown");
970         } catch (OrekitException oe) {
971             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
972             Assert.assertEquals(oe.getParts()[0], "test-1");
973         }
974         try {
975             state.ensureCompatibleAdditionalStates(extended);
976             Assert.fail("an exception should have been thrown");
977         } catch (OrekitException oe) {
978             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
979             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
980         }
981         try {
982             extended.ensureCompatibleAdditionalStates(state);
983             Assert.fail("an exception should have been thrown");
984         } catch (OrekitException oe) {
985             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
986             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
987         }
988         try {
989             T[] kk = MathArrays.buildArray(field, 7);
990             extended.ensureCompatibleAdditionalStates(extended.addAdditionalState("test-2", kk));
991             Assert.fail("an exception should have been thrown");
992         } catch (MathIllegalStateException mise) {
993             Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
994             Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
995         }
996         Assert.assertEquals(2, extended.getAdditionalStatesValues().size());
997         Assert.assertTrue(extended.hasAdditionalState("test-1"));
998         Assert.assertTrue(extended.hasAdditionalState("test-2"));
999         Assert.assertEquals( 1.0, extended.getAdditionalState("test-1")[0].getReal(), 1.0e-15);
1000         Assert.assertEquals( 2.0, extended.getAdditionalState("test-1")[1].getReal(), 1.0e-15);
1001         Assert.assertEquals(42.0, extended.getAdditionalState("test-2")[0].getReal(), 1.0e-15);
1002 
1003         // test various constructors
1004         T[] dd = MathArrays.buildArray(field, 1);
1005         dd[0] = zero.add(-6.0);
1006         FieldArrayDictionary<T> map = new FieldArrayDictionary<>(field);
1007         map.put("test-3", dd);
1008         FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getAbsPVA(), map);
1009         Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1010         FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), map);
1011         Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1012         FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getMass(), map);
1013         Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1014         FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), map);
1015         Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1016         FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
1017         Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1018 
1019     }
1020 
1021     @Deprecated
1022     private <T extends CalculusFieldElement<T>> void doTestDeprecatedAbsPV(final Field<T> field) {
1023 
1024         T zero = field.getZero();
1025         T x_f     = zero.add(0.8);
1026         T y_f     = zero.add(0.2);
1027         T z_f     = zero;
1028         T vx_f    = zero;
1029         T vy_f    = zero;
1030         T vz_f    = zero.add(0.1);
1031 
1032         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
1033                                                             TimeComponents.H00,
1034                                                             TimeScalesFactory.getUTC());
1035 
1036         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
1037 
1038         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
1039 
1040         FieldNumericalPropagator<T> prop = new FieldNumericalPropagator<>(field,
1041                         new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001));
1042         prop.setOrbitType(null);
1043 
1044         final FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(absPV_f);
1045 
1046         prop.resetInitialState(initialState);
1047 
1048         final FieldSpacecraftState<T> state = prop.propagate(absPV_f.getDate().shiftedBy(60));
1049 
1050         // test various constructors
1051         T[] dd = MathArrays.buildArray(field, 1);
1052         dd[0] = zero.add(-6.0);
1053         Map<String, T[]> map = new HashMap<String, T[]>();
1054         map.put("test-3", dd);
1055         FieldSpacecraftState<T> sO = new FieldSpacecraftState<>(state.getAbsPVA(), map);
1056         Assert.assertEquals(-6.0, sO.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1057         FieldSpacecraftState<T> sOA = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), map);
1058         Assert.assertEquals(-6.0, sOA.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1059         FieldSpacecraftState<T> sOM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getMass(), map);
1060         Assert.assertEquals(-6.0, sOM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1061         FieldSpacecraftState<T> sOAM = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), map);
1062         Assert.assertEquals(-6.0, sOAM.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1063         FieldSpacecraftState<T> sFromDouble = new FieldSpacecraftState<>(field, sOAM.toSpacecraftState());
1064         Assert.assertEquals(-6.0, sFromDouble.getAdditionalState("test-3")[0].getReal(), 1.0e-15);
1065 
1066     }
1067 
1068     private <T extends CalculusFieldElement<T>> void doTestAdditionalStatesDerivativesAbsPV(final Field<T> field) {
1069 
1070         T zero = field.getZero();
1071         T x_f     = zero.add(0.8);
1072         T y_f     = zero.add(0.2);
1073         T z_f     = zero;
1074         T vx_f    = zero;
1075         T vy_f    = zero;
1076         T vz_f    = zero.add(0.1);
1077 
1078         FieldAbsoluteDate<T> date = new FieldAbsoluteDate<>(field, new DateComponents(2004, 01, 01),
1079                                                             TimeComponents.H00,
1080                                                             TimeScalesFactory.getUTC());
1081 
1082         FieldPVCoordinates<T> pva_f = new FieldPVCoordinates<>(new FieldVector3D<>(x_f,y_f,z_f), new FieldVector3D<>(vx_f,vy_f,vz_f));
1083 
1084         FieldAbsolutePVCoordinates<T> absPV_f = new FieldAbsolutePVCoordinates<>(FramesFactory.getEME2000(), date, pva_f);
1085 
1086         FieldNumericalPropagator<T> prop = new FieldNumericalPropagator<>(field,
1087                         new DormandPrince853FieldIntegrator<>(field, 0.1, 500, 0.001, 0.001));
1088         prop.setOrbitType(null);
1089 
1090         final FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(absPV_f);
1091 
1092         prop.resetInitialState(initialState);
1093 
1094         final FieldSpacecraftState<T> state = prop.propagate(absPV_f.getDate().shiftedBy(60));
1095         T[] add = MathArrays.buildArray(field, 2);
1096         add[0] = zero.add(1.);
1097         add[1] = zero.add(2.);
1098         final FieldSpacecraftState<T> extended =
1099                 state.
1100                  addAdditionalStateDerivative("test-1", add).
1101                   addAdditionalStateDerivative("test-2", zero.add(42.0));
1102         Assert.assertEquals(0, state.getAdditionalStatesDerivatives().size());
1103         Assert.assertFalse(state.hasAdditionalStateDerivative("test-1"));
1104         try {
1105             state.getAdditionalStateDerivative("test-1");
1106             Assert.fail("an exception should have been thrown");
1107         } catch (OrekitException oe) {
1108             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
1109             Assert.assertEquals(oe.getParts()[0], "test-1");
1110         }
1111         try {
1112             state.ensureCompatibleAdditionalStates(extended);
1113             Assert.fail("an exception should have been thrown");
1114         } catch (OrekitException oe) {
1115             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
1116             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
1117         }
1118         try {
1119             extended.ensureCompatibleAdditionalStates(state);
1120             Assert.fail("an exception should have been thrown");
1121         } catch (OrekitException oe) {
1122             Assert.assertEquals(oe.getSpecifier(), OrekitMessages.UNKNOWN_ADDITIONAL_STATE);
1123             Assert.assertTrue(oe.getParts()[0].toString().startsWith("test-"));
1124         }
1125         try {
1126             T[] kk = MathArrays.buildArray(field, 7);
1127             extended.ensureCompatibleAdditionalStates(extended.addAdditionalStateDerivative("test-2", kk));
1128             Assert.fail("an exception should have been thrown");
1129         } catch (MathIllegalStateException mise) {
1130             Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH, mise.getSpecifier());
1131             Assert.assertEquals(7, ((Integer) mise.getParts()[0]).intValue());
1132         }
1133         Assert.assertEquals(2, extended.getAdditionalStatesDerivatives().size());
1134         Assert.assertTrue(extended.hasAdditionalStateDerivative("test-1"));
1135         Assert.assertTrue(extended.hasAdditionalStateDerivative("test-2"));
1136         Assert.assertEquals( 1.0, extended.getAdditionalStateDerivative("test-1")[0].getReal(), 1.0e-15);
1137         Assert.assertEquals( 2.0, extended.getAdditionalStateDerivative("test-1")[1].getReal(), 1.0e-15);
1138         Assert.assertEquals(42.0, extended.getAdditionalStateDerivative("test-2")[0].getReal(), 1.0e-15);
1139 
1140         // test most complete constructor
1141         T[] dd = MathArrays.buildArray(field, 1);
1142         dd[0] = zero.add(-6.0);
1143         FieldArrayDictionary<T> dictionary = new FieldArrayDictionary<T>(field);
1144         dictionary.put("test-3", dd);
1145         FieldSpacecraftState<T> s = new FieldSpacecraftState<>(state.getAbsPVA(), state.getAttitude(), state.getMass(), null, dictionary);
1146         Assert.assertFalse(s.hasAdditionalState("test-3"));
1147         Assert.assertEquals(-6.0, s.getAdditionalStateDerivative("test-3")[0].getReal(), 1.0e-15);
1148 
1149     }
1150 
1151     private <T extends CalculusFieldElement<T>> void doTestAdditionalTestResetOnEventAnalytical(final Field<T> field) {
1152 
1153         T zero = field.getZero();
1154 
1155         // Build orbit
1156         FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1157         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1158                                          PositionAngle.TRUE, FramesFactory.getGCRF(), date0,
1159                                          zero.add(Constants.WGS84_EARTH_MU));
1160 
1161         // Build propagator
1162         FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1163 
1164         // Create initial state with one additional state and add it to the propagator
1165         final String name = "A";
1166         FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(orbit).
1167                                        addAdditionalState(name, zero.add(-1));
1168 
1169         propagator.resetInitialState(initialState);
1170 
1171         // Create date detector and handler
1172         FieldAbsoluteDate<T> changeDate = date0.shiftedBy(3);
1173         FieldDateDetector<T> dateDetector = new FieldDateDetector<>(changeDate).
1174                                     withHandler(new FieldEventHandler<FieldDateDetector<T>, T>() {
1175 
1176             @Override
1177             public Action eventOccurred(FieldSpacecraftState<T> s, FieldDateDetector<T> detector, boolean increasing) {
1178               return Action.RESET_STATE;
1179             }
1180 
1181             @Override
1182             public FieldSpacecraftState<T> resetState(FieldDateDetector<T> detector, FieldSpacecraftState<T> oldState) {
1183                 return oldState.addAdditionalState(name, zero.add(+1));
1184             }
1185 
1186         });
1187 
1188         propagator.addEventDetector(dateDetector);
1189         propagator.setStepHandler(zero.add(0.125), s -> {
1190             if (s.getDate().durationFrom(changeDate).getReal() < -0.001) {
1191                 Assert.assertEquals(-1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1192             } else if (s.getDate().durationFrom(changeDate).getReal() > +0.001) {
1193                 Assert.assertEquals(+1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1194             }
1195         });
1196         FieldSpacecraftState<T> finalState = propagator.propagate(date0, date0.shiftedBy(5));
1197         Assert.assertEquals(+1, finalState.getAdditionalState(name)[0].getReal(), 1.0e-15);
1198 
1199     }
1200 
1201     private <T extends CalculusFieldElement<T>> void doTestAdditionalTestResetOnEventNumerical(final Field<T> field) {
1202 
1203         T zero = field.getZero();
1204 
1205         // Build orbit
1206         FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1207         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1208                                          PositionAngle.TRUE, FramesFactory.getGCRF(), date0,
1209                                          zero.add(Constants.WGS84_EARTH_MU));
1210 
1211         // Build propagator
1212         FieldODEIntegrator<T> odeIntegrator = new DormandPrince853FieldIntegrator<>(field, 1E-3, 1E3, 1E-6, 1E-6);
1213         FieldNumericalPropagator<T> propagator = new FieldNumericalPropagator<>(field, odeIntegrator);
1214 
1215         // Create initial state with one additional state and add it to the propagator
1216         final String name = "A";
1217         FieldSpacecraftState<T> initialState = new FieldSpacecraftState<>(orbit).
1218                                        addAdditionalState(name, zero.add(-1));
1219 
1220         propagator.resetInitialState(initialState);
1221 
1222         // Create date detector and handler
1223         FieldAbsoluteDate<T> changeDate = date0.shiftedBy(3);
1224         FieldDateDetector<T> dateDetector = new FieldDateDetector<>(changeDate).
1225                                     withHandler(new FieldEventHandler<FieldDateDetector<T>, T>() {
1226 
1227             @Override
1228             public Action eventOccurred(FieldSpacecraftState<T> s, FieldDateDetector<T> detector, boolean increasing) {
1229               return Action.RESET_STATE;
1230             }
1231 
1232             @Override
1233             public FieldSpacecraftState<T> resetState(FieldDateDetector<T> detector, FieldSpacecraftState<T> oldState) {
1234                 return oldState.addAdditionalState(name, zero.add(+1));
1235             }
1236 
1237         });
1238 
1239         propagator.addEventDetector(dateDetector);
1240         propagator.setStepHandler(zero.add(0.125), s -> {
1241             if (s.getDate().durationFrom(changeDate).getReal() < -0.001) {
1242                 Assert.assertEquals(-1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1243             } else if (s.getDate().durationFrom(changeDate).getReal() > +0.001) {
1244                 Assert.assertEquals(+1, s.getAdditionalState(name)[0].getReal(), 1.0e-15);
1245             }
1246         });
1247         FieldSpacecraftState<T> finalState = propagator.propagate(date0, date0.shiftedBy(5));
1248         Assert.assertEquals(+1, finalState.getAdditionalState(name)[0].getReal(), 1.0e-15);
1249 
1250     }
1251 
1252     private <T extends CalculusFieldElement<T>> void doTestIssue775(final Field<T> field) {
1253         final T zero = field.getZero();
1254         // Conversion from double to Field
1255          FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 01), TimeComponents.H00, TimeScalesFactory.getUTC()));
1256          FieldAbsoluteDate<T> finalDate = new FieldAbsoluteDate<>(field, new AbsoluteDate(new DateComponents(2004, 01, 02), TimeComponents.H00, TimeScalesFactory.getUTC()));
1257          Frame inertialFrame = FramesFactory.getEME2000();
1258 
1259          // Initial PV coordinates
1260          FieldAbsolutePVCoordinates<T> initPV = new FieldAbsolutePVCoordinates<>(inertialFrame,
1261                  new TimeStampedFieldPVCoordinates<>(initDate,
1262                          new FieldPVCoordinates<>(new FieldVector3D<>(zero.add(-29536113.0), zero.add(30329259.0), zero.add(-100125.0)),
1263                                            new FieldVector3D<>(zero.add(-2194.0), zero.add(-2141.0), zero.add(-8.0))))) ;
1264 
1265          // Input parameters
1266          int numberOfInterals = 15;
1267          T deltaT = finalDate.durationFrom(initDate).divide(numberOfInterals);
1268 
1269          // Build the list of spacecraft states
1270          List<FieldSpacecraftState<T>> states = new ArrayList<FieldSpacecraftState<T>>(numberOfInterals + 1);
1271          for (int j = 0; j<= numberOfInterals; j++) {
1272              states.add(new FieldSpacecraftState<T>(initPV).shiftedBy(deltaT.multiply(j)));
1273          }
1274 
1275          // Get initial state without orbit
1276           FieldSpacecraftState<T> withoutOrbit = states.get(0);
1277          // Interpolation
1278           withoutOrbit = withoutOrbit.interpolate(states.get(10).getDate(), states.stream());
1279           Assert.assertEquals(0.0, FieldVector3D.distance(withoutOrbit.getAbsPVA().getPosition(), states.get(10).getAbsPVA().getPosition()).getReal(), 1.0e-10);
1280     }
1281 
1282     private <T extends CalculusFieldElement<T>> void doTestShiftAdditionalDerivativesDouble(final Field<T> field) {
1283 
1284         T zero = field.getZero();
1285 
1286         // Build orbit
1287         FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1288         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1289                                          PositionAngle.TRUE, FramesFactory.getGCRF(), date0,
1290                                          zero.add(Constants.WGS84_EARTH_MU));
1291 
1292         // Build propagator
1293         FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1294 
1295         final String valueAndDerivative = "value-and-derivative";
1296         final String valueOnly          = "value-only";
1297         final String derivativeOnly     = "derivative-only";
1298         final FieldSpacecraftState<T> s0 = propagator.getInitialState().
1299                                            addAdditionalState(valueAndDerivative,           convert(field, new double[] { 1.0,  2.0 })).
1300                                            addAdditionalStateDerivative(valueAndDerivative, convert(field, new double[] { 3.0,  2.0 })).
1301                                            addAdditionalState(valueOnly,                    convert(field, new double[] { 5.0,  4.0 })).
1302                                            addAdditionalStateDerivative(derivativeOnly,     convert(field, new double[] { 1.0, -1.0 }));
1303         Assert.assertEquals( 1.0, s0.getAdditionalState(valueAndDerivative)[0].getReal(),           1.0e-15);
1304         Assert.assertEquals( 2.0, s0.getAdditionalState(valueAndDerivative)[1].getReal(),           1.0e-15);
1305         Assert.assertEquals( 3.0, s0.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1306         Assert.assertEquals( 2.0, s0.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1307         Assert.assertEquals( 5.0, s0.getAdditionalState(valueOnly)[0].getReal(),                    1.0e-15);
1308         Assert.assertEquals( 4.0, s0.getAdditionalState(valueOnly)[1].getReal(),                    1.0e-15);
1309         Assert.assertEquals( 1.0, s0.getAdditionalStateDerivative(derivativeOnly)[0].getReal(),     1.0e-15);
1310         Assert.assertEquals(-1.0, s0.getAdditionalStateDerivative(derivativeOnly)[1].getReal(),     1.0e-15);
1311         final FieldSpacecraftState<T> s1 = s0.shiftedBy(-2.0);
1312         Assert.assertEquals(-5.0, s1.getAdditionalState(valueAndDerivative)[0].getReal(),           1.0e-15);
1313         Assert.assertEquals(-2.0, s1.getAdditionalState(valueAndDerivative)[1].getReal(),           1.0e-15);
1314         Assert.assertEquals( 3.0, s1.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1315         Assert.assertEquals( 2.0, s1.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1316         Assert.assertEquals( 5.0, s1.getAdditionalState(valueOnly)[0].getReal(),                    1.0e-15);
1317         Assert.assertEquals( 4.0, s1.getAdditionalState(valueOnly)[1].getReal(),                    1.0e-15);
1318         Assert.assertEquals( 1.0, s1.getAdditionalStateDerivative(derivativeOnly)[0].getReal(),     1.0e-15);
1319         Assert.assertEquals(-1.0, s1.getAdditionalStateDerivative(derivativeOnly)[1].getReal(),     1.0e-15);
1320 
1321     }
1322 
1323     private <T extends CalculusFieldElement<T>> void doTestShiftAdditionalDerivativesField(final Field<T> field) {
1324 
1325         T zero = field.getZero();
1326 
1327         // Build orbit
1328         FieldAbsoluteDate<T> date0 = new FieldAbsoluteDate<>(field, 2000, 1, 1, TimeScalesFactory.getUTC());
1329         FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(7.1E6), zero, zero, zero, zero, zero,
1330                                          PositionAngle.TRUE, FramesFactory.getGCRF(), date0,
1331                                          zero.add(Constants.WGS84_EARTH_MU));
1332 
1333         // Build propagator
1334         FieldKeplerianPropagator<T> propagator = new FieldKeplerianPropagator<>(orbit);
1335 
1336         final String valueAndDerivative = "value-and-derivative";
1337         final String valueOnly          = "value-only";
1338         final String derivativeOnly     = "derivative-only";
1339         final FieldSpacecraftState<T> s0 = propagator.getInitialState().
1340                                            addAdditionalState(valueAndDerivative,           convert(field, new double[] { 1.0,  2.0 })).
1341                                            addAdditionalStateDerivative(valueAndDerivative, convert(field, new double[] { 3.0,  2.0 })).
1342                                            addAdditionalState(valueOnly,                    convert(field, new double[] { 5.0,  4.0 })).
1343                                            addAdditionalStateDerivative(derivativeOnly,     convert(field, new double[] { 1.0, -1.0 }));
1344         Assert.assertEquals( 1.0, s0.getAdditionalState(valueAndDerivative)[0].getReal(),           1.0e-15);
1345         Assert.assertEquals( 2.0, s0.getAdditionalState(valueAndDerivative)[1].getReal(),           1.0e-15);
1346         Assert.assertEquals( 3.0, s0.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1347         Assert.assertEquals( 2.0, s0.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1348         Assert.assertEquals( 5.0, s0.getAdditionalState(valueOnly)[0].getReal(),                    1.0e-15);
1349         Assert.assertEquals( 4.0, s0.getAdditionalState(valueOnly)[1].getReal(),                    1.0e-15);
1350         Assert.assertEquals( 1.0, s0.getAdditionalStateDerivative(derivativeOnly)[0].getReal(),     1.0e-15);
1351         Assert.assertEquals(-1.0, s0.getAdditionalStateDerivative(derivativeOnly)[1].getReal(),     1.0e-15);
1352         final FieldSpacecraftState<T> s1 = s0.shiftedBy(field.getZero().newInstance(-2.0));
1353         Assert.assertEquals(-5.0, s1.getAdditionalState(valueAndDerivative)[0].getReal(),           1.0e-15);
1354         Assert.assertEquals(-2.0, s1.getAdditionalState(valueAndDerivative)[1].getReal(),           1.0e-15);
1355         Assert.assertEquals( 3.0, s1.getAdditionalStateDerivative(valueAndDerivative)[0].getReal(), 1.0e-15);
1356         Assert.assertEquals( 2.0, s1.getAdditionalStateDerivative(valueAndDerivative)[1].getReal(), 1.0e-15);
1357         Assert.assertEquals( 5.0, s1.getAdditionalState(valueOnly)[0].getReal(),                    1.0e-15);
1358         Assert.assertEquals( 4.0, s1.getAdditionalState(valueOnly)[1].getReal(),                    1.0e-15);
1359         Assert.assertEquals( 1.0, s1.getAdditionalStateDerivative(derivativeOnly)[0].getReal(),     1.0e-15);
1360         Assert.assertEquals(-1.0, s1.getAdditionalStateDerivative(derivativeOnly)[1].getReal(),     1.0e-15);
1361 
1362     }
1363 
1364     @Before
1365     public void setUp(){
1366         try {
1367 
1368             Utils.setDataRoot("regular-data");
1369             mu  = 3.9860047e14;
1370 
1371             double a     = 7187990.1979844316;
1372             double e     = 0.5e-4;
1373             double i     = 1.7105407051081795;
1374             double omega = 1.9674147913622104;
1375             double OMEGA = FastMath.toRadians(261);
1376             double lv    =    0;
1377 
1378             rDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
1379                                      TimeComponents.H00,
1380                                      TimeScalesFactory.getUTC());
1381             rOrbit = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
1382                                         FramesFactory.getEME2000(), rDate, mu);
1383             earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
1384                                          Constants.WGS84_EARTH_FLATTENING,
1385                                          FramesFactory.getITRF(IERSConventions.IERS_2010, true));
1386         } catch (OrekitException oe) {
1387 
1388             Assert.fail(oe.getLocalizedMessage());
1389         }
1390     }
1391 
1392     private <T extends CalculusFieldElement<T>> T[] convert(final Field<T> field, final double[] a) {
1393         final T[] converted = MathArrays.buildArray(field, a.length);
1394         for (int i = 0; i < a.length; ++i) {
1395             converted[i] = field.getZero().newInstance(a[i]);
1396         }
1397         return converted;
1398     }
1399 
1400     private AbsoluteDate rDate;
1401     private double mu;
1402     private Orbit rOrbit;
1403     private OneAxisEllipsoid earth;
1404 
1405 }
1406