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 static org.hamcrest.CoreMatchers.is;
20
21 import java.util.Arrays;
22 import java.util.Collections;
23 import java.util.List;
24 import java.util.stream.Stream;
25
26 import org.hamcrest.MatcherAssert;
27 import org.hipparchus.CalculusFieldElement;
28 import org.hipparchus.Field;
29 import org.hipparchus.analysis.differentiation.DerivativeStructure;
30 import org.hipparchus.exception.LocalizedCoreFormats;
31 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
32 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
33 import org.hipparchus.geometry.euclidean.threed.Rotation;
34 import org.hipparchus.geometry.euclidean.threed.Vector3D;
35 import org.hipparchus.linear.MatrixUtils;
36 import org.hipparchus.linear.RealMatrix;
37 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
38 import org.hipparchus.ode.nonstiff.DormandPrince54Integrator;
39 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
40 import org.hipparchus.util.FastMath;
41 import org.junit.Assert;
42 import org.junit.Before;
43 import org.junit.Test;
44 import org.orekit.Utils;
45 import org.orekit.attitudes.Attitude;
46 import org.orekit.attitudes.AttitudeProvider;
47 import org.orekit.attitudes.InertialProvider;
48 import org.orekit.errors.OrekitException;
49 import org.orekit.forces.AbstractForceModel;
50 import org.orekit.forces.ForceModel;
51 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
52 import org.orekit.forces.gravity.NewtonianAttraction;
53 import org.orekit.forces.gravity.potential.GravityFieldFactory;
54 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
55 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
56 import org.orekit.forces.maneuvers.Maneuver;
57 import org.orekit.forces.maneuvers.jacobians.TriggerDate;
58 import org.orekit.forces.maneuvers.propulsion.BasicConstantThrustPropulsionModel;
59 import org.orekit.forces.maneuvers.propulsion.PropulsionModel;
60 import org.orekit.forces.maneuvers.trigger.DateBasedManeuverTriggers;
61 import org.orekit.frames.Frame;
62 import org.orekit.frames.FramesFactory;
63 import org.orekit.orbits.CartesianOrbit;
64 import org.orekit.orbits.KeplerianOrbit;
65 import org.orekit.orbits.Orbit;
66 import org.orekit.orbits.OrbitType;
67 import org.orekit.orbits.PositionAngle;
68 import org.orekit.propagation.AdditionalStateProvider;
69 import org.orekit.propagation.FieldSpacecraftState;
70 import org.orekit.propagation.MatricesHarvester;
71 import org.orekit.propagation.PropagatorsParallelizer;
72 import org.orekit.propagation.SpacecraftState;
73 import org.orekit.propagation.events.EventDetector;
74 import org.orekit.propagation.events.FieldEventDetector;
75 import org.orekit.propagation.integration.AbstractIntegratedPropagator;
76 import org.orekit.propagation.integration.AdditionalDerivativesProvider;
77 import org.orekit.time.AbsoluteDate;
78 import org.orekit.time.DateComponents;
79 import org.orekit.time.TimeComponents;
80 import org.orekit.time.TimeScalesFactory;
81 import org.orekit.utils.Constants;
82 import org.orekit.utils.IERSConventions;
83 import org.orekit.utils.PVCoordinates;
84 import org.orekit.utils.ParameterDriver;
85
86
87 public class StateTransitionMatrixGeneratorTest {
88
89 @Before
90 public void setUp() {
91 Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
92 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
93 }
94
95 @Test
96 public void testInterrupt() {
97 final AbsoluteDate firing = new AbsoluteDate(new DateComponents(2004, 1, 2),
98 new TimeComponents(4, 15, 34.080),
99 TimeScalesFactory.getUTC());
100 final double duration = 200.0;
101
102
103 DateBasedManeuverTriggers triggers1 = new DateBasedManeuverTriggers("MAN_0", firing, duration);
104 final NumericalPropagator propagator1 = buildPropagator(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, 20,
105 firing, duration, triggers1);
106 propagator1.
107 getAllForceModels().
108 forEach(fm -> fm.
109 getParametersDrivers().
110 stream().
111 filter(d -> d.getName().equals("MAN_0_START") ||
112 d.getName().equals(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT)).
113 forEach(d -> d.setSelected(true)));
114 final MatricesHarvester harvester1 = propagator1.setupMatricesComputation("stm", null, null);
115 final SpacecraftState state1 = propagator1.propagate(firing.shiftedBy(2 * duration));
116 final RealMatrix stm1 = harvester1.getStateTransitionMatrix(state1);
117 final RealMatrix jacobian1 = harvester1.getParametersJacobian(state1);
118
119
120 DateBasedManeuverTriggers triggers2 = new DateBasedManeuverTriggers("MAN_0", firing, duration);
121 final NumericalPropagator propagator2 = buildPropagator(OrbitType.EQUINOCTIAL, PositionAngle.TRUE, 20,
122 firing, duration, triggers2);
123 propagator2.
124 getAllForceModels().
125 forEach(fm -> fm.
126 getParametersDrivers().
127 stream().
128 filter(d -> d.getName().equals("MAN_0_START") ||
129 d.getName().equals(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT)).
130 forEach(d -> d.setSelected(true)));
131
132
133 final StateTransitionMatrixGenerator dummyStmGenerator =
134 new StateTransitionMatrixGenerator("dummy-1",
135 Collections.emptyList(),
136 propagator2.getAttitudeProvider());
137 propagator2.addAdditionalDerivativesProvider(dummyStmGenerator);
138 propagator2.setInitialState(propagator2.getInitialState().addAdditionalState(dummyStmGenerator.getName(), new double[36]));
139 propagator2.addAdditionalDerivativesProvider(new IntegrableJacobianColumnGenerator(dummyStmGenerator, "dummy-2"));
140 propagator2.setInitialState(propagator2.getInitialState().addAdditionalState("dummy-2", new double[6]));
141 propagator2.addAdditionalDerivativesProvider(new AdditionalDerivativesProvider() {
142 public String getName() { return "dummy-3"; }
143 public int getDimension() { return 1; }
144 public double[] derivatives(SpacecraftState s) { return new double[1]; }
145 });
146 propagator2.setInitialState(propagator2.getInitialState().addAdditionalState("dummy-3", new double[1]));
147 propagator2.addAdditionalStateProvider(new TriggerDate(dummyStmGenerator.getName(), "dummy-4", true,
148 (Maneuver) propagator2.getAllForceModels().get(1),
149 1.0e-6));
150 propagator2.addAdditionalStateProvider(new AdditionalStateProvider() {
151 public String getName() { return "dummy-5"; }
152 public double[] getAdditionalState(SpacecraftState s) { return new double[1]; }
153 });
154 final MatricesHarvester harvester2 = propagator2.setupMatricesComputation("stm", null, null);
155 final SpacecraftState intermediate = propagator2.propagate(firing.shiftedBy(0.5 * duration));
156 final RealMatrix stmI = harvester2.getStateTransitionMatrix(intermediate);
157 final RealMatrix jacobianI = harvester2.getParametersJacobian(intermediate);
158
159
160 Assert.assertEquals(0.1253, stmI.subtract(stm1).getNorm1() / stm1.getNorm1(), 1.0e-4);
161 Assert.assertEquals(0.0225, jacobianI.subtract(jacobian1).getNorm1() / jacobian1.getNorm1(), 1.0e-4);
162
163
164 final SpacecraftState state2 = propagator2.propagate(firing.shiftedBy(2 * duration));
165 final RealMatrix stm2 = harvester2.getStateTransitionMatrix(state2);
166 final RealMatrix jacobian2 = harvester2.getParametersJacobian(state2);
167
168
169 Assert.assertEquals(0.0, stm2.subtract(stm1).getNorm1(), 1.3e-13 * stm1.getNorm1());
170 Assert.assertEquals(0.0, jacobian2.subtract(jacobian1).getNorm1(), 3.0e-14 * jacobian1.getNorm1());
171
172 }
173
174
175
176
177 @Test
178 public void testComputeDerivativesStateVelocity() {
179
180
181
182 final AbsoluteDate date = AbsoluteDate.J2000_EPOCH;
183
184 final double gm = Constants.EIGEN5C_EARTH_MU;
185
186 Frame eci = FramesFactory.getGCRF();
187 NumericalPropagator propagator = new NumericalPropagator(new DormandPrince54Integrator(1, 500, 0.001, 0.001));
188 MockForceModel forceModel = new MockForceModel();
189 propagator.addForceModel(forceModel);
190 StateTransitionMatrixGenerator stmGenerator =
191 new StateTransitionMatrixGenerator("stm",
192 propagator.getAllForceModels(),
193 propagator.getAttitudeProvider());
194 Vector3D p = new Vector3D(7378137, 0, 0);
195 Vector3D v = new Vector3D(0, 7500, 0);
196 PVCoordinates pv = new PVCoordinates(p, v);
197 SpacecraftState state = stmGenerator.setInitialStateTransitionMatrix(new SpacecraftState(new CartesianOrbit(pv, eci, date, gm)),
198 MatrixUtils.createRealIdentityMatrix(6),
199 propagator.getOrbitType(),
200 propagator.getPositionAngleType());
201
202
203 stmGenerator.derivatives(state);
204
205
206 MatcherAssert.assertThat(forceModel.accelerationDerivativesPosition.toVector3D(), is(pv.getPosition()));
207 MatcherAssert.assertThat(forceModel.accelerationDerivativesVelocity.toVector3D(), is(pv.getVelocity()));
208
209 }
210
211 @Test
212 public void testPropagationTypesElliptical() {
213
214 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
215 ForceModel gravityField =
216 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
217 Orbit initialOrbit =
218 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
219 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
220 provider.getMu());
221
222 double dt = 900;
223 double dP = 0.001;
224 for (OrbitType orbitType : OrbitType.values()) {
225 for (PositionAngle angleType : PositionAngle.values()) {
226
227
228 NumericalPropagator propagator =
229 setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
230 final SpacecraftState initialState = new SpacecraftState(initialOrbit);
231 propagator.setInitialState(initialState);
232 PickUpHandler pickUp = new PickUpHandler(propagator, null, null, null);
233 propagator.getMultiplexer().add(pickUp);
234 propagator.propagate(initialState.getDate().shiftedBy(dt));
235
236
237 double[][] dYdY0Ref = finiteDifferencesStm(initialOrbit, orbitType, angleType, dP, dt, gravityField);
238
239 for (int i = 0; i < 6; ++i) {
240 for (int j = 0; j < 6; ++j) {
241 double error = FastMath.abs((pickUp.getStm().getEntry(i, j) - dYdY0Ref[i][j]) / dYdY0Ref[i][j]);
242 Assert.assertEquals(0, error, 6.0e-2);
243
244 }
245 }
246
247 }
248 }
249
250 }
251
252 @Test
253 public void testPropagationTypesHyperbolic() {
254
255 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
256 ForceModel gravityField =
257 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
258 Orbit initialOrbit =
259 new KeplerianOrbit(new PVCoordinates(new Vector3D(-1551946.0, 708899.0, 6788204.0),
260 new Vector3D(-9875.0, -3941.0, -1845.0)),
261 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
262 provider.getMu());
263
264 double dt = 900;
265 double dP = 0.001;
266 for (OrbitType orbitType : new OrbitType[] { OrbitType.KEPLERIAN, OrbitType.CARTESIAN }) {
267 for (PositionAngle angleType : PositionAngle.values()) {
268
269
270 NumericalPropagator propagator =
271 setUpPropagator(initialOrbit, dP, orbitType, angleType, gravityField);
272 final SpacecraftState initialState = new SpacecraftState(initialOrbit);
273 PickUpHandler pickUp = new PickUpHandler(propagator, null, null, null);
274 propagator.setInitialState(initialState);
275 propagator.setStepHandler(pickUp);
276 propagator.propagate(initialState.getDate().shiftedBy(dt));
277
278
279 double[][] dYdY0Ref = finiteDifferencesStm(initialOrbit, orbitType, angleType, dP, dt, gravityField);
280 for (int i = 0; i < 6; ++i) {
281 for (int j = 0; j < 6; ++j) {
282 double error = FastMath.abs((pickUp.getStm().getEntry(i, j) - dYdY0Ref[i][j]) / dYdY0Ref[i][j]);
283 Assert.assertEquals(0, error, 1.0e-3);
284
285 }
286 }
287
288 }
289 }
290
291 }
292
293 @Test
294 public void testAccelerationPartialNewtonOnly() {
295
296 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
297 ForceModel newton = new NewtonianAttraction(provider.getMu());
298 ParameterDriver gmDriver = newton.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT);
299 gmDriver.setSelected(true);
300 Orbit initialOrbit =
301 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
302 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
303 provider.getMu());
304
305 NumericalPropagator propagator =
306 setUpPropagator(initialOrbit, 0.001, OrbitType.EQUINOCTIAL, PositionAngle.MEAN,
307 newton);
308 final SpacecraftState initialState = new SpacecraftState(initialOrbit);
309 propagator.setInitialState(initialState);
310 AbsoluteDate pickupDate = initialOrbit.getDate().shiftedBy(200);
311 PickUpHandler pickUp = new PickUpHandler(propagator, pickupDate, gmDriver.getName(), gmDriver.getName());
312 propagator.setStepHandler(pickUp);
313 propagator.propagate(initialState.getDate().shiftedBy(900.0));
314 Assert.assertEquals(0.0, pickUp.getState().getDate().durationFrom(pickupDate), 1.0e-10);
315 final Vector3D position = pickUp.getState().getPVCoordinates().getPosition();
316 final double r = position.getNorm();
317
318
319 Assert.assertEquals(-position.getX() / (r * r * r), pickUp.getAccPartial()[0], 1.0e-15 / (r * r));
320 Assert.assertEquals(-position.getY() / (r * r * r), pickUp.getAccPartial()[1], 1.0e-15 / (r * r));
321 Assert.assertEquals(-position.getZ() / (r * r * r), pickUp.getAccPartial()[2], 1.0e-15 / (r * r));
322
323 }
324
325 @Test
326 public void testAccelerationPartialGravity5x5() {
327
328 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
329 ForceModel gravityField =
330 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
331 ParameterDriver gmDriver = gravityField.getParameterDriver(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT);
332 gmDriver.setSelected(true);
333 ForceModel newton = new NewtonianAttraction(provider.getMu());
334 Orbit initialOrbit =
335 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
336 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
337 provider.getMu());
338
339 NumericalPropagator propagator =
340 setUpPropagator(initialOrbit, 0.001, OrbitType.EQUINOCTIAL, PositionAngle.MEAN,
341 gravityField, newton);
342 final SpacecraftState initialState = new SpacecraftState(initialOrbit);
343 propagator.setInitialState(initialState);
344 AbsoluteDate pickupDate = initialOrbit.getDate().shiftedBy(200);
345 PickUpHandler pickUp = new PickUpHandler(propagator, pickupDate, gmDriver.getName(), gmDriver.getName());
346 propagator.setStepHandler(pickUp);
347 propagator.propagate(initialState.getDate().shiftedBy(900.0));
348 Assert.assertEquals(0.0, pickUp.getState().getDate().durationFrom(pickupDate), 1.0e-10);
349 final Vector3D position = pickUp.getState().getPVCoordinates().getPosition();
350 final double r = position.getNorm();
351
352 Assert.assertTrue(FastMath.abs(-position.getX() / (r * r * r) - pickUp.getAccPartial()[0]) > 2.0e-4 / (r * r));
353 Assert.assertTrue(FastMath.abs(-position.getY() / (r * r * r) - pickUp.getAccPartial()[1]) > 2.0e-4 / (r * r));
354 Assert.assertTrue(FastMath.abs(-position.getZ() / (r * r * r) - pickUp.getAccPartial()[2]) > 2.0e-4 / (r * r));
355
356 }
357
358 @Test
359 public void testMultiSat() {
360
361 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
362 Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
363 Orbit initialOrbitA =
364 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
365 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
366 provider.getMu());
367 Orbit initialOrbitB =
368 new KeplerianOrbit(7900000.0, 0.015, 0.04, 0.7, 0, 1.2, PositionAngle.TRUE,
369 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
370 provider.getMu());
371
372 double dt = 900;
373 double dP = 0.001;
374 for (OrbitType orbitType : OrbitType.values()) {
375 for (PositionAngle angleType : PositionAngle.values()) {
376
377
378 NumericalPropagator propagatorA1 = setUpPropagator(initialOrbitA, dP, orbitType, angleType,
379 new HolmesFeatherstoneAttractionModel(itrf, provider));
380 final SpacecraftState initialStateA = new SpacecraftState(initialOrbitA);
381 propagatorA1.setInitialState(initialStateA);
382 final PickUpHandler pickUpA = new PickUpHandler(propagatorA1, null, null, null);
383 propagatorA1.setStepHandler(pickUpA);
384
385 NumericalPropagator propagatorB1 = setUpPropagator(initialOrbitB, dP, orbitType, angleType,
386 new HolmesFeatherstoneAttractionModel(itrf, provider));
387 final SpacecraftState initialStateB1 = new SpacecraftState(initialOrbitB);
388 propagatorB1.setInitialState(initialStateB1);
389 final PickUpHandler pickUpB = new PickUpHandler(propagatorB1, null, null, null);
390 propagatorB1.setStepHandler(pickUpB);
391
392 PropagatorsParallelizer parallelizer = new PropagatorsParallelizer(Arrays.asList(propagatorA1, propagatorB1),
393 interpolators -> {});
394 parallelizer.propagate(initialStateA.getDate(), initialStateA.getDate().shiftedBy(dt));
395
396
397 double[][] dYdY0RefA = finiteDifferencesStm(initialOrbitA, orbitType, angleType, dP, dt,
398 new HolmesFeatherstoneAttractionModel(itrf, provider));
399 for (int i = 0; i < 6; ++i) {
400 for (int j = 0; j < 6; ++j) {
401 double error = FastMath.abs((pickUpA.getStm().getEntry(i, j) - dYdY0RefA[i][j]) / dYdY0RefA[i][j]);
402 Assert.assertEquals(0, error, 6.0e-2);
403
404 }
405 }
406
407 double[][] dYdY0RefB = finiteDifferencesStm(initialOrbitB, orbitType, angleType, dP, dt,
408 new HolmesFeatherstoneAttractionModel(itrf, provider));
409 for (int i = 0; i < 6; ++i) {
410 for (int j = 0; j < 6; ++j) {
411 double error = FastMath.abs((pickUpB.getStm().getEntry(i, j) - dYdY0RefB[i][j]) / dYdY0RefB[i][j]);
412 Assert.assertEquals(0, error, 6.0e-2);
413
414 }
415 }
416
417 }
418 }
419
420 }
421
422 @Test
423 public void testParallelStm() {
424
425 double a = 7187990.1979844316;
426 double e = 0.5e-4;
427 double i = 1.7105407051081795;
428 double omega = 1.9674147913622104;
429 double OMEGA = FastMath.toRadians(261);
430 double lv = 0;
431
432 AbsoluteDate date = new AbsoluteDate(new DateComponents(2004, 01, 01),
433 TimeComponents.H00,
434 TimeScalesFactory.getUTC());
435 Orbit orbit = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
436 FramesFactory.getEME2000(), date, Constants.EIGEN5C_EARTH_MU);
437 final AbsoluteDate startDate = orbit.getDate();
438 final AbsoluteDate endDate = startDate.shiftedBy(120.0);
439 OrbitType type = OrbitType.CARTESIAN;
440 double minStep = 0.0001;
441 double maxStep = 60;
442 double[][] tolerances = NumericalPropagator.tolerances(0.001, orbit, type);
443 AdaptiveStepsizeIntegrator integrator0 = new DormandPrince853Integrator(minStep, maxStep, tolerances[0], tolerances[1]);
444 integrator0.setInitialStepSize(1.0);
445 NumericalPropagator p0 = new NumericalPropagator(integrator0);
446 p0.setInitialState(new SpacecraftState(orbit).addAdditionalState("tmp", new double[1]));
447 p0.setupMatricesComputation("stm0", null, null);
448 AdaptiveStepsizeIntegrator integrator1 = new DormandPrince853Integrator(minStep, maxStep, tolerances[0], tolerances[1]);
449 integrator1.setInitialStepSize(1.0);
450 NumericalPropagator p1 = new NumericalPropagator(integrator1);
451 p1.setInitialState(new SpacecraftState(orbit));
452 p1.setupMatricesComputation("stm1", null, null);
453 final List<SpacecraftState> results = new PropagatorsParallelizer(Arrays.asList(p0, p1), interpolators -> {}).
454 propagate(startDate, endDate);
455 Assert.assertEquals(-0.07953750951271785, results.get(0).getAdditionalState("stm0")[0], 1.0e-10);
456 Assert.assertEquals(-0.07953750951271785, results.get(1).getAdditionalState("stm1")[0], 1.0e-10);
457 }
458
459 @Test
460 public void testNotInitialized() {
461 Orbit initialOrbit =
462 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
463 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
464 Constants.EIGEN5C_EARTH_MU);
465
466 double dP = 0.001;
467 NumericalPropagator propagator =
468 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
469 StateTransitionMatrixGenerator stmGenerator = new StateTransitionMatrixGenerator("stm",
470 propagator.getAllForceModels(),
471 propagator.getAttitudeProvider());
472 propagator.addAdditionalDerivativesProvider(stmGenerator);
473 Assert.assertTrue(stmGenerator.yield(new SpacecraftState(initialOrbit)));
474 }
475
476 @Test
477 public void testMismatchedDimensions() {
478 Orbit initialOrbit =
479 new KeplerianOrbit(8000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngle.TRUE,
480 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
481 Constants.EIGEN5C_EARTH_MU);
482
483 double dP = 0.001;
484 NumericalPropagator propagator =
485 setUpPropagator(initialOrbit, dP, OrbitType.EQUINOCTIAL, PositionAngle.TRUE);
486 StateTransitionMatrixGenerator stmGenerator = new StateTransitionMatrixGenerator("stm",
487 propagator.getAllForceModels(),
488 propagator.getAttitudeProvider());
489 propagator.addAdditionalDerivativesProvider(stmGenerator);
490 try {
491 stmGenerator.setInitialStateTransitionMatrix(new SpacecraftState(initialOrbit),
492 MatrixUtils.createRealMatrix(5, 6),
493 propagator.getOrbitType(),
494 propagator.getPositionAngleType());
495 } catch (OrekitException oe) {
496 Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2, oe.getSpecifier());
497 Assert.assertEquals(5, ((Integer) oe.getParts()[0]).intValue());
498 Assert.assertEquals(6, ((Integer) oe.getParts()[1]).intValue());
499 Assert.assertEquals(6, ((Integer) oe.getParts()[2]).intValue());
500 Assert.assertEquals(6, ((Integer) oe.getParts()[3]).intValue());
501 }
502
503 try {
504 stmGenerator.setInitialStateTransitionMatrix(new SpacecraftState(initialOrbit),
505 MatrixUtils.createRealMatrix(6, 5),
506 propagator.getOrbitType(),
507 propagator.getPositionAngleType());
508 } catch (OrekitException oe) {
509 Assert.assertEquals(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2, oe.getSpecifier());
510 Assert.assertEquals(6, ((Integer) oe.getParts()[0]).intValue());
511 Assert.assertEquals(5, ((Integer) oe.getParts()[1]).intValue());
512 Assert.assertEquals(6, ((Integer) oe.getParts()[2]).intValue());
513 Assert.assertEquals(6, ((Integer) oe.getParts()[3]).intValue());
514 }
515
516 }
517
518 private void fillJacobianColumn(double[][] jacobian, int column,
519 OrbitType orbitType, PositionAngle angleType, double h,
520 SpacecraftState sM4h, SpacecraftState sM3h,
521 SpacecraftState sM2h, SpacecraftState sM1h,
522 SpacecraftState sP1h, SpacecraftState sP2h,
523 SpacecraftState sP3h, SpacecraftState sP4h) {
524 double[] aM4h = stateToArray(sM4h, orbitType, angleType)[0];
525 double[] aM3h = stateToArray(sM3h, orbitType, angleType)[0];
526 double[] aM2h = stateToArray(sM2h, orbitType, angleType)[0];
527 double[] aM1h = stateToArray(sM1h, orbitType, angleType)[0];
528 double[] aP1h = stateToArray(sP1h, orbitType, angleType)[0];
529 double[] aP2h = stateToArray(sP2h, orbitType, angleType)[0];
530 double[] aP3h = stateToArray(sP3h, orbitType, angleType)[0];
531 double[] aP4h = stateToArray(sP4h, orbitType, angleType)[0];
532 for (int i = 0; i < jacobian.length; ++i) {
533 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
534 32 * (aP3h[i] - aM3h[i]) -
535 168 * (aP2h[i] - aM2h[i]) +
536 672 * (aP1h[i] - aM1h[i])) / (840 * h);
537 }
538 }
539
540 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType, PositionAngle angleType,
541 double delta, int column) {
542
543 double[][] array = stateToArray(state, orbitType, angleType);
544 array[0][column] += delta;
545
546 return arrayToState(array, orbitType, angleType, state.getFrame(), state.getDate(),
547 state.getMu(), state.getAttitude());
548
549 }
550
551 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType, PositionAngle angleType) {
552 double[][] array = new double[2][6];
553 orbitType.mapOrbitToArray(state.getOrbit(), angleType, array[0], array[1]);
554 return array;
555 }
556
557 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType, PositionAngle angleType,
558 Frame frame, AbsoluteDate date, double mu,
559 Attitude attitude) {
560 Orbit orbit = orbitType.mapArrayToOrbit(array[0], array[1], angleType, date, mu, frame);
561 return new SpacecraftState(orbit, attitude);
562 }
563
564 private NumericalPropagator setUpPropagator(Orbit orbit, double dP,
565 OrbitType orbitType, PositionAngle angleType,
566 ForceModel... models) {
567
568 final double minStep = 0.001;
569 final double maxStep = 1000;
570
571 double[][] tol = NumericalPropagator.tolerances(dP, orbit, orbitType);
572 NumericalPropagator propagator =
573 new NumericalPropagator(new DormandPrince853Integrator(minStep, maxStep, tol[0], tol[1]));
574 propagator.setOrbitType(orbitType);
575 propagator.setPositionAngleType(angleType);
576 for (ForceModel model : models) {
577 propagator.addForceModel(model);
578 }
579 return propagator;
580 }
581
582 private double[][] finiteDifferencesStm(final Orbit initialOrbit, final OrbitType orbitType, final PositionAngle angleType,
583 final double dP, final double dt, ForceModel... models) {
584
585
586 double[][] dYdY0Ref = new double[6][6];
587 AbstractIntegratedPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType, models);
588 final SpacecraftState initialState = new SpacecraftState(initialOrbit);
589 double[] steps = NumericalPropagator.tolerances(1000000 * dP, initialOrbit, orbitType)[0];
590 for (int i = 0; i < 6; ++i) {
591 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -4 * steps[i], i));
592 SpacecraftState sM4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
593 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -3 * steps[i], i));
594 SpacecraftState sM3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
595 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -2 * steps[i], i));
596 SpacecraftState sM2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
597 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, -1 * steps[i], i));
598 SpacecraftState sM1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
599 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 1 * steps[i], i));
600 SpacecraftState sP1h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
601 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 2 * steps[i], i));
602 SpacecraftState sP2h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
603 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 3 * steps[i], i));
604 SpacecraftState sP3h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
605 propagator2.resetInitialState(shiftState(initialState, orbitType, angleType, 4 * steps[i], i));
606 SpacecraftState sP4h = propagator2.propagate(initialState.getDate().shiftedBy(dt));
607 fillJacobianColumn(dYdY0Ref, i, orbitType, angleType, steps[i],
608 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
609 }
610
611 return dYdY0Ref;
612
613 }
614
615 private NumericalPropagator buildPropagator(final OrbitType orbitType, final PositionAngle positionAngle,
616 final int degree, final AbsoluteDate firing, final double duration,
617 final DateBasedManeuverTriggers triggers) {
618
619 final AttitudeProvider attitudeProvider = buildAttitudeProvider();
620 SpacecraftState initialState = buildInitialState(attitudeProvider);
621
622 final double isp = 318;
623 final double f = 420;
624 PropulsionModel propulsionModel = new BasicConstantThrustPropulsionModel(f, isp, Vector3D.PLUS_I, "ABM");
625
626 double[][] tol = NumericalPropagator.tolerances(0.01, initialState.getOrbit(), orbitType);
627 AdaptiveStepsizeIntegrator integrator = new DormandPrince853Integrator(0.001, 1000, tol[0], tol[1]);
628 integrator.setInitialStepSize(60);
629 final NumericalPropagator propagator = new NumericalPropagator(integrator);
630
631 propagator.setOrbitType(orbitType);
632 propagator.setPositionAngleType(positionAngle);
633 propagator.setAttitudeProvider(attitudeProvider);
634 if (degree > 0) {
635 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true),
636 GravityFieldFactory.getNormalizedProvider(degree, degree)));
637 }
638 final Maneuver maneuver = new Maneuver(null, triggers, propulsionModel);
639 propagator.addForceModel(maneuver);
640 propagator.addAdditionalStateProvider(new AdditionalStateProvider() {
641 public String getName() { return triggers.getName().concat("-acc"); }
642 public double[] getAdditionalState(SpacecraftState state) {
643 double[] parameters = Arrays.copyOfRange(maneuver.getParameters(), 0, propulsionModel.getParametersDrivers().size());
644 return new double[] {
645 propulsionModel.getAcceleration(state, state.getAttitude(), parameters).getNorm()
646 };
647 }
648 });
649 propagator.setInitialState(initialState);
650 return propagator;
651
652 }
653
654 private SpacecraftState buildInitialState(final AttitudeProvider attitudeProvider) {
655 final double mass = 2500;
656 final double a = 24396159;
657 final double e = 0.72831215;
658 final double i = FastMath.toRadians(7);
659 final double omega = FastMath.toRadians(180);
660 final double OMEGA = FastMath.toRadians(261);
661 final double lv = 0;
662
663 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 1, 1), new TimeComponents(23, 30, 00.000),
664 TimeScalesFactory.getUTC());
665 final Orbit orbit = new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngle.TRUE,
666 FramesFactory.getEME2000(), initDate, Constants.EIGEN5C_EARTH_MU);
667 return new SpacecraftState(orbit, attitudeProvider.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
668 }
669
670 private AttitudeProvider buildAttitudeProvider() {
671 final double delta = FastMath.toRadians(-7.4978);
672 final double alpha = FastMath.toRadians(351);
673 return new InertialProvider(new Rotation(new Vector3D(alpha, delta), Vector3D.PLUS_I));
674 }
675
676
677 private static class MockForceModel extends AbstractForceModel {
678
679
680
681
682
683 public FieldVector3D<DerivativeStructure> accelerationDerivativesPosition;
684
685
686
687
688 public FieldVector3D<DerivativeStructure> accelerationDerivativesVelocity;
689
690
691 @Override
692 public boolean dependsOnPositionOnly() {
693 return false;
694 }
695
696 @Override
697 public <T extends CalculusFieldElement<T>> void
698 addContribution(FieldSpacecraftState<T> s,
699 FieldTimeDerivativesEquations<T> adder) {
700 }
701
702 @Override
703 public Vector3D acceleration(final SpacecraftState s, final double[] parameters)
704 {
705 return s.getPVCoordinates().getPosition();
706 }
707
708 @SuppressWarnings("unchecked")
709 @Override
710 public <T extends CalculusFieldElement<T>> FieldVector3D<T> acceleration(final FieldSpacecraftState<T> s,
711 final T[] parameters)
712 {
713 this.accelerationDerivativesPosition = (FieldVector3D<DerivativeStructure>) s.getPVCoordinates().getPosition();
714 this.accelerationDerivativesVelocity = (FieldVector3D<DerivativeStructure>) s.getPVCoordinates().getVelocity();
715 return s.getPVCoordinates().getPosition();
716 }
717
718 @Override
719 public Stream<EventDetector> getEventsDetectors() {
720 return Stream.empty();
721 }
722
723 @Override
724 public List<ParameterDriver> getParametersDrivers() {
725 return Collections.emptyList();
726 }
727
728 @Override
729 public <T extends CalculusFieldElement<T>> Stream<FieldEventDetector<T>> getFieldEventsDetectors(final Field<T> field) {
730 return Stream.empty();
731 }
732
733 }
734
735 }