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