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.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.util.Precision;
27 import org.junit.Assert;
28 import org.orekit.attitudes.AttitudeProvider;
29 import org.orekit.attitudes.FieldAttitude;
30 import org.orekit.orbits.FieldCartesianOrbit;
31 import org.orekit.orbits.OrbitType;
32 import org.orekit.orbits.PositionAngle;
33 import org.orekit.propagation.FieldSpacecraftState;
34 import org.orekit.propagation.SpacecraftState;
35 import org.orekit.propagation.numerical.JacobiansMapper;
36 import org.orekit.propagation.numerical.NumericalPropagator;
37 import org.orekit.propagation.numerical.PartialDerivativesEquations;
38 import org.orekit.propagation.sampling.OrekitStepHandler;
39 import org.orekit.propagation.sampling.OrekitStepInterpolator;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.time.FieldAbsoluteDate;
42 import org.orekit.utils.Differentiation;
43 import org.orekit.utils.ParameterDriver;
44 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
45 import org.orekit.utils.TimeStampedFieldPVCoordinates;
46
47
48 public abstract class AbstractForceModelTest {
49
50 protected void checkParameterDerivative(SpacecraftState state,
51 ForceModel forceModel, String name,
52 double hFactor, double tol)
53 {
54
55 final DSFactory factory11 = new DSFactory(1, 1);
56 final Field<DerivativeStructure> field = factory11.getDerivativeField();
57 final FieldSpacecraftState<DerivativeStructure> stateF = new FieldSpacecraftState<DerivativeStructure>(field, state);
58 final ParameterDriver[] drivers = forceModel.getParametersDrivers();
59 final DerivativeStructure[] parametersDS = new DerivativeStructure[drivers.length];
60 for (int i = 0; i < parametersDS.length; ++i) {
61 if (drivers[i].getName().equals(name)) {
62 parametersDS[i] = factory11.variable(0, drivers[i].getValue());
63 } else {
64 parametersDS[i] = factory11.constant(drivers[i].getValue());
65 }
66 }
67 FieldVector3D<DerivativeStructure> accDer = forceModel.acceleration(stateF, parametersDS);
68 Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1),
69 accDer.getY().getPartialDerivative(1),
70 accDer.getZ().getPartialDerivative(1));
71
72 int selected = -1;
73 final double[] parameters = new double[drivers.length];
74 for (int i = 0; i < drivers.length; ++i) {
75 parameters[i] = drivers[i].getValue();
76 if (drivers[i].getName().equals(name)) {
77 selected = i;
78 }
79 }
80 double p0 = parameters[selected];
81 double hParam = hFactor * p0;
82 drivers[selected].setValue(p0 - 1 * hParam);
83 parameters[selected] = drivers[selected].getValue();
84 Assert.assertEquals(p0 - 1 * hParam, parameters[selected], 1.0e-10);
85 final Vector3D gammaM1h = forceModel.acceleration(state, parameters);
86 drivers[selected].setValue(p0 + 1 * hParam);
87 parameters[selected] = drivers[selected].getValue();
88 Assert.assertEquals(p0 + 1 * hParam, parameters[selected], 1.0e-10);
89 final Vector3D gammaP1h = forceModel.acceleration(state, parameters);
90 drivers[selected].setValue(p0);
91
92 final Vector3D reference = new Vector3D( 1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
93 final Vector3D delta = derivative.subtract(reference);
94 Assert.assertEquals(0, delta.getNorm(), tol * reference.getNorm());
95
96 }
97
98 protected FieldSpacecraftState<DerivativeStructure> toDS(final SpacecraftState state,
99 final AttitudeProvider attitudeProvider)
100 {
101
102 final Vector3D p = state.getPVCoordinates().getPosition();
103 final Vector3D v = state.getPVCoordinates().getVelocity();
104 final Vector3D a = state.getPVCoordinates().getAcceleration();
105 DSFactory factory = new DSFactory(6, 1);
106 Field<DerivativeStructure> field = factory.getDerivativeField();
107 final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state.getDate());
108 final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA =
109 new TimeStampedFieldPVCoordinates<>(fDate,
110 new FieldVector3D<>(factory.variable(0, p.getX()),
111 factory.variable(1, p.getY()),
112 factory.variable(2, p.getZ())),
113 new FieldVector3D<>(factory.variable(3, v.getX()),
114 factory.variable(4, v.getY()),
115 factory.variable(5, v.getZ())),
116 new FieldVector3D<>(factory.constant(a.getX()),
117 factory.constant(a.getY()),
118 factory.constant(a.getZ())));
119 final FieldCartesianOrbit<DerivativeStructure> orbit =
120 new FieldCartesianOrbit<>(fPVA, state.getFrame(), field.getZero().add(state.getMu()));
121 final FieldAttitude<DerivativeStructure> attitude =
122 attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame());
123
124 return new FieldSpacecraftState<>(orbit, attitude, field.getZero().add(state.getMass()));
125
126 }
127
128 protected void checkStateJacobianVsFiniteDifferences(final SpacecraftState state0, final ForceModel forceModel,
129 final AttitudeProvider provider, final double dP,
130 final double checkTolerance, final boolean print)
131 {
132
133 double[][] finiteDifferencesJacobian =
134 Differentiation.differentiate(state -> forceModel.acceleration(state, forceModel.getParameters()).toArray(),
135 3, provider, OrbitType.CARTESIAN, PositionAngle.MEAN,
136 dP, 5).
137 value(state0);
138
139 DSFactory factory = new DSFactory(6, 1);
140 Field<DerivativeStructure> field = factory.getDerivativeField();
141 final FieldAbsoluteDate<DerivativeStructure> fDate = new FieldAbsoluteDate<>(field, state0.getDate());
142 final Vector3D p = state0.getPVCoordinates().getPosition();
143 final Vector3D v = state0.getPVCoordinates().getVelocity();
144 final Vector3D a = state0.getPVCoordinates().getAcceleration();
145 final TimeStampedFieldPVCoordinates<DerivativeStructure> fPVA =
146 new TimeStampedFieldPVCoordinates<>(fDate,
147 new FieldVector3D<>(factory.variable(0, p.getX()),
148 factory.variable(1, p.getY()),
149 factory.variable(2, p.getZ())),
150 new FieldVector3D<>(factory.variable(3, v.getX()),
151 factory.variable(4, v.getY()),
152 factory.variable(5, v.getZ())),
153 new FieldVector3D<>(factory.constant(a.getX()),
154 factory.constant(a.getY()),
155 factory.constant(a.getZ())));
156 final TimeStampedFieldAngularCoordinates<DerivativeStructure> fAC =
157 new TimeStampedFieldAngularCoordinates<>(fDate,
158 new FieldRotation<>(field, state0.getAttitude().getRotation()),
159 new FieldVector3D<>(field, state0.getAttitude().getSpin()),
160 new FieldVector3D<>(field, state0.getAttitude().getRotationAcceleration()));
161 final FieldSpacecraftState<DerivativeStructure> fState =
162 new FieldSpacecraftState<>(new FieldCartesianOrbit<>(fPVA, state0.getFrame(), field.getZero().add(state0.getMu())),
163 new FieldAttitude<>(state0.getFrame(), fAC),
164 field.getZero().add(state0.getMass()));
165 FieldVector3D<DerivativeStructure> dsJacobian = forceModel.acceleration(fState,
166 forceModel.getParameters(fState.getDate().getField()));
167
168 Vector3D dFdPXRef = new Vector3D(finiteDifferencesJacobian[0][0],
169 finiteDifferencesJacobian[1][0],
170 finiteDifferencesJacobian[2][0]);
171 Vector3D dFdPXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(1, 0, 0, 0, 0, 0),
172 dsJacobian.getY().getPartialDerivative(1, 0, 0, 0, 0, 0),
173 dsJacobian.getZ().getPartialDerivative(1, 0, 0, 0, 0, 0));
174 Vector3D dFdPYRef = new Vector3D(finiteDifferencesJacobian[0][1],
175 finiteDifferencesJacobian[1][1],
176 finiteDifferencesJacobian[2][1]);
177 Vector3D dFdPYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 1, 0, 0, 0, 0),
178 dsJacobian.getY().getPartialDerivative(0, 1, 0, 0, 0, 0),
179 dsJacobian.getZ().getPartialDerivative(0, 1, 0, 0, 0, 0));
180 Vector3D dFdPZRef = new Vector3D(finiteDifferencesJacobian[0][2],
181 finiteDifferencesJacobian[1][2],
182 finiteDifferencesJacobian[2][2]);
183 Vector3D dFdPZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 1, 0, 0, 0),
184 dsJacobian.getY().getPartialDerivative(0, 0, 1, 0, 0, 0),
185 dsJacobian.getZ().getPartialDerivative(0, 0, 1, 0, 0, 0));
186 Vector3D dFdVXRef = new Vector3D(finiteDifferencesJacobian[0][3],
187 finiteDifferencesJacobian[1][3],
188 finiteDifferencesJacobian[2][3]);
189 Vector3D dFdVXRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 1, 0, 0),
190 dsJacobian.getY().getPartialDerivative(0, 0, 0, 1, 0, 0),
191 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 1, 0, 0));
192 Vector3D dFdVYRef = new Vector3D(finiteDifferencesJacobian[0][4],
193 finiteDifferencesJacobian[1][4],
194 finiteDifferencesJacobian[2][4]);
195 Vector3D dFdVYRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 1, 0),
196 dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 1, 0),
197 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 1, 0));
198 Vector3D dFdVZRef = new Vector3D(finiteDifferencesJacobian[0][5],
199 finiteDifferencesJacobian[1][5],
200 finiteDifferencesJacobian[2][5]);
201 Vector3D dFdVZRes = new Vector3D(dsJacobian.getX().getPartialDerivative(0, 0, 0, 0, 0, 1),
202 dsJacobian.getY().getPartialDerivative(0, 0, 0, 0, 0, 1),
203 dsJacobian.getZ().getPartialDerivative(0, 0, 0, 0, 0, 1));
204 if (print) {
205 System.out.println("dF/dPX ref: " + dFdPXRef.getX() + " " + dFdPXRef.getY() + " " + dFdPXRef.getZ());
206 System.out.println("dF/dPX res: " + dFdPXRes.getX() + " " + dFdPXRes.getY() + " " + dFdPXRes.getZ());
207 System.out.println("dF/dPY ref: " + dFdPYRef.getX() + " " + dFdPYRef.getY() + " " + dFdPYRef.getZ());
208 System.out.println("dF/dPY res: " + dFdPYRes.getX() + " " + dFdPYRes.getY() + " " + dFdPYRes.getZ());
209 System.out.println("dF/dPZ ref: " + dFdPZRef.getX() + " " + dFdPZRef.getY() + " " + dFdPZRef.getZ());
210 System.out.println("dF/dPZ res: " + dFdPZRes.getX() + " " + dFdPZRes.getY() + " " + dFdPZRes.getZ());
211 System.out.println("dF/dPX ref norm: " + dFdPXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPXRef, dFdPXRes) + ", rel error: " + (Vector3D.distance(dFdPXRef, dFdPXRes) / dFdPXRef.getNorm()));
212 System.out.println("dF/dPY ref norm: " + dFdPYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPYRef, dFdPYRes) + ", rel error: " + (Vector3D.distance(dFdPYRef, dFdPYRes) / dFdPYRef.getNorm()));
213 System.out.println("dF/dPZ ref norm: " + dFdPZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdPZRef, dFdPZRes) + ", rel error: " + (Vector3D.distance(dFdPZRef, dFdPZRes) / dFdPZRef.getNorm()));
214 System.out.println("dF/dVX ref norm: " + dFdVXRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVXRef, dFdVXRes) + ", rel error: " + (Vector3D.distance(dFdVXRef, dFdVXRes) / dFdVXRef.getNorm()));
215 System.out.println("dF/dVY ref norm: " + dFdVYRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVYRef, dFdVYRes) + ", rel error: " + (Vector3D.distance(dFdVYRef, dFdVYRes) / dFdVYRef.getNorm()));
216 System.out.println("dF/dVZ ref norm: " + dFdVZRef.getNorm() + ", abs error: " + Vector3D.distance(dFdVZRef, dFdVZRes) + ", rel error: " + (Vector3D.distance(dFdVZRef, dFdVZRes) / dFdVZRef.getNorm()));
217 }
218 checkdFdP(dFdPXRef, dFdPXRes, checkTolerance);
219 checkdFdP(dFdPYRef, dFdPYRes, checkTolerance);
220 checkdFdP(dFdPZRef, dFdPZRes, checkTolerance);
221 checkdFdP(dFdVXRef, dFdVXRes, checkTolerance);
222 checkdFdP(dFdVYRef, dFdVYRes, checkTolerance);
223 checkdFdP(dFdVZRef, dFdVZRes, checkTolerance);
224 }
225
226 private void checkdFdP(final Vector3D reference, final Vector3D result, final double checkTolerance) {
227 if (reference.getNorm() == 0) {
228
229
230 Assert.assertEquals(0, result.getNorm(), Precision.SAFE_MIN);
231 } else {
232 Assert.assertEquals(0, Vector3D.distance(reference, result), checkTolerance * reference.getNorm());
233 }
234 }
235
236 protected void checkStateJacobian(NumericalPropagator propagator, SpacecraftState state0,
237 AbsoluteDate targetDate, double hFactor,
238 double[] integratorAbsoluteTolerances, double checkTolerance)
239 {
240
241 propagator.setInitialState(state0);
242 double[][] reference = new double[][] {
243 jacobianColumn(propagator, state0, targetDate, 0, hFactor * integratorAbsoluteTolerances[0]),
244 jacobianColumn(propagator, state0, targetDate, 1, hFactor * integratorAbsoluteTolerances[1]),
245 jacobianColumn(propagator, state0, targetDate, 2, hFactor * integratorAbsoluteTolerances[2]),
246 jacobianColumn(propagator, state0, targetDate, 3, hFactor * integratorAbsoluteTolerances[3]),
247 jacobianColumn(propagator, state0, targetDate, 4, hFactor * integratorAbsoluteTolerances[4]),
248 jacobianColumn(propagator, state0, targetDate, 5, hFactor * integratorAbsoluteTolerances[5])
249 };
250 for (int j = 0; j < 6; ++j) {
251 for (int k = j + 1; k < 6; ++k) {
252 double tmp = reference[j][k];
253 reference[j][k] = reference[k][j];
254 reference[k][j] = tmp;
255 }
256 }
257
258 final String name = "pde";
259 PartialDerivativesEquations pde = new PartialDerivativesEquations(name, propagator);
260 propagator.setInitialState(pde.setInitialJacobians(state0));
261 final JacobiansMapper mapper = pde.getMapper();
262 final double[][] dYdY0 = new double[6][6];
263 propagator.setMasterMode(new OrekitStepHandler() {
264
265 public void handleStep(OrekitStepInterpolator interpolator, boolean isLast)
266 {
267 if (isLast) {
268
269 mapper.getStateJacobian(interpolator.getCurrentState(), dYdY0);
270 }
271 }
272
273 });
274 propagator.propagate(targetDate);
275
276 for (int j = 0; j < 6; ++j) {
277 for (int k = 0; k < 6; ++k) {
278 double scale = integratorAbsoluteTolerances[j] / integratorAbsoluteTolerances[k];
279 Assert.assertEquals(reference[j][k], dYdY0[j][k], checkTolerance * scale);
280 }
281 }
282
283 }
284
285 private double[] jacobianColumn(final NumericalPropagator propagator, final SpacecraftState state0,
286 final AbsoluteDate targetDate, final int index,
287 final double h)
288 {
289 return differential4(integrateShiftedState(propagator, state0, targetDate, index, -2 * h),
290 integrateShiftedState(propagator, state0, targetDate, index, -1 * h),
291 integrateShiftedState(propagator, state0, targetDate, index, +1 * h),
292 integrateShiftedState(propagator, state0, targetDate, index, +2 * h),
293 h);
294 }
295
296 private double[] integrateShiftedState(final NumericalPropagator propagator,
297 final SpacecraftState state0,
298 final AbsoluteDate targetDate,
299 final int index, final double h)
300 {
301 OrbitType orbitType = propagator.getOrbitType();
302 PositionAngle angleType = propagator.getPositionAngleType();
303 double[] a = new double[6];
304 double[] aDot = new double[6];
305 orbitType.mapOrbitToArray(state0.getOrbit(), angleType, a, aDot);
306 a[index] += h;
307 SpacecraftState shiftedState = new SpacecraftState(orbitType.mapArrayToOrbit(a, aDot, angleType, state0.getDate(),
308 state0.getMu(), state0.getFrame()),
309 state0.getAttitude(),
310 state0.getMass());
311 propagator.setInitialState(shiftedState);
312 SpacecraftState integratedState = propagator.propagate(targetDate);
313 orbitType.mapOrbitToArray(integratedState.getOrbit(), angleType, a, null);
314 return a;
315 }
316
317 protected double[] differential8(final double[] fM4h, final double[] fM3h, final double[] fM2h, final double[] fM1h,
318 final double[] fP1h, final double[] fP2h, final double[] fP3h, final double[] fP4h,
319 final double h) {
320
321 double[] a = new double[fM4h.length];
322 for (int i = 0; i < a.length; ++i) {
323 a[i] = differential8(fM4h[i], fM3h[i], fM2h[i], fM1h[i], fP1h[i], fP2h[i], fP3h[i], fP4h[i], h);
324 }
325 return a;
326 }
327
328 protected double differential8(final double fM4h, final double fM3h, final double fM2h, final double fM1h,
329 final double fP1h, final double fP2h, final double fP3h, final double fP4h,
330 final double h) {
331
332
333
334 return (-3 * (fP4h - fM4h) + 32 * (fP3h - fM3h) - 168 * (fP2h - fM2h) + 672 * (fP1h - fM1h)) / (840 * h);
335
336 }
337
338 protected double[] differential4(final double[] fM2h, final double[] fM1h,
339 final double[] fP1h, final double[] fP2h,
340 final double h) {
341
342 double[] a = new double[fM2h.length];
343 for (int i = 0; i < a.length; ++i) {
344 a[i] = differential4(fM2h[i], fM1h[i], fP1h[i], fP2h[i], h);
345 }
346 return a;
347 }
348
349 protected double differential4(final double fM2h, final double fM1h,
350 final double fP1h, final double fP2h,
351 final double h) {
352
353
354
355 return (-1 * (fP2h - fM2h) + 8 * (fP1h - fM1h)) / (12 * h);
356
357 }
358
359 }
360
361