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.apache.commons.math3.analysis.differentiation.DerivativeStructure;
21 import org.apache.commons.math3.geometry.euclidean.threed.FieldVector3D;
22 import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
23 import org.apache.commons.math3.ode.UnknownParameterException;
24 import org.junit.Assert;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.errors.OrekitMessages;
27 import org.orekit.errors.PropagationException;
28 import org.orekit.frames.Frame;
29 import org.orekit.orbits.OrbitType;
30 import org.orekit.orbits.PositionAngle;
31 import org.orekit.propagation.SpacecraftState;
32 import org.orekit.propagation.numerical.JacobiansMapper;
33 import org.orekit.propagation.numerical.NumericalPropagator;
34 import org.orekit.propagation.numerical.PartialDerivativesEquations;
35 import org.orekit.propagation.numerical.TimeDerivativesEquations;
36 import org.orekit.propagation.sampling.OrekitStepHandler;
37 import org.orekit.propagation.sampling.OrekitStepInterpolator;
38 import org.orekit.time.AbsoluteDate;
39
40
41 public abstract class AbstractForceModelTest {
42
43 protected static class AccelerationRetriever implements TimeDerivativesEquations {
44
45 private Vector3D acceleration;
46
47 public AccelerationRetriever() {
48 acceleration = Vector3D.ZERO;
49 }
50
51 public void addKeplerContribution(double mu) {
52 }
53
54 public void addXYZAcceleration(double x, double y, double z) {
55 acceleration = new Vector3D(x, y, z);
56 }
57
58 public void addAcceleration(Vector3D gamma, Frame frame) {
59 acceleration = gamma;
60 }
61
62 public void addMassDerivative(double q) {
63 }
64
65 public Vector3D getAcceleration() {
66 return acceleration;
67 }
68
69 }
70
71 protected void checkParameterDerivative(SpacecraftState state,
72 ForceModel forceModel, String name,
73 double hFactor, double tol)
74 throws OrekitException {
75
76 try {
77 forceModel.accelerationDerivatives(state, "not a parameter");
78 Assert.fail("an exception should have been thrown");
79 } catch (UnknownParameterException upe) {
80
81 } catch (OrekitException oe) {
82
83 Assert.assertEquals(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, oe.getSpecifier());
84 }
85 FieldVector3D<DerivativeStructure> accDer = forceModel.accelerationDerivatives(state, name);
86 Vector3D derivative = new Vector3D(accDer.getX().getPartialDerivative(1),
87 accDer.getY().getPartialDerivative(1),
88 accDer.getZ().getPartialDerivative(1));
89
90 AccelerationRetriever accelerationRetriever = new AccelerationRetriever();
91 double p0 = forceModel.getParameter(name);
92 double hParam = hFactor * forceModel.getParameter(name);
93 forceModel.setParameter(name, p0 - 1 * hParam);
94 Assert.assertEquals(p0 - 1 * hParam, forceModel.getParameter(name), 1.0e-10);
95 forceModel.addContribution(state, accelerationRetriever);
96 final Vector3D gammaM1h = accelerationRetriever.getAcceleration();
97 forceModel.setParameter(name, p0 + 1 * hParam);
98 Assert.assertEquals(p0 + 1 * hParam, forceModel.getParameter(name), 1.0e-10);
99 forceModel.addContribution(state, accelerationRetriever);
100 final Vector3D gammaP1h = accelerationRetriever.getAcceleration();
101
102 final Vector3D reference = new Vector3D( 1 / (2 * hParam), gammaP1h.subtract(gammaM1h));
103 final Vector3D delta = derivative.subtract(reference);
104 Assert.assertEquals(0, delta.getNorm(), tol * reference.getNorm());
105
106 }
107
108 protected void checkStateJacobian(NumericalPropagator propagator, SpacecraftState state0,
109 AbsoluteDate targetDate, double hFactor,
110 double[] integratorAbsoluteTolerances, double checkTolerance)
111 throws OrekitException {
112
113 propagator.setInitialState(state0);
114 double[][] reference = new double[][] {
115 jacobianColumn(propagator, state0, targetDate, 0, hFactor * integratorAbsoluteTolerances[0]),
116 jacobianColumn(propagator, state0, targetDate, 1, hFactor * integratorAbsoluteTolerances[1]),
117 jacobianColumn(propagator, state0, targetDate, 2, hFactor * integratorAbsoluteTolerances[2]),
118 jacobianColumn(propagator, state0, targetDate, 3, hFactor * integratorAbsoluteTolerances[3]),
119 jacobianColumn(propagator, state0, targetDate, 4, hFactor * integratorAbsoluteTolerances[4]),
120 jacobianColumn(propagator, state0, targetDate, 5, hFactor * integratorAbsoluteTolerances[5])
121 };
122 for (int j = 0; j < 6; ++j) {
123 for (int k = j + 1; k < 6; ++k) {
124 double tmp = reference[j][k];
125 reference[j][k] = reference[k][j];
126 reference[k][j] = tmp;
127 }
128 }
129
130 final String name = "pde";
131 PartialDerivativesEquations pde = new PartialDerivativesEquations(name, propagator);
132 propagator.setInitialState(pde.setInitialJacobians(state0, 6, 0));
133 final JacobiansMapper mapper = pde.getMapper();
134 final double[][] dYdY0 = new double[6][6];
135 propagator.setMasterMode(new OrekitStepHandler() {
136
137 public void init(SpacecraftState s0, AbsoluteDate t) {
138 }
139
140 public void handleStep(OrekitStepInterpolator interpolator, boolean isLast)
141 throws PropagationException {
142 if (isLast) {
143 try {
144
145 interpolator.setInterpolatedDate(interpolator.getCurrentDate());
146 mapper.getStateJacobian(interpolator.getInterpolatedState(), dYdY0);
147 } catch (OrekitException oe) {
148 throw new PropagationException(oe);
149 }
150 }
151 }
152
153 });
154 propagator.propagate(targetDate);
155
156 for (int j = 0; j < 6; ++j) {
157 for (int k = 0; k < 6; ++k) {
158 double scale = integratorAbsoluteTolerances[j] / integratorAbsoluteTolerances[k];
159 Assert.assertEquals(reference[j][k], dYdY0[j][k], checkTolerance * scale);
160 }
161 }
162
163 }
164
165 private double[] jacobianColumn(final NumericalPropagator propagator, final SpacecraftState state0,
166 final AbsoluteDate targetDate, final int index,
167 final double h)
168 throws PropagationException {
169 return differential4(integrateShiftedState(propagator, state0, targetDate, index, -2 * h),
170 integrateShiftedState(propagator, state0, targetDate, index, -1 * h),
171 integrateShiftedState(propagator, state0, targetDate, index, +1 * h),
172 integrateShiftedState(propagator, state0, targetDate, index, +2 * h),
173 h);
174 }
175
176 private double[] integrateShiftedState(final NumericalPropagator propagator,
177 final SpacecraftState state0,
178 final AbsoluteDate targetDate,
179 final int index, final double h)
180 throws PropagationException {
181 OrbitType orbitType = propagator.getOrbitType();
182 PositionAngle angleType = propagator.getPositionAngleType();
183 double[] a = new double[6];
184 orbitType.mapOrbitToArray(state0.getOrbit(), angleType, a);
185 a[index] += h;
186 SpacecraftState shiftedState = new SpacecraftState(orbitType.mapArrayToOrbit(a, angleType, state0.getDate(),
187 state0.getMu(), state0.getFrame()),
188 state0.getAttitude(),
189 state0.getMass());
190 propagator.setInitialState(shiftedState);
191 SpacecraftState integratedState = propagator.propagate(targetDate);
192 orbitType.mapOrbitToArray(integratedState.getOrbit(), angleType, a);
193 return a;
194 }
195
196 protected double[] differential8(final double[] fM4h, final double[] fM3h, final double[] fM2h, final double[] fM1h,
197 final double[] fP1h, final double[] fP2h, final double[] fP3h, final double[] fP4h,
198 final double h) {
199
200 double[] a = new double[fM4h.length];
201 for (int i = 0; i < a.length; ++i) {
202 a[i] = differential8(fM4h[i], fM3h[i], fM2h[i], fM1h[i], fP1h[i], fP2h[i], fP3h[i], fP4h[i], h);
203 }
204 return a;
205 }
206
207 protected double differential8(final double fM4h, final double fM3h, final double fM2h, final double fM1h,
208 final double fP1h, final double fP2h, final double fP3h, final double fP4h,
209 final double h) {
210
211
212
213 return (-3 * (fP4h - fM4h) + 32 * (fP3h - fM3h) - 168 * (fP2h - fM2h) + 672 * (fP1h - fM1h)) / (840 * h);
214
215 }
216
217 protected double[] differential4(final double[] fM2h, final double[] fM1h,
218 final double[] fP1h, final double[] fP2h,
219 final double h) {
220
221 double[] a = new double[fM2h.length];
222 for (int i = 0; i < a.length; ++i) {
223 a[i] = differential4(fM2h[i], fM1h[i], fP1h[i], fP2h[i], h);
224 }
225 return a;
226 }
227
228 protected double differential4(final double fM2h, final double fM1h,
229 final double fP1h, final double fP2h,
230 final double h) {
231
232
233
234 return (-1 * (fP2h - fM2h) + 8 * (fP1h - fM1h)) / (12 * h);
235
236 }
237
238 }
239
240