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