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