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.ode.nonstiff.AdaptiveStepsizeIntegrator;
25 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
26 import org.hipparchus.util.FastMath;
27 import org.junit.Assert;
28 import org.junit.Before;
29 import org.junit.Test;
30 import org.orekit.Utils;
31 import org.orekit.attitudes.Attitude;
32 import org.orekit.attitudes.AttitudeProvider;
33 import org.orekit.attitudes.InertialProvider;
34 import org.orekit.bodies.CelestialBodyFactory;
35 import org.orekit.bodies.OneAxisEllipsoid;
36 import org.orekit.errors.OrekitException;
37 import org.orekit.errors.OrekitMessages;
38 import org.orekit.forces.ForceModel;
39 import org.orekit.forces.drag.DragForce;
40 import org.orekit.forces.drag.DragSensitive;
41 import org.orekit.forces.drag.IsotropicDrag;
42 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
43 import org.orekit.forces.gravity.NewtonianAttraction;
44 import org.orekit.forces.gravity.ThirdBodyAttraction;
45 import org.orekit.forces.gravity.potential.GravityFieldFactory;
46 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
47 import org.orekit.forces.gravity.potential.SHMFormatReader;
48 import org.orekit.forces.maneuvers.ConstantThrustManeuver;
49 import org.orekit.frames.Frame;
50 import org.orekit.frames.FramesFactory;
51 import org.orekit.models.earth.atmosphere.HarrisPriester;
52 import org.orekit.orbits.KeplerianOrbit;
53 import org.orekit.orbits.Orbit;
54 import org.orekit.orbits.OrbitType;
55 import org.orekit.orbits.PositionAngle;
56 import org.orekit.propagation.SpacecraftState;
57 import org.orekit.propagation.integration.AbstractIntegratedPropagator;
58 import org.orekit.propagation.sampling.OrekitStepHandler;
59 import org.orekit.propagation.sampling.OrekitStepInterpolator;
60 import org.orekit.time.AbsoluteDate;
61 import org.orekit.time.DateComponents;
62 import org.orekit.time.TimeComponents;
63 import org.orekit.time.TimeScalesFactory;
64 import org.orekit.utils.Constants;
65 import org.orekit.utils.IERSConventions;
66 import org.orekit.utils.PVCoordinates;
67 import org.orekit.utils.ParameterDriver;
68 import org.orekit.utils.ParameterDriversList;
69
70 @Deprecated
71 public class PartialDerivativesTest {
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 for (final ForceModel forceModel : propagator.getAllForceModels()) {
114 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
115 driver.setValue(driver.getReferenceValue());
116 driver.setSelected(driver.getName().equals(parameterName));
117 }
118 }
119
120 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
121 final SpacecraftState initialState =
122 partials.setInitialJacobians(new SpacecraftState(initialOrbit));
123 propagator.setInitialState(initialState);
124 final JacobiansMapper mapper = partials.getMapper();
125 PickUpHandler pickUp = new PickUpHandler(mapper, null);
126 propagator.setStepHandler(pickUp);
127 propagator.propagate(initialState.getDate().shiftedBy(dt));
128 double[][] dYdP = pickUp.getdYdP();
129
130
131 double[][] dYdPRef = new double[6][1];
132 NumericalPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType,
133 drag, gravityField);
134 propagator2.setMu(provider.getMu());
135 ParameterDriversList bound = new ParameterDriversList();
136 for (final ForceModel forceModel : propagator2.getAllForceModels()) {
137 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
138 if (driver.getName().equals(parameterName)) {
139 driver.setSelected(true);
140 bound.add(driver);
141 } else {
142 driver.setSelected(false);
143 }
144 }
145 }
146 ParameterDriver selected = bound.getDrivers().get(0);
147 double p0 = selected.getReferenceValue();
148 double h = selected.getScale();
149 selected.setValue(p0 - 4 * h);
150 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
151 orbitType, angleType,
152 initialState.getFrame(), initialState.getDate(),
153 propagator2.getMu(),
154 initialState.getAttitude()));
155 SpacecraftState sM4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
156 selected.setValue(p0 - 3 * h);
157 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
158 orbitType, angleType,
159 initialState.getFrame(), initialState.getDate(),
160 propagator2.getMu(),
161 initialState.getAttitude()));
162 SpacecraftState sM3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
163 selected.setValue(p0 - 2 * h);
164 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
165 orbitType, angleType,
166 initialState.getFrame(), initialState.getDate(),
167 propagator2.getMu(),
168 initialState.getAttitude()));
169 SpacecraftState sM2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
170 selected.setValue(p0 - 1 * h);
171 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
172 orbitType, angleType,
173 initialState.getFrame(), initialState.getDate(),
174 propagator2.getMu(),
175 initialState.getAttitude()));
176 SpacecraftState sM1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
177 selected.setValue(p0 + 1 * h);
178 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
179 orbitType, angleType,
180 initialState.getFrame(), initialState.getDate(),
181 propagator2.getMu(),
182 initialState.getAttitude()));
183 SpacecraftState sP1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
184 selected.setValue(p0 + 2 * h);
185 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
186 orbitType, angleType,
187 initialState.getFrame(), initialState.getDate(),
188 propagator2.getMu(),
189 initialState.getAttitude()));
190 SpacecraftState sP2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
191 selected.setValue(p0 + 3 * h);
192 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
193 orbitType, angleType,
194 initialState.getFrame(), initialState.getDate(),
195 propagator2.getMu(),
196 initialState.getAttitude()));
197 SpacecraftState sP3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
198 selected.setValue(p0 + 4 * h);
199 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
200 orbitType, angleType,
201 initialState.getFrame(), initialState.getDate(),
202 propagator2.getMu(),
203 initialState.getAttitude()));
204 SpacecraftState sP4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
205 fillJacobianColumn(dYdPRef, 0, orbitType, angleType, h,
206 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
207
208 for (int i = 0; i < 6; ++i) {
209 Assert.assertEquals(dYdPRef[i][0], dYdP[i][0], FastMath.abs(dYdPRef[i][0] * tolerance));
210 }
211
212 }
213 }
214
215 }
216
217 @Test
218 public void testPropagationTypesElliptical() {
219
220 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
221 ForceModel gravityField =
222 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
223 Orbit initialOrbit =
224 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
225 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
226 provider.getMu());
227
228 double dt = 900;
229 double dP = 0.001;
230 for (OrbitType orbitType : OrbitType.values()) {
231 for (PositionAngle angleType : PositionAngle.values()) {
232
233
234 NumericalPropagator propagator =
235 setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
236 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
237 final SpacecraftState initialState =
238 partials.setInitialJacobians(new SpacecraftState(initialOrbit));
239 propagator.setInitialState(initialState);
240 final JacobiansMapper mapper = partials.getMapper();
241 PickUpHandler pickUp = new PickUpHandler(mapper, null);
242 propagator.setStepHandler(pickUp);
243 propagator.propagate(initialState.getDate().shiftedBy(dt));
244 double[][] dYdY0 = pickUp.getdYdY0();
245
246
247 double[][] dYdY0Ref = new double[6][6];
248 AbstractIntegratedPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
249 double[] steps = NumericalPropagator.tolerances(1000000 * dP, initialOrbit, orbitType)[0];
250 for (int i = 0; i < 6; ++i) {
251 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -4 * steps[i], i));
252 SpacecraftState sM4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
253 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -3 * steps[i], i));
254 SpacecraftState sM3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
255 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -2 * steps[i], i));
256 SpacecraftState sM2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
257 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -1 * steps[i], i));
258 SpacecraftState sM1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
259 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 1 * steps[i], i));
260 SpacecraftState sP1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
261 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 2 * steps[i], i));
262 SpacecraftState sP2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
263 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 3 * steps[i], i));
264 SpacecraftState sP3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
265 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 4 * steps[i], i));
266 SpacecraftState sP4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
267 fillJacobianColumn(dYdY0Ref, i, orbitType, angleType, steps[i],
268 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
269 }
270
271 for (int i = 0; i < 6; ++i) {
272 for (int j = 0; j < 6; ++j) {
273 double error = FastMath.abs((dYdY0[i][j] - dYdY0Ref[i][j]) / dYdY0Ref[i][j]);
274 Assert.assertEquals(0, error, 6.0e-2);
275
276 }
277 }
278
279 }
280 }
281
282 }
283
284 @Test
285 public void testPropagationTypesHyperbolic() {
286
287 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
288 ForceModel gravityField =
289 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
290 Orbit initialOrbit =
291 new KeplerianOrbit(new PVCoordinates(new Vector3D(-1551946.0, 708899.0, 6788204.0),
292 new Vector3D(-9875.0, -3941.0, -1845.0)),
293 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
294 provider.getMu());
295
296 double dt = 900;
297 double dP = 0.001;
298 for (OrbitType orbitType : new OrbitType[] { OrbitType.KEPLERIAN, OrbitType.CARTESIAN }) {
299 for (PositionAngle angleType : PositionAngle.values()) {
300
301
302 NumericalPropagator propagator =
303 setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
304 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
305 final SpacecraftState initialState =
306 partials.setInitialJacobians(new SpacecraftState(initialOrbit));
307 propagator.setInitialState(initialState);
308 final JacobiansMapper mapper = partials.getMapper();
309 PickUpHandler pickUp = new PickUpHandler(mapper, null);
310 propagator.setStepHandler(pickUp);
311 propagator.propagate(initialState.getDate().shiftedBy(dt));
312 double[][] dYdY0 = pickUp.getdYdY0();
313
314
315 double[][] dYdY0Ref = new double[6][6];
316 AbstractIntegratedPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
317 double[] steps = NumericalPropagator.tolerances(1000000 * dP, initialOrbit, orbitType)[0];
318 for (int i = 0; i < 6; ++i) {
319 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -4 * steps[i], i));
320 SpacecraftState sM4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
321 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -3 * steps[i], i));
322 SpacecraftState sM3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
323 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -2 * steps[i], i));
324 SpacecraftState sM2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
325 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -1 * steps[i], i));
326 SpacecraftState sM1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
327 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 1 * steps[i], i));
328 SpacecraftState sP1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
329 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 2 * steps[i], i));
330 SpacecraftState sP2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
331 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 3 * steps[i], i));
332 SpacecraftState sP3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
333 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 4 * steps[i], i));
334 SpacecraftState sP4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
335 fillJacobianColumn(dYdY0Ref, i, orbitType, angleType, steps[i],
336 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
337 }
338
339 for (int i = 0; i < 6; ++i) {
340 for (int j = 0; j < 6; ++j) {
341 double error = FastMath.abs((dYdY0[i][j] - dYdY0Ref[i][j]) / dYdY0Ref[i][j]);
342 Assert.assertEquals(0, error, 1.0e-3);
343
344 }
345 }
346
347 }
348 }
349
350 }
351
352 @Test
353 public void testJacobianIssue18() {
354
355
356 final double mu = 3.9860047e14;
357
358 final double isp = 318;
359 final double mass = 2500;
360 final double a = 24396159;
361 final double e = 0.72831215;
362 final double i = FastMath.toRadians(7);
363 final double omega = FastMath.toRadians(180);
364 final double OMEGA = FastMath.toRadians(261);
365 final double lv = 0;
366
367 final double duration = 3653.99;
368 final double f = 420;
369 final double delta = FastMath.toRadians(-7.4978);
370 final double alpha = FastMath.toRadians(351);
371 final AttitudeProvider law = new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
372
373 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
374 new TimeComponents(23, 30, 00.000),
375 TimeScalesFactory.getUTC());
376 final Orbit orbit =
377 new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
378 FramesFactory.getEME2000(), initDate, mu);
379 final SpacecraftState initialState =
380 new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
381
382 final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
383 new TimeComponents(04, 15, 34.080),
384 TimeScalesFactory.getUTC());
385 final ConstantThrustManeuver maneuver =
386 new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
387
388 double[] absTolerance = {
389 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
390 };
391 double[] relTolerance = {
392 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
393 };
394 AdaptiveStepsizeIntegrator integrator =
395 new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
396 integrator.setInitialStepSize(60);
397 final NumericalPropagator propagator = new NumericalPropagator(integrator);
398
399 propagator.setAttitudeProvider(law);
400 propagator.addForceModel(maneuver);
401 maneuver.getParameterDriver("thrust").setSelected(true);
402
403 propagator.setOrbitType(OrbitType.CARTESIAN);
404 PartialDerivativesEquations PDE = new PartialDerivativesEquations("derivatives", propagator);
405 Assert.assertEquals(1, PDE.getSelectedParameters().getNbParams());
406 propagator.setInitialState(PDE.setInitialJacobians(initialState));
407
408 final AbsoluteDate finalDate = fireDate.shiftedBy(3800);
409 final SpacecraftState finalorb = propagator.propagate(finalDate);
410 Assert.assertEquals(0, finalDate.durationFrom(finalorb.getDate()), 1.0e-11);
411
412 }
413
414 @Test(expected=OrekitException.class)
415 public void testNotInitialized() {
416 Orbit initialOrbit =
417 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
418 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
419 Constants.EIGEN5C_EARTH_MU);
420
421 double dP = 0.001;
422 NumericalPropagator propagator =
423 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
424 new PartialDerivativesEquations("partials", propagator).getMapper();
425 }
426
427 @Test(expected=OrekitException.class)
428 public void testTooSmallDimension() {
429 Orbit initialOrbit =
430 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
431 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
432 Constants.EIGEN5C_EARTH_MU);
433
434 double dP = 0.001;
435 NumericalPropagator propagator =
436 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
437 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
438 partials.setInitialJacobians(new SpacecraftState(initialOrbit),
439 new double[5][6], new double[6][2]);
440 }
441
442 @Test(expected=OrekitException.class)
443 public void testTooLargeDimension() {
444 Orbit initialOrbit =
445 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
446 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
447 Constants.EIGEN5C_EARTH_MU);
448
449 double dP = 0.001;
450 NumericalPropagator propagator =
451 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
452 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
453 partials.setInitialJacobians(new SpacecraftState(initialOrbit),
454 new double[8][6], new double[6][2]);
455 }
456
457 @Test(expected=OrekitException.class)
458 public void testMismatchedDimensions() {
459 Orbit initialOrbit =
460 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
461 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
462 Constants.EIGEN5C_EARTH_MU);
463
464 double dP = 0.001;
465 NumericalPropagator propagator =
466 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
467 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
468 partials.setInitialJacobians(new SpacecraftState(initialOrbit),
469 new double[6][6], new double[7][2]);
470 }
471
472 @Test
473 public void testWrongParametersDimension() {
474 Orbit initialOrbit =
475 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
476 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
477 Constants.EIGEN5C_EARTH_MU);
478
479 double dP = 0.001;
480 ForceModel sunAttraction = new ThirdBodyAttraction(CelestialBodyFactory.getSun());
481 sunAttraction.getParameterDriver("Sun attraction coefficient").setSelected(true);
482 ForceModel moonAttraction = new ThirdBodyAttraction(CelestialBodyFactory.getMoon());
483 NumericalPropagator propagator =
484 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE,
485 sunAttraction, moonAttraction);
486 PartialDerivativesEquations partials = new PartialDerivativesEquations("partials", propagator);
487 try {
488 partials.setInitialJacobians(new SpacecraftState(initialOrbit),
489 new double[6][6], new double[6][3]);
490 partials.computeDerivatives(new SpacecraftState(initialOrbit), new double[6]);
491 Assert.fail("an exception should have been thrown");
492 } catch (OrekitException oe) {
493 Assert.assertEquals(OrekitMessages.INITIAL_MATRIX_AND_PARAMETERS_NUMBER_MISMATCH,
494 oe.getSpecifier());
495 }
496 }
497
498 private void fillJacobianColumn(double[][] jacobian, int column,
499 OrbitType orbitType, PositionAngle angleType, double h,
500 SpacecraftState sM4h, SpacecraftState sM3h,
501 SpacecraftState sM2h, SpacecraftState sM1h,
502 SpacecraftState sP1h, SpacecraftState sP2h,
503 SpacecraftState sP3h, SpacecraftState sP4h) {
504 boolean withMass = jacobian.length > 6;
505 double[] aM4h = stateToArray(sM4h, orbitType, angleType, withMass)[0];
506 double[] aM3h = stateToArray(sM3h, orbitType, angleType, withMass)[0];
507 double[] aM2h = stateToArray(sM2h, orbitType, angleType, withMass)[0];
508 double[] aM1h = stateToArray(sM1h, orbitType, angleType, withMass)[0];
509 double[] aP1h = stateToArray(sP1h, orbitType, angleType, withMass)[0];
510 double[] aP2h = stateToArray(sP2h, orbitType, angleType, withMass)[0];
511 double[] aP3h = stateToArray(sP3h, orbitType, angleType, withMass)[0];
512 double[] aP4h = stateToArray(sP4h, orbitType, angleType, withMass)[0];
513 for (int i = 0; i < jacobian.length; ++i) {
514 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
515 32 * (aP3h[i] - aM3h[i]) -
516 168 * (aP2h[i] - aM2h[i]) +
517 672 * (aP1h[i] - aM1h[i])) / (840 * h);
518 }
519 }
520
521 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType, PositionAngle angleType,
522 double delta, int column) {
523
524 double[][] array = stateToArray(state, orbitType, angleType, true);
525 array[0][column] += delta;
526
527 return arrayToState(array, orbitType, angleType, state.getFrame(), state.getDate(),
528 state.getMu(), state.getAttitude());
529
530 }
531
532 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType, PositionAngle angleType,
533 boolean withMass) {
534 double[][] array = new double[2][withMass ? 7 : 6];
535 orbitType.mapOrbitToArray(state.getOrbit(), angleType, array[0], array[1]);
536 if (withMass) {
537 array[0][6] = state.getMass();
538 }
539 return array;
540 }
541
542 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType, PositionAngle angleType,
543 Frame frame, AbsoluteDate date, double mu,
544 Attitude attitude) {
545 Orbit orbit = orbitType.mapArrayToOrbit(array[0], array[1], angleType, date, mu, frame);
546 return (array.length > 6) ?
547 new SpacecraftState(orbit, attitude) :
548 new SpacecraftState(orbit, attitude, array[0][6]);
549 }
550
551 private NumericalPropagator setUpPropagator(Orbit orbit, double dP,
552 OrbitType orbitType, PositionAngle angleType,
553 ForceModel... models)
554 {
555
556 final double minStep = 0.001;
557 final double maxStep = 1000;
558
559 double[][] tol = NumericalPropagator.tolerances(dP, orbit, orbitType);
560 NumericalPropagator propagator =
561 new NumericalPropagator(new DormandPrince853Integrator(minStep, maxStep, tol[0], tol[1]));
562 propagator.setOrbitType(orbitType);
563 propagator.setPositionAngleType(angleType);
564 for (ForceModel model : models) {
565 propagator.addForceModel(model);
566 }
567 return propagator;
568 }
569
570 private static class PickUpHandler implements OrekitStepHandler {
571
572 private final JacobiansMapper mapper;
573 private final AbsoluteDate pickUpDate;
574 private final double[][] dYdY0;
575 private final double[][] dYdP;
576
577 public PickUpHandler(JacobiansMapper mapper, AbsoluteDate pickUpDate) {
578 this.mapper = mapper;
579 this.pickUpDate = pickUpDate;
580 dYdY0 = new double[JacobiansMapper.STATE_DIMENSION][JacobiansMapper.STATE_DIMENSION];
581 dYdP = new double[JacobiansMapper.STATE_DIMENSION][mapper.getParameters()];
582 }
583
584 public double[][] getdYdY0() {
585 return dYdY0;
586 }
587
588 public double[][] getdYdP() {
589 return dYdP;
590 }
591
592 public void handleStep(OrekitStepInterpolator interpolator) {
593 if (pickUpDate != null) {
594
595 double dt0 = pickUpDate.durationFrom(interpolator.getPreviousState().getDate());
596 double dt1 = pickUpDate.durationFrom(interpolator.getCurrentState().getDate());
597 if (dt0 * dt1 > 0) {
598
599 return;
600 } else {
601 checkState(interpolator.getInterpolatedState(pickUpDate));
602 }
603 }
604 }
605
606 public void finish(SpacecraftState finalState) {
607 checkState(finalState);
608 }
609
610 private void checkState(final SpacecraftState state) {
611 Assert.assertEquals(1, state.getAdditionalStatesValues().size());
612 Assert.assertEquals(mapper.getName(), state.getAdditionalStatesValues().getData().get(0).getKey());
613 mapper.getStateJacobian(state, dYdY0);
614 mapper.getParametersJacobian(state, dYdP);
615 }
616
617 }
618
619 @Before
620 public void setUp() {
621 Utils.setDataRoot("regular-data:potential/shm-format");
622 GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
623 }
624
625 }
626