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