1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.numerical;
18
19 import java.io.IOException;
20 import java.text.ParseException;
21
22 import org.hipparchus.geometry.euclidean.threed.Rotation;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
26 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
27 import org.hipparchus.util.FastMath;
28 import org.junit.Assert;
29 import org.junit.Before;
30 import org.junit.Test;
31 import org.orekit.Utils;
32 import org.orekit.attitudes.Attitude;
33 import org.orekit.attitudes.AttitudeProvider;
34 import org.orekit.attitudes.InertialProvider;
35 import org.orekit.bodies.CelestialBodyFactory;
36 import org.orekit.bodies.OneAxisEllipsoid;
37 import org.orekit.forces.ForceModel;
38 import org.orekit.forces.drag.DragForce;
39 import org.orekit.forces.drag.DragSensitive;
40 import org.orekit.forces.drag.IsotropicDrag;
41 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
42 import org.orekit.forces.gravity.NewtonianAttraction;
43 import org.orekit.forces.gravity.potential.GravityFieldFactory;
44 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
45 import org.orekit.forces.gravity.potential.SHMFormatReader;
46 import org.orekit.forces.maneuvers.ConstantThrustManeuver;
47 import org.orekit.frames.Frame;
48 import org.orekit.frames.FramesFactory;
49 import org.orekit.models.earth.atmosphere.HarrisPriester;
50 import org.orekit.orbits.KeplerianOrbit;
51 import org.orekit.orbits.Orbit;
52 import org.orekit.orbits.OrbitType;
53 import org.orekit.orbits.PositionAngle;
54 import org.orekit.propagation.SpacecraftState;
55 import org.orekit.time.AbsoluteDate;
56 import org.orekit.time.DateComponents;
57 import org.orekit.time.TimeComponents;
58 import org.orekit.time.TimeScalesFactory;
59 import org.orekit.utils.Constants;
60 import org.orekit.utils.IERSConventions;
61 import org.orekit.utils.ParameterDriver;
62 import org.orekit.utils.ParameterDriversList;
63
64
65 public class IntegrableJacobianColumnGeneratorTest {
66
67 @Before
68 public void setUp() {
69 Utils.setDataRoot("regular-data:potential/shm-format");
70 GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
71 }
72
73 @Test
74 public void testDragParametersDerivatives() throws ParseException, IOException {
75 doTestParametersDerivatives(DragSensitive.DRAG_COEFFICIENT, 2.4e-3, OrbitType.values());
76 }
77
78 @Test
79 public void testMuParametersDerivatives() throws ParseException, IOException {
80
81
82
83 doTestParametersDerivatives(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT, 5e-7,
84 OrbitType.CARTESIAN);
85 }
86
87 private void doTestParametersDerivatives(String parameterName, double tolerance,
88 OrbitType... orbitTypes) {
89
90 OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
91 Constants.WGS84_EARTH_FLATTENING,
92 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
93 ForceModel drag = new DragForce(new HarrisPriester(CelestialBodyFactory.getSun(), earth),
94 new IsotropicDrag(2.5, 1.2));
95
96 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
97 ForceModel gravityField =
98 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
99 Orbit baseOrbit =
100 new KeplerianOrbit(7000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
101 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
102 provider.getMu());
103
104 double dt = 900;
105 double dP = 1.0;
106 for (OrbitType orbitType : orbitTypes) {
107 final Orbit initialOrbit = orbitType.convertType(baseOrbit);
108 for (PositionAngle angleType : PositionAngle.values()) {
109
110 NumericalPropagator propagator =
111 setUpPropagator(initialOrbit, dP, orbitType, angleType, drag, gravityField);
112 propagator.setMu(provider.getMu());
113 ParameterDriver selected = null;
114 for (final ForceModel forceModel : propagator.getAllForceModels()) {
115 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
116 driver.setValue(driver.getReferenceValue());
117 if (driver.getName().equals(parameterName)) {
118 driver.setSelected(true);
119 selected = driver;
120 } else {
121 driver.setSelected(false);
122 }
123 }
124 }
125
126 SpacecraftState initialState = new SpacecraftState(initialOrbit);
127 propagator.setInitialState(initialState);
128 PickUpHandler pickUp = new PickUpHandler(propagator, null, null, selected.getName());
129 propagator.setStepHandler(pickUp);
130 propagator.propagate(initialState.getDate().shiftedBy(dt));
131 RealMatrix dYdP = pickUp.getdYdP();
132
133
134 double[][] dYdPRef = new double[6][1];
135 NumericalPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType,
136 drag, gravityField);
137 propagator2.setMu(provider.getMu());
138 ParameterDriversList bound = new ParameterDriversList();
139 for (final ForceModel forceModel : propagator2.getAllForceModels()) {
140 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
141 if (driver.getName().equals(parameterName)) {
142 driver.setSelected(true);
143 bound.add(driver);
144 } else {
145 driver.setSelected(false);
146 }
147 }
148 }
149 ParameterDriver selected2 = bound.getDrivers().get(0);
150 double p0 = selected2.getReferenceValue();
151 double h = selected2.getScale();
152 selected2.setValue(p0 - 4 * h);
153 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
154 orbitType, angleType,
155 initialState.getFrame(), initialState.getDate(),
156 propagator2.getMu(),
157 initialState.getAttitude()));
158 SpacecraftState sM4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
159 selected2.setValue(p0 - 3 * h);
160 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
161 orbitType, angleType,
162 initialState.getFrame(), initialState.getDate(),
163 propagator2.getMu(),
164 initialState.getAttitude()));
165 SpacecraftState sM3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
166 selected2.setValue(p0 - 2 * h);
167 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
168 orbitType, angleType,
169 initialState.getFrame(), initialState.getDate(),
170 propagator2.getMu(),
171 initialState.getAttitude()));
172 SpacecraftState sM2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
173 selected2.setValue(p0 - 1 * h);
174 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
175 orbitType, angleType,
176 initialState.getFrame(), initialState.getDate(),
177 propagator2.getMu(),
178 initialState.getAttitude()));
179 SpacecraftState sM1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
180 selected2.setValue(p0 + 1 * h);
181 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
182 orbitType, angleType,
183 initialState.getFrame(), initialState.getDate(),
184 propagator2.getMu(),
185 initialState.getAttitude()));
186 SpacecraftState sP1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
187 selected2.setValue(p0 + 2 * h);
188 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
189 orbitType, angleType,
190 initialState.getFrame(), initialState.getDate(),
191 propagator2.getMu(),
192 initialState.getAttitude()));
193 SpacecraftState sP2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
194 selected2.setValue(p0 + 3 * h);
195 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
196 orbitType, angleType,
197 initialState.getFrame(), initialState.getDate(),
198 propagator2.getMu(),
199 initialState.getAttitude()));
200 SpacecraftState sP3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
201 selected2.setValue(p0 + 4 * h);
202 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
203 orbitType, angleType,
204 initialState.getFrame(), initialState.getDate(),
205 propagator2.getMu(),
206 initialState.getAttitude()));
207 SpacecraftState sP4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
208 fillJacobianColumn(dYdPRef, 0, orbitType, angleType, h,
209 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
210
211 for (int i = 0; i < 6; ++i) {
212 Assert.assertEquals(dYdPRef[i][0], dYdP.getEntry(i, 0), FastMath.abs(dYdPRef[i][0] * tolerance));
213 }
214
215 }
216 }
217
218 }
219
220 @Test
221 public void testJacobianIssue18() {
222
223
224 final double mu = 3.9860047e14;
225
226 final double isp = 318;
227 final double mass = 2500;
228 final double a = 24396159;
229 final double e = 0.72831215;
230 final double i = FastMath.toRadians(7);
231 final double omega = FastMath.toRadians(180);
232 final double OMEGA = FastMath.toRadians(261);
233 final double lv = 0;
234
235 final double duration = 3653.99;
236 final double f = 420;
237 final double delta = FastMath.toRadians(-7.4978);
238 final double alpha = FastMath.toRadians(351);
239 final AttitudeProvider law = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
240
241 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
242 new TimeComponents(23, 30, 00.000),
243 TimeScalesFactory.getUTC());
244 final Orbit orbit =
245 new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
246 FramesFactory.getEME2000(), initDate, mu);
247 SpacecraftState initialState =
248 new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
249
250 final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
251 new TimeComponents(04, 15, 34.080),
252 TimeScalesFactory.getUTC());
253 final ConstantThrustManeuver maneuver =
254 new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
255
256 double[] absTolerance = {
257 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
258 };
259 double[] relTolerance = {
260 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
261 };
262 AdaptiveStepsizeIntegrator integrator =
263 new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
264 integrator.setInitialStepSize(60);
265 final NumericalPropagator propagator = new NumericalPropagator(integrator);
266
267 propagator.setAttitudeProvider(law);
268 propagator.addForceModel(maneuver);
269 ParameterDriver selected = maneuver.getParameterDriver("thrust");
270 selected.setSelected(true);
271
272 final OrbitType orbitType = OrbitType.CARTESIAN;
273 final PositionAngle angleType = PositionAngle.TRUE;
274 propagator.setOrbitType(orbitType);
275 StateTransitionMatrixGenerator stmGenerator =
276 new StateTransitionMatrixGenerator("stm",
277 propagator.getAllForceModels(),
278 propagator.getAttitudeProvider());
279 IntegrableJacobianColumnGenerator columnGenerator = new IntegrableJacobianColumnGenerator(stmGenerator, selected.getName());
280 propagator.addAdditionalDerivativesProvider(columnGenerator);
281 propagator.addAdditionalDerivativesProvider(stmGenerator);
282
283 initialState = stmGenerator.setInitialStateTransitionMatrix(initialState, null, orbitType, angleType);
284 initialState = initialState.addAdditionalState(columnGenerator.getName(), new double[6]);
285 propagator.setInitialState(initialState);
286
287 final AbsoluteDate finalDate = fireDate.shiftedBy(3800);
288 final SpacecraftState finalorb = propagator.propagate(finalDate);
289 Assert.assertEquals(0, finalDate.durationFrom(finalorb.getDate()), 1.0e-11);
290
291 }
292
293 private void fillJacobianColumn(double[][] jacobian, int column,
294 OrbitType orbitType, PositionAngle angleType, double h,
295 SpacecraftState sM4h, SpacecraftState sM3h,
296 SpacecraftState sM2h, SpacecraftState sM1h,
297 SpacecraftState sP1h, SpacecraftState sP2h,
298 SpacecraftState sP3h, SpacecraftState sP4h) {
299 boolean withMass = jacobian.length > 6;
300 double[] aM4h = stateToArray(sM4h, orbitType, angleType, withMass)[0];
301 double[] aM3h = stateToArray(sM3h, orbitType, angleType, withMass)[0];
302 double[] aM2h = stateToArray(sM2h, orbitType, angleType, withMass)[0];
303 double[] aM1h = stateToArray(sM1h, orbitType, angleType, withMass)[0];
304 double[] aP1h = stateToArray(sP1h, orbitType, angleType, withMass)[0];
305 double[] aP2h = stateToArray(sP2h, orbitType, angleType, withMass)[0];
306 double[] aP3h = stateToArray(sP3h, orbitType, angleType, withMass)[0];
307 double[] aP4h = stateToArray(sP4h, orbitType, angleType, withMass)[0];
308 for (int i = 0; i < jacobian.length; ++i) {
309 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
310 32 * (aP3h[i] - aM3h[i]) -
311 168 * (aP2h[i] - aM2h[i]) +
312 672 * (aP1h[i] - aM1h[i])) / (840 * h);
313 }
314 }
315
316 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType, PositionAngle angleType,
317 boolean withMass) {
318 double[][] array = new double[2][withMass ? 7 : 6];
319 orbitType.mapOrbitToArray(state.getOrbit(), angleType, array[0], array[1]);
320 if (withMass) {
321 array[0][6] = state.getMass();
322 }
323 return array;
324 }
325
326 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType, PositionAngle angleType,
327 Frame frame, AbsoluteDate date, double mu,
328 Attitude attitude) {
329 Orbit orbit = orbitType.mapArrayToOrbit(array[0], array[1], angleType, date, mu, frame);
330 return (array.length > 6) ?
331 new SpacecraftState(orbit, attitude) :
332 new SpacecraftState(orbit, attitude, array[0][6]);
333 }
334
335 private NumericalPropagator setUpPropagator(Orbit orbit, double dP,
336 OrbitType orbitType, PositionAngle angleType,
337 ForceModel... models)
338 {
339
340 final double minStep = 0.001;
341 final double maxStep = 1000;
342
343 double[][] tol = NumericalPropagator.tolerances(dP, orbit, orbitType);
344 NumericalPropagator propagator =
345 new NumericalPropagator(new DormandPrince853Integrator(minStep, maxStep, tol[0], tol[1]));
346 propagator.setOrbitType(orbitType);
347 propagator.setPositionAngleType(angleType);
348 for (ForceModel model : models) {
349 propagator.addForceModel(model);
350 }
351 return propagator;
352 }
353
354 }