1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces;
18
19 import org.hamcrest.MatcherAssert;
20 import org.hamcrest.Matchers;
21 import org.hipparchus.Field;
22 import org.hipparchus.analysis.differentiation.DSFactory;
23 import org.hipparchus.analysis.differentiation.DerivativeStructure;
24 import org.hipparchus.analysis.differentiation.Gradient;
25 import org.hipparchus.analysis.differentiation.GradientField;
26 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
27 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
28 import org.hipparchus.geometry.euclidean.threed.Vector3D;
29 import org.hipparchus.linear.RealMatrix;
30 import org.hipparchus.random.GaussianRandomGenerator;
31 import org.hipparchus.random.RandomGenerator;
32 import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
33 import org.hipparchus.random.Well19937a;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.Precision;
36 import org.junit.jupiter.api.Assertions;
37 import org.orekit.OrekitMatchers;
38 import org.orekit.attitudes.AttitudeProvider;
39 import org.orekit.attitudes.FieldAttitude;
40 import org.orekit.orbits.FieldCartesianOrbit;
41 import org.orekit.orbits.FieldKeplerianOrbit;
42 import org.orekit.orbits.KeplerianOrbit;
43 import org.orekit.orbits.OrbitType;
44 import org.orekit.orbits.PositionAngleType;
45 import org.orekit.propagation.FieldSpacecraftState;
46 import org.orekit.propagation.MatricesHarvester;
47 import org.orekit.propagation.SpacecraftState;
48 import org.orekit.propagation.numerical.FieldNumericalPropagator;
49 import org.orekit.propagation.numerical.NumericalPropagator;
50 import org.orekit.propagation.sampling.OrekitStepHandler;
51 import org.orekit.propagation.sampling.OrekitStepInterpolator;
52 import org.orekit.time.AbsoluteDate;
53 import org.orekit.time.FieldAbsoluteDate;
54 import org.orekit.utils.Constants;
55 import org.orekit.utils.Differentiation;
56 import org.orekit.utils.FieldAbsolutePVCoordinates;
57 import org.orekit.utils.FieldPVCoordinates;
58 import org.orekit.utils.PVCoordinates;
59 import org.orekit.utils.ParameterDriver;
60 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
61 import org.orekit.utils.TimeStampedFieldPVCoordinates;
62
63 import java.util.List;
64 import java.util.concurrent.atomic.AtomicReference;
65
66
67 public abstract class AbstractForceModelTest {
68
69 protected void checkParameterDerivative(SpacecraftState state,
70 ForceModel forceModel, String name,
71 double hFactor, double tol) {
72
73 final DSFactory factory11 = new DSFactory(1, 1);
74 final Field<DerivativeStructure> field = factory11.getDerivativeField();
75 final FieldSpacecraftState<DerivativeStructure> stateF = new FieldSpacecraftState<DerivativeStructure>(field, state);
76 final List<ParameterDriver> drivers = forceModel.getParametersDrivers();
77 final DerivativeStructure[] parametersDS = new DerivativeStructure[drivers.size()];
78 for (int i = 0; i < parametersDS.length; ++i) {
79 if (drivers.get(i).getName().equals(name)) {
80 parametersDS[i] = factory11.variable(0, drivers.get(i).getValue(state.getDate()));
81 } else {
82 parametersDS[i] = factory11.constant(drivers.get(i).getValue(state.getDate()));
83 }
84 }
85 FieldVector3D<DerivativeStructure> accDer = forceModel.acceleration(stateF, parametersDS);
86 Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1),
87 accDer.getY().getPartialDerivative(1),
88 accDer.getZ().getPartialDerivative(1));
89
90 int selected = -1;
91 final double[] parameters = new double[drivers.size()];
92 for (int i = 0; i < drivers.size(); ++i) {
93 parameters[i] = drivers.get(i).getValue(state.getDate());
94 if (drivers.get(i).getName().equals(name)) {
95 selected = i;
96 }
97 }
98 double p0 = parameters[selected];
99 double hParam = hFactor * p0;
100 drivers.get(selected).setValue(p0 - 1 * hParam, state.getDate());
101 parameters[selected] = drivers.get(selected).getValue(state.getDate());
102 Assertions.assertEquals(p0 - 1 * hParam, parameters[selected], 1.0e-10);
103 final Vector3D gammaM1h = forceModel.acceleration(state, parameters);
104 drivers.get(selected).setValue(p0 + 1 * hParam, state.getDate());
105 parameters[selected] = drivers.get(selected).getValue(state.getDate());
106 Assertions.assertEquals(p0 + 1 * hParam, parameters[selected], 1.0e-10);
107 final Vector3D gammaP1h = forceModel.acceleration(state, parameters);
108 drivers.get(selected).setValue(p0, state.getDate());
109
110 final Vector3D reference = new Vector3D( 1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
111 final Vector3D delta = derivative.subtract(reference);
112 Assertions.assertEquals(0., delta.getNorm(), tol * reference.getNorm());
113
114 }
115
116 protected void checkParameterDerivativeGradient(SpacecraftState state,
117 ForceModel forceModel, String name,
118 double hFactor, double tol) {
119
120 final int freeParameters = 1;
121 final Field<Gradient> field = GradientField.getField(freeParameters);
122 final FieldSpacecraftState<Gradient> stateF = new FieldSpacecraftState<>(field, state);
123 final List<ParameterDriver> drivers = forceModel.getParametersDrivers();
124 final Gradient[] parametersDS = new Gradient[drivers.size()];
125 for (int i = 0; i < parametersDS.length; ++i) {
126 if (drivers.get(i).getName().equals(name)) {
127 parametersDS[i] = Gradient.variable(freeParameters, 0, drivers.get(i).getValue(state.getDate()));
128 } else {
129 parametersDS[i] = Gradient.constant(freeParameters, drivers.get(i).getValue(state.getDate()));
130 }
131 }
132 FieldVector3D<Gradient> accDer = forceModel.acceleration(stateF, parametersDS);
133 Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(0),
134 accDer.getY().getPartialDerivative(0),
135 accDer.getZ().getPartialDerivative(0));
136
137 int selected = -1;
138 final double[] parameters = new double[drivers.size()];
139 for (int i = 0; i < drivers.size(); ++i) {
140 parameters[i] = drivers.get(i).getValue(state.getDate());
141 if (drivers.get(i).getName().equals(name)) {
142 selected = i;
143 }
144 }
145 double p0 = parameters[selected];
146 double hParam = hFactor * p0;
147 drivers.get(selected).setValue(p0 - 1 * hParam, state.getDate());
148 parameters[selected] = drivers.get(selected).getValue(state.getDate());
149 Assertions.assertEquals(p0 - 1 * hParam, parameters[selected], 1.0e-10);
150 final Vector3D gammaM1h = forceModel.acceleration(state, parameters);
151 drivers.get(selected).setValue(p0 + 1 * hParam, state.getDate());
152 parameters[selected] = drivers.get(selected).getValue(state.getDate());
153 Assertions.assertEquals(p0 + 1 * hParam, parameters[selected], 1.0e-10);
154 final Vector3D gammaP1h = forceModel.acceleration(state, parameters);
155 drivers.get(selected).setValue(p0, state.getDate());
156
157 final Vector3D reference = new Vector3D( 1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
158 final Vector3D delta = derivative.subtract(reference);
159 Assertions.assertEquals(0, delta.getNorm(), tol * reference.getNorm());
160
161 }
162
163 protected FieldSpacecraftState<DerivativeStructure> toDS(final SpacecraftState state,
164 final AttitudeProvider attitudeProvider) {
165
166 final Vector3D p = state.getPosition();
167 final Vector3D v = state.getVelocity();
168 final Vector3D a = state.getPVCoordinates().getAcceleration();
169 DSFactory factory = new DSFactory(6, 1);
170 Field<DerivativeStructure> field = factory.getDerivativeField();
171 final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state.getDate());
172 final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA =
173 new TimeStampedFieldPVCoordinates<>(fDate,
174 new FieldVector3D<>(factory.variable(0, p.getX()),
175 factory.variable(1, p.getY()),
176 factory.variable(2, p.getZ())),
177 new FieldVector3D<>(factory.variable(3, v.getX()),
178 factory.variable(4, v.getY()),
179 factory.variable(5, v.getZ())),
180 new FieldVector3D<>(factory.constant(a.getX()),
181 factory.constant(a.getY()),
182 factory.constant(a.getZ())));
183 final DerivativeStructure fMass = field.getZero().newInstance(state.getMass());
184 if (state.isOrbitDefined()) {
185 final FieldCartesianOrbit<DerivativeStructure> orbit =
186 new FieldCartesianOrbit<>(fPVA, state.getFrame(), field.getZero().newInstance(state.getOrbit().getMu()));
187 final FieldAttitude<DerivativeStructure> attitude =
188 attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
189
190 return new FieldSpacecraftState<>(orbit, attitude).withMass(fMass);
191 } else {
192 final FieldAbsolutePVCoordinates<DerivativeStructure> absolutePVCoordinates =
193 new FieldAbsolutePVCoordinates<>(state.getFrame(), fPVA);
194 final FieldAttitude<DerivativeStructure> attitude =
195 attitudeProvider.getAttitude(absolutePVCoordinates, absolutePVCoordinates.getDate(), absolutePVCoordinates.getFrame());
196
197 return new FieldSpacecraftState<>(absolutePVCoordinates, attitude).withMass(fMass);
198 }
199
200 }
201
202 protected FieldSpacecraftState<Gradient> toGradient(final SpacecraftState state,
203 final AttitudeProvider attitudeProvider) {
204
205 final Vector3D p = state.getPosition();
206 final Vector3D v = state.getVelocity();
207 final Vector3D a = state.getPVCoordinates().getAcceleration();
208 final int freeParameters = 6;
209 Field<Gradient> field = GradientField.getField(freeParameters);
210 final FieldAbsoluteDate<Gradient> fDate = new FieldAbsoluteDate<>(field, state.getDate());
211 final TimeStampedFieldPVCoordinates<Gradient> fPVA =
212 new TimeStampedFieldPVCoordinates<>(fDate,
213 new FieldVector3D<>(Gradient.variable(freeParameters, 0, p.getX()),
214 Gradient.variable(freeParameters, 1, p.getY()),
215 Gradient.variable(freeParameters, 2, p.getZ())),
216 new FieldVector3D<>(Gradient.variable(freeParameters, 3, v.getX()),
217 Gradient.variable(freeParameters, 4, v.getY()),
218 Gradient.variable(freeParameters, 5, v.getZ())),
219 new FieldVector3D<>(Gradient.constant(freeParameters, a.getX()),
220 Gradient.constant(freeParameters, a.getY()),
221 Gradient.constant(freeParameters, a.getZ())));
222 final Gradient gMass = field.getZero().newInstance(state.getMass());
223 if (state.isOrbitDefined()) {
224 final FieldCartesianOrbit<Gradient> orbit =
225 new FieldCartesianOrbit<>(fPVA, state.getFrame(), field.getZero().add(state.getOrbit().getMu()));
226 final FieldAttitude<Gradient> attitude =
227 attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
228
229 return new FieldSpacecraftState<>(orbit, attitude).withMass(gMass);
230 } else {
231 final FieldAbsolutePVCoordinates<Gradient> absolutePVCoordinates =
232 new FieldAbsolutePVCoordinates<>(state.getFrame(), fPVA);
233 final FieldAttitude<Gradient> attitude =
234 attitudeProvider.getAttitude(absolutePVCoordinates, absolutePVCoordinates.getDate(), absolutePVCoordinates.getFrame());
235
236 return new FieldSpacecraftState<>(absolutePVCoordinates, attitude).withMass(gMass);
237 }
238
239 }
240
241 protected void checkStateJacobianVsFiniteDifferences(final SpacecraftState state0, final ForceModel forceModel,
242 final AttitudeProvider provider, final double dP,
243 final double checkTolerance, final boolean print) {
244
245 double[][] finiteDifferencesJacobian =
246 Differentiation.differentiate(state -> forceModel.acceleration(state, forceModel.getParameters(state0.getDate())).toArray(),
247 3, provider, OrbitType.CARTESIAN, PositionAngleType.MEAN,
248 dP, 5).
249 value(state0);
250
251 DSFactory factory = new DSFactory(6, 1);
252 Field<DerivativeStructure> field = factory.getDerivativeField();
253 final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state0.getDate());
254 final Vector3D p = state0.getPosition();
255 final Vector3D v = state0.getVelocity();
256 final Vector3D a = state0.getPVCoordinates().getAcceleration();
257 final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA =
258 new TimeStampedFieldPVCoordinates<>(fDate,
259 new FieldVector3D<>(factory.variable(0, p.getX()),
260 factory.variable(1, p.getY()),
261 factory.variable(2, p.getZ())),
262 new FieldVector3D<>(factory.variable(3, v.getX()),
263 factory.variable(4, v.getY()),
264 factory.variable(5, v.getZ())),
265 new FieldVector3D<>(factory.constant(a.getX()),
266 factory.constant(a.getY()),
267 factory.constant(a.getZ())));
268 final TimeStampedFieldAngularCoordinates<DerivativeStructure> fAC =
269 new TimeStampedFieldAngularCoordinates<>(fDate,
270 new FieldRotation<>(field, state0.getAttitude().getRotation()),
271 new FieldVector3D<>(field, state0.getAttitude().getSpin()),
272 new FieldVector3D<>(field, state0.getAttitude().getRotationAcceleration()));
273 final FieldSpacecraftState<DerivativeStructure> fState =
274 new FieldSpacecraftState<>(new FieldCartesianOrbit<>(fPVA, state0.getFrame(), field.getZero().add(state0.getOrbit().getMu())),
275 new FieldAttitude<>(state0.getFrame(), fAC)).withMass(
276 field.getZero().add(state0.getMass()));
277 FieldVector3D<DerivativeStructure> dsJacobian = forceModel.acceleration(fState,
278 forceModel.getParameters(fState.getDate().getField(), fState.getDate()));
279
280 Vector3D dFdPXRef = new Vector3D(finiteDifferencesJacobian[0][0],
281 finiteDifferencesJacobian[1][0],
282 finiteDifferencesJacobian[2][0]);
283 Vector3D dFdPXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(1, 0, 0, 0, 0, 0),
284 dsJacobian.getY().getPartialDerivative(1, 0, 0, 0, 0, 0),
285 dsJacobian.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
286 Vector3D dFdPYRef = new Vector3D(finiteDifferencesJacobian[0][1],
287 finiteDifferencesJacobian[1][1],
288 finiteDifferencesJacobian[2][1]);
289 Vector3D dFdPYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 1, 0, 0, 0, 0),
290 dsJacobian.getY().getPartialDerivative(0, 1, 0, 0, 0, 0),
291 dsJacobian.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
292 Vector3D dFdPZRef = new Vector3D(finiteDifferencesJacobian[0][2],
293 finiteDifferencesJacobian[1][2],
294 finiteDifferencesJacobian[2][2]);
295 Vector3D dFdPZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 1, 0, 0, 0),
296 dsJacobian.getY().getPartialDerivative(0, 0, 1, 0, 0, 0),
297 dsJacobian.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
298 Vector3D dFdVXRef = new Vector3D(finiteDifferencesJacobian[0][3],
299 finiteDifferencesJacobian[1][3],
300 finiteDifferencesJacobian[2][3]);
301 Vector3D dFdVXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 1, 0, 0),
302 dsJacobian.getY().getPartialDerivative(0, 0, 0, 1, 0, 0),
303 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
304 Vector3D dFdVYRef = new Vector3D(finiteDifferencesJacobian[0][4],
305 finiteDifferencesJacobian[1][4],
306 finiteDifferencesJacobian[2][4]);
307 Vector3D dFdVYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 1, 0),
308 dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 1, 0),
309 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
310 Vector3D dFdVZRef = new Vector3D(finiteDifferencesJacobian[0][5],
311 finiteDifferencesJacobian[1][5],
312 finiteDifferencesJacobian[2][5]);
313 Vector3D dFdVZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 0, 1),
314 dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 0, 1),
315 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
316 if (print) {
317 System.out.println("dF/dPX ref: " + dFdPXRef.getX() + " " + dFdPXRef.getY() + " " + dFdPXRef.getZ());
318 System.out.println("dF/dPX res: " + dFdPXRes.getX() + " " + dFdPXRes.getY() + " " + dFdPXRes.getZ());
319 System.out.println("dF/dPY ref: " + dFdPYRef.getX() + " " + dFdPYRef.getY() + " " + dFdPYRef.getZ());
320 System.out.println("dF/dPY res: " + dFdPYRes.getX() + " " + dFdPYRes.getY() + " " + dFdPYRes.getZ());
321 System.out.println("dF/dPZ ref: " + dFdPZRef.getX() + " " + dFdPZRef.getY() + " " + dFdPZRef.getZ());
322 System.out.println("dF/dPZ res: " + dFdPZRes.getX() + " " + dFdPZRes.getY() + " " + dFdPZRes.getZ());
323 System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
324 System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
325 System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
326 System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
327 System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
328 System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
329 }
330 checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
331 checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
332 checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
333 checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
334 checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
335 checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
336 }
337
338 protected void checkStateJacobianVsFiniteDifferencesGradient(final SpacecraftState state0, final ForceModel forceModel,
339 final AttitudeProvider provider, final double dP,
340 final double checkTolerance, final boolean print) {
341
342 double[][] finiteDifferencesJacobian =
343 Differentiation.differentiate(state -> forceModel.acceleration(state, forceModel.getParameters(state0.getDate())).toArray(),
344 3, provider, OrbitType.CARTESIAN, PositionAngleType.MEAN,
345 dP, 5).
346 value(state0);
347
348 final int freePrameters = 6;
349 Field<Gradient> field = GradientField.getField(freePrameters);
350 final FieldAbsoluteDate<Gradient> fDate = new FieldAbsoluteDate<>(field, state0.getDate());
351 final Vector3D p = state0.getPosition();
352 final Vector3D v = state0.getVelocity();
353 final Vector3D a = state0.getPVCoordinates().getAcceleration();
354 final TimeStampedFieldPVCoordinates<Gradient> fPVA =
355 new TimeStampedFieldPVCoordinates<>(fDate,
356 new FieldVector3D<>(Gradient.variable(freePrameters, 0, p.getX()),
357 Gradient.variable(freePrameters, 1, p.getY()),
358 Gradient.variable(freePrameters, 2, p.getZ())),
359 new FieldVector3D<>(Gradient.variable(freePrameters, 3, v.getX()),
360 Gradient.variable(freePrameters, 4, v.getY()),
361 Gradient.variable(freePrameters, 5, v.getZ())),
362 new FieldVector3D<>(Gradient.constant(freePrameters, a.getX()),
363 Gradient.constant(freePrameters, a.getY()),
364 Gradient.constant(freePrameters, a.getZ())));
365 final TimeStampedFieldAngularCoordinates<Gradient> fAC =
366 new TimeStampedFieldAngularCoordinates<>(fDate,
367 new FieldRotation<>(field, state0.getAttitude().getRotation()),
368 new FieldVector3D<>(field, state0.getAttitude().getSpin()),
369 new FieldVector3D<>(field, state0.getAttitude().getRotationAcceleration()));
370 final FieldSpacecraftState<Gradient> fState =
371 new FieldSpacecraftState<>(new FieldCartesianOrbit<>(fPVA, state0.getFrame(), field.getZero().add(state0.getOrbit().getMu())),
372 new FieldAttitude<>(state0.getFrame(), fAC)).withMass(
373 field.getZero().add(state0.getMass()));
374 FieldVector3D<Gradient> gJacobian = forceModel.acceleration(fState,
375 forceModel.getParameters(fState.getDate().getField(), fState.getDate()));
376
377 Vector3D dFdPXRef = new Vector3D(finiteDifferencesJacobian[0][0],
378 finiteDifferencesJacobian[1][0],
379 finiteDifferencesJacobian[2][0]);
380 Vector3D dFdPXRes = new Vector3D(gJacobian.getX().getPartialDerivative(0),
381 gJacobian.getY().getPartialDerivative(0),
382 gJacobian.getZ().getPartialDerivative(0));
383 Vector3D dFdPYRef = new Vector3D(finiteDifferencesJacobian[0][1],
384 finiteDifferencesJacobian[1][1],
385 finiteDifferencesJacobian[2][1]);
386 Vector3D dFdPYRes = new Vector3D(gJacobian.getX().getPartialDerivative(1),
387 gJacobian.getY().getPartialDerivative(1),
388 gJacobian.getZ().getPartialDerivative(1));
389 Vector3D dFdPZRef = new Vector3D(finiteDifferencesJacobian[0][2],
390 finiteDifferencesJacobian[1][2],
391 finiteDifferencesJacobian[2][2]);
392 Vector3D dFdPZRes = new Vector3D(gJacobian.getX().getPartialDerivative(2),
393 gJacobian.getY().getPartialDerivative(2),
394 gJacobian.getZ().getPartialDerivative(2));
395 Vector3D dFdVXRef = new Vector3D(finiteDifferencesJacobian[0][3],
396 finiteDifferencesJacobian[1][3],
397 finiteDifferencesJacobian[2][3]);
398 Vector3D dFdVXRes = new Vector3D(gJacobian.getX().getPartialDerivative(3),
399 gJacobian.getY().getPartialDerivative(3),
400 gJacobian.getZ().getPartialDerivative(3));
401 Vector3D dFdVYRef = new Vector3D(finiteDifferencesJacobian[0][4],
402 finiteDifferencesJacobian[1][4],
403 finiteDifferencesJacobian[2][4]);
404 Vector3D dFdVYRes = new Vector3D(gJacobian.getX().getPartialDerivative(4),
405 gJacobian.getY().getPartialDerivative(4),
406 gJacobian.getZ().getPartialDerivative(4));
407 Vector3D dFdVZRef = new Vector3D(finiteDifferencesJacobian[0][5],
408 finiteDifferencesJacobian[1][5],
409 finiteDifferencesJacobian[2][5]);
410 Vector3D dFdVZRes = new Vector3D(gJacobian.getX().getPartialDerivative(5),
411 gJacobian.getY().getPartialDerivative(5),
412 gJacobian.getZ().getPartialDerivative(5));
413 if (print) {
414 System.out.println("dF/dPX ref: " + dFdPXRef.getX() + " " + dFdPXRef.getY() + " " + dFdPXRef.getZ());
415 System.out.println("dF/dPX res: " + dFdPXRes.getX() + " " + dFdPXRes.getY() + " " + dFdPXRes.getZ());
416 System.out.println("dF/dPY ref: " + dFdPYRef.getX() + " " + dFdPYRef.getY() + " " + dFdPYRef.getZ());
417 System.out.println("dF/dPY res: " + dFdPYRes.getX() + " " + dFdPYRes.getY() + " " + dFdPYRes.getZ());
418 System.out.println("dF/dPZ ref: " + dFdPZRef.getX() + " " + dFdPZRef.getY() + " " + dFdPZRef.getZ());
419 System.out.println("dF/dPZ res: " + dFdPZRes.getX() + " " + dFdPZRes.getY() + " " + dFdPZRes.getZ());
420 System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
421 System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
422 System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
423 System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
424 System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
425 System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
426 }
427 checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
428 checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
429 checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
430 checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
431 checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
432 checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
433 }
434
435 private void checkdFdP(final Vector3D reference, final Vector3D result, final double checkTolerance) {
436 MatcherAssert.assertThat(result,
437 OrekitMatchers.vectorCloseTo(reference, Precision.SAFE_MIN, checkTolerance));
438 if (reference.getNorm() == 0) {
439
440
441 Assertions.assertEquals(0, result.getNorm(), Precision.SAFE_MIN);
442 } else {
443 Assertions.assertEquals(0, Vector3D.distance(reference, result), checkTolerance * reference.getNorm());
444 }
445 }
446
447 protected void checkStateJacobian(NumericalPropagator propagator, SpacecraftState state0,
448 AbsoluteDate targetDate, double hFactor,
449 double[] integratorAbsoluteTolerances, double checkTolerance) {
450
451 propagator.setInitialState(state0);
452 double[][] reference = new double[][] {
453 jacobianColumn(propagator, state0, targetDate, 0, hFactor * integratorAbsoluteTolerances[0]),
454 jacobianColumn(propagator, state0, targetDate, 1, hFactor * integratorAbsoluteTolerances[1]),
455 jacobianColumn(propagator, state0, targetDate, 2, hFactor * integratorAbsoluteTolerances[2]),
456 jacobianColumn(propagator, state0, targetDate, 3, hFactor * integratorAbsoluteTolerances[3]),
457 jacobianColumn(propagator, state0, targetDate, 4, hFactor * integratorAbsoluteTolerances[4]),
458 jacobianColumn(propagator, state0, targetDate, 5, hFactor * integratorAbsoluteTolerances[5])
459 };
460 for (int j = 0; j < 6; ++j) {
461 for (int k = j + 1; k < 6; ++k) {
462 double tmp = reference[j][k];
463 reference[j][k] = reference[k][j];
464 reference[k][j] = tmp;
465 }
466 }
467
468 propagator.setInitialState(state0);
469 MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, null);
470 final AtomicReference<RealMatrix> dYdY0 = new AtomicReference<>();
471 propagator.setStepHandler(new OrekitStepHandler() {
472 public void handleStep(OrekitStepInterpolator interpolator) {
473 }
474 public void finish(SpacecraftState finalState) {
475
476 dYdY0.set(harvester.getStateTransitionMatrix(finalState));
477 }
478
479 });
480 propagator.propagate(targetDate);
481
482 for (int j = 0; j < 6; ++j) {
483 for (int k = 0; k < 6; ++k) {
484 double scale = integratorAbsoluteTolerances[j] / integratorAbsoluteTolerances[k];
485 final double actual = dYdY0.get().getEntry(j, k);
486 final double tol = checkTolerance * scale;
487 MatcherAssert.assertThat(actual, Matchers.closeTo(reference[j][k], tol));
488 Assertions.assertEquals(reference[j][k], actual, tol);
489 }
490 }
491
492 }
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510 protected void checkRealFieldPropagation(final FieldKeplerianOrbit<DerivativeStructure> initialOrbit,
511 final PositionAngleType positionAngleType,
512 final double duration,
513 final NumericalPropagator propagator,
514 final FieldNumericalPropagator<DerivativeStructure> fieldpropagator,
515 final double positionRelativeTolerancePropag,
516 final double positionRelativeToleranceTaylor,
517 final double velocityRelativeToleranceTaylor,
518 final double accelerationRelativeToleranceTaylor,
519 final int nbTests,
520 final boolean print) {
521
522
523
524
525
526
527 FieldAbsoluteDate<DerivativeStructure> target = initialOrbit.getDate().shiftedBy(duration);
528 FieldSpacecraftState<DerivativeStructure> finalState_DS = fieldpropagator.propagate(target);
529 SpacecraftState finalState_R = propagator.propagate(target.toAbsoluteDate());
530 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
531 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
532
533
534 final Vector3D finPosition_DS = finPVC_DS.toPVCoordinates().getPosition();
535 final Vector3D finPosition_R = finPVC_R.getPosition();
536 final double finPositionDelta = Vector3D.distance(finPosition_DS, finPosition_R);
537 final double propagPosTol = finPosition_R.getNorm() * positionRelativeTolerancePropag;
538 if (print) {
539 System.out.println("1 - Check ΔP after propagation");
540 System.out.println("\t Pf_DS = " + finPosition_DS.getX() + " " + finPosition_DS.getY() + " " + finPosition_DS.getZ());
541 System.out.println("\t Pf_R = " + finPosition_R.getX() + " " + finPosition_R.getY() + " " + finPosition_R.getZ());
542 System.out.println("\t ΔPf = " + finPositionDelta);
543 System.out.println("\t εP = " + propagPosTol);
544 System.out.println("\tΔPf / ||Pf_R|| = " + finPositionDelta / finPosition_R.getNorm());
545 }
546 Assertions.assertEquals(0., finPositionDelta, propagPosTol);
547
548
549
550
551
552
553
554 long number = 23091991;
555 RandomGenerator RG = new Well19937a(number);
556 GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
557 UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 },
558 new double[] {1e3, 0.005, 0.005, 0.01, 0.01, 0.01},
559 NGG);
560
561 double a_R = initialOrbit.getA().getReal();
562 double e_R = initialOrbit.getE().getReal();
563 double i_R = initialOrbit.getI().getReal();
564 double R_R = initialOrbit.getPerigeeArgument().getReal();
565 double O_R = initialOrbit.getRightAscensionOfAscendingNode().getReal();
566 double n_R = initialOrbit.getAnomaly(positionAngleType).getReal();
567
568
569 double maxP = 0.;
570 double maxV = 0.;
571 double maxA = 0.;
572
573
574 for (int ii = 0; ii < nbTests; ii++){
575
576
577 double[] rand_next = URVG.nextVector();
578 double a_shift = a_R + rand_next[0];
579 double e_shift = e_R + rand_next[1];
580 double i_shift = i_R + rand_next[2];
581 double R_shift = R_R + rand_next[3];
582 double O_shift = O_R + rand_next[4];
583 double n_shift = n_R + rand_next[5];
584
585 KeplerianOrbit shiftedOrb = new KeplerianOrbit(a_shift, e_shift, i_shift, R_shift, O_shift, n_shift,
586 PositionAngleType.MEAN,
587 initialOrbit.getFrame(),
588 initialOrbit.getDate().toAbsoluteDate(),
589 Constants.EIGEN5C_EARTH_MU
590 );
591
592 SpacecraftState shift_iSR = new SpacecraftState(shiftedOrb);
593
594
595 propagator.setInitialState(shift_iSR);
596 SpacecraftState finalState_shift = propagator.propagate(target.toAbsoluteDate());
597 PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
598
599
600
601
602
603
604 FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
605 double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
606 double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
607 double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
608
609
610 double x = finPVC_shift.getPosition().getX();
611 double y = finPVC_shift.getPosition().getY();
612 double z = finPVC_shift.getPosition().getZ();
613
614
615 maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal())));
616 maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal())));
617 maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal())));
618
619
620
621
622
623
624 FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
625 double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
626 double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
627 double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
628
629
630 double vx = finPVC_shift.getVelocity().getX();
631 double vy = finPVC_shift.getVelocity().getY();
632 double vz = finPVC_shift.getVelocity().getZ();
633
634
635 maxV = FastMath.max(maxV, FastMath.abs((vx_DS - vx) / vx));
636 maxV = FastMath.max(maxV, FastMath.abs((vy_DS - vy) / vy));
637 maxV = FastMath.max(maxV, FastMath.abs((vz_DS - vz) / vz));
638
639
640
641
642
643
644 FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
645 double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
646 double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
647 double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
648
649
650 double ax = finPVC_shift.getAcceleration().getX();
651 double ay = finPVC_shift.getAcceleration().getY();
652 double az = finPVC_shift.getAcceleration().getZ();
653
654
655 maxA = FastMath.max(maxA, FastMath.abs((ax_DS - ax) / ax));
656 maxA = FastMath.max(maxA, FastMath.abs((ay_DS - ay) / ay));
657 maxA = FastMath.max(maxA, FastMath.abs((az_DS - az) / az));
658 }
659
660
661 if (print) {
662 System.out.println("\n2 - Check P, V, A with Taylor expansion");
663 System.out.println("\tMax ΔP = " + maxP);
664 System.out.println("\tMax ΔV = " + maxV);
665 System.out.println("\tMax ΔA = " + maxA);
666 }
667 Assertions.assertEquals(0, maxP, positionRelativeToleranceTaylor);
668 Assertions.assertEquals(0, maxV, velocityRelativeToleranceTaylor);
669 Assertions.assertEquals(0, maxA, accelerationRelativeToleranceTaylor);
670 }
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687 protected void checkRealFieldPropagationGradient(final FieldKeplerianOrbit<Gradient> initialOrbit,
688 final PositionAngleType positionAngleType,
689 final double duration,
690 final NumericalPropagator propagator,
691 final FieldNumericalPropagator<Gradient> fieldpropagator,
692 final double positionRelativeTolerancePropag,
693 final double positionRelativeToleranceTaylor,
694 final double velocityRelativeToleranceTaylor,
695 final double accelerationRelativeToleranceTaylor,
696 final int nbTests,
697 final boolean print) {
698
699
700
701
702
703
704 FieldAbsoluteDate<Gradient> target = initialOrbit.getDate().shiftedBy(duration);
705 FieldSpacecraftState<Gradient> finalState_G = fieldpropagator.propagate(target);
706 SpacecraftState finalState_R = propagator.propagate(target.toAbsoluteDate());
707 FieldPVCoordinates<Gradient> finPVC_G = finalState_G.getPVCoordinates();
708 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
709
710
711 final Vector3D finPosition_G = finPVC_G.toPVCoordinates().getPosition();
712 final Vector3D finPosition_R = finPVC_R.getPosition();
713 final double finPositionDelta = Vector3D.distance(finPosition_G, finPosition_R);
714 final double propagPosTol = finPosition_R.getNorm() * positionRelativeTolerancePropag;
715 if (print) {
716 System.out.println("1 - Check ΔP after propagation");
717 System.out.println("\t Pf_DS = " + finPosition_G.getX() + " " + finPosition_G.getY() + " " + finPosition_G.getZ());
718 System.out.println("\t Pf_R = " + finPosition_R.getX() + " " + finPosition_R.getY() + " " + finPosition_R.getZ());
719 System.out.println("\t ΔPf = " + finPositionDelta);
720 System.out.println("\t εP = " + propagPosTol);
721 System.out.println("\tΔPf / ||Pf_R|| = " + finPositionDelta / finPosition_R.getNorm());
722 }
723 Assertions.assertEquals(0., finPositionDelta, propagPosTol);
724
725
726
727
728
729
730
731 long number = 23091991;
732 RandomGenerator RG = new Well19937a(number);
733 GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
734 UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 },
735 new double[] {1e3, 0.005, 0.005, 0.01, 0.01, 0.01},
736 NGG);
737
738 double a_R = initialOrbit.getA().getReal();
739 double e_R = initialOrbit.getE().getReal();
740 double i_R = initialOrbit.getI().getReal();
741 double R_R = initialOrbit.getPerigeeArgument().getReal();
742 double O_R = initialOrbit.getRightAscensionOfAscendingNode().getReal();
743 double n_R = initialOrbit.getAnomaly(positionAngleType).getReal();
744
745
746 double maxP = 0.;
747 double maxV = 0.;
748 double maxA = 0.;
749
750
751 for (int ii = 0; ii < nbTests; ii++){
752
753
754 double[] rand_next = URVG.nextVector();
755 double a_shift = a_R + rand_next[0];
756 double e_shift = e_R + rand_next[1];
757 double i_shift = i_R + rand_next[2];
758 double R_shift = R_R + rand_next[3];
759 double O_shift = O_R + rand_next[4];
760 double n_shift = n_R + rand_next[5];
761
762 KeplerianOrbit shiftedOrb = new KeplerianOrbit(a_shift, e_shift, i_shift, R_shift, O_shift, n_shift,
763 PositionAngleType.MEAN,
764 initialOrbit.getFrame(),
765 initialOrbit.getDate().toAbsoluteDate(),
766 Constants.EIGEN5C_EARTH_MU
767 );
768
769 SpacecraftState shift_iSR = new SpacecraftState(shiftedOrb);
770
771
772 propagator.setInitialState(shift_iSR);
773 SpacecraftState finalState_shift = propagator.propagate(target.toAbsoluteDate());
774 PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
775
776
777
778
779
780
781 FieldVector3D<Gradient> pos_DS = finPVC_G.getPosition();
782 double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
783 double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
784 double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
785
786
787 double x = finPVC_shift.getPosition().getX();
788 double y = finPVC_shift.getPosition().getY();
789 double z = finPVC_shift.getPosition().getZ();
790
791
792 maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal())));
793 maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal())));
794 maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal())));
795
796
797
798
799
800
801 FieldVector3D<Gradient> vel_G = finPVC_G.getVelocity();
802 double vx_G = vel_G.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
803 double vy_G = vel_G.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
804 double vz_G = vel_G.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
805
806
807 double vx = finPVC_shift.getVelocity().getX();
808 double vy = finPVC_shift.getVelocity().getY();
809 double vz = finPVC_shift.getVelocity().getZ();
810
811
812 maxV = FastMath.max(maxV, FastMath.abs((vx_G - vx) / vx));
813 maxV = FastMath.max(maxV, FastMath.abs((vy_G - vy) / vy));
814 maxV = FastMath.max(maxV, FastMath.abs((vz_G - vz) / vz));
815
816
817
818
819
820
821 FieldVector3D<Gradient> acc_G = finPVC_G.getAcceleration();
822 double ax_G = acc_G.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
823 double ay_G = acc_G.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
824 double az_G = acc_G.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
825
826
827 double ax = finPVC_shift.getAcceleration().getX();
828 double ay = finPVC_shift.getAcceleration().getY();
829 double az = finPVC_shift.getAcceleration().getZ();
830
831
832 maxA = FastMath.max(maxA, FastMath.abs((ax_G - ax) / ax));
833 maxA = FastMath.max(maxA, FastMath.abs((ay_G - ay) / ay));
834 maxA = FastMath.max(maxA, FastMath.abs((az_G - az) / az));
835 }
836
837
838 if (print) {
839 System.out.println("\n2 - Check P, V, A with Taylor expansion");
840 System.out.println("\tMax ΔP = " + maxP);
841 System.out.println("\tMax ΔV = " + maxV);
842 System.out.println("\tMax ΔA = " + maxA);
843 }
844 Assertions.assertEquals(0, maxP, positionRelativeToleranceTaylor);
845 Assertions.assertEquals(0, maxV, velocityRelativeToleranceTaylor);
846 Assertions.assertEquals(0, maxA, accelerationRelativeToleranceTaylor);
847 }
848
849 private double[] jacobianColumn(final NumericalPropagator propagator, final SpacecraftState state0,
850 final AbsoluteDate targetDate, final int index,
851 final double h)
852 {
853 return differential4(integrateShiftedState(propagator, state0, targetDate, index, -2 * h),
854 integrateShiftedState(propagator, state0, targetDate, index, -1 * h),
855 integrateShiftedState(propagator, state0, targetDate, index, +1 * h),
856 integrateShiftedState(propagator, state0, targetDate, index, +2 * h),
857 h);
858 }
859
860 private double[] integrateShiftedState(final NumericalPropagator propagator,
861 final SpacecraftState state0,
862 final AbsoluteDate targetDate,
863 final int index, final double h) {
864 OrbitType orbitType = propagator.getOrbitType();
865 PositionAngleType angleType = propagator.getPositionAngleType();
866 double[] a = new double[6];
867 double[] aDot = new double[6];
868 orbitType.mapOrbitToArray(state0.getOrbit(), angleType, a, aDot);
869 a[index] += h;
870 SpacecraftState shiftedState = new SpacecraftState(orbitType.mapArrayToOrbit(a, aDot, angleType, state0.getDate(),
871 state0.getOrbit().getMu(), state0.getFrame()),
872 state0.getAttitude()).withMass(state0.getMass());
873 propagator.setInitialState(shiftedState);
874 SpacecraftState integratedState = propagator.propagate(targetDate);
875 orbitType.mapOrbitToArray(integratedState.getOrbit(), angleType, a, null);
876 return a;
877 }
878
879 protected double[] differential8(final double[] fM4h, final double[] fM3h, final double[] fM2h, final double[] fM1h,
880 final double[] fP1h, final double[] fP2h, final double[] fP3h, final double[] fP4h,
881 final double h) {
882
883 double[] a = new double[fM4h.length];
884 for (int i = 0; i < a.length; ++i) {
885 a[i] = differential8(fM4h[i], fM3h[i], fM2h[i], fM1h[i], fP1h[i], fP2h[i], fP3h[i], fP4h[i], h);
886 }
887 return a;
888 }
889
890 protected double differential8(final double fM4h, final double fM3h, final double fM2h, final double fM1h,
891 final double fP1h, final double fP2h, final double fP3h, final double fP4h,
892 final double h) {
893
894
895
896 return (-3 * (fP4h - fM4h) + 32 * (fP3h - fM3h) - 168 * (fP2h - fM2h) + 672 * (fP1h - fM1h)) / (840 * h);
897
898 }
899
900 protected double[] differential4(final double[] fM2h, final double[] fM1h,
901 final double[] fP1h, final double[] fP2h,
902 final double h) {
903
904 double[] a = new double[fM2h.length];
905 for (int i = 0; i < a.length; ++i) {
906 a[i] = differential4(fM2h[i], fM1h[i], fP1h[i], fP2h[i], h);
907 }
908 return a;
909 }
910
911 protected double differential4(final double fM2h, final double fM1h,
912 final double fP1h, final double fP2h,
913 final double h) {
914
915
916
917 return (-1 * (fP2h - fM2h) + 8 * (fP1h - fM1h)) / (12 * h);
918
919 }
920
921 }
922
923