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