1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18 package org.orekit.propagation.analytical;
19
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.linear.RealMatrix;
22 import org.hipparchus.util.FastMath;
23 import org.junit.jupiter.api.Assertions;
24 import org.junit.jupiter.api.BeforeEach;
25 import org.junit.jupiter.api.Test;
26 import org.orekit.Utils;
27 import org.orekit.attitudes.Attitude;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.frames.Frame;
30 import org.orekit.frames.FramesFactory;
31 import org.orekit.orbits.CartesianOrbit;
32 import org.orekit.orbits.EquinoctialOrbit;
33 import org.orekit.orbits.Orbit;
34 import org.orekit.orbits.OrbitType;
35 import org.orekit.orbits.PositionAngleType;
36 import org.orekit.propagation.MatricesHarvester;
37 import org.orekit.propagation.SpacecraftState;
38 import org.orekit.propagation.ToleranceProvider;
39 import org.orekit.time.AbsoluteDate;
40 import org.orekit.utils.Constants;
41 import org.orekit.utils.PVCoordinates;
42
43 public class KeplerianStateTransitionMatrixTest {
44
45 @BeforeEach
46 public void setUp() {
47 Utils.setDataRoot("regular-data");
48 }
49
50 @Test
51 public void testNullStmName() {
52 Assertions.assertThrows(OrekitException.class, () -> {
53
54
55 Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
56 Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
57
58 AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
59 Orbit initialOrbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
60 FramesFactory.getEME2000(), initDate, Constants.WGS84_EARTH_MU);
61
62
63
64 KeplerianPropagator extrapolator = new KeplerianPropagator(initialOrbit);
65 extrapolator.setupMatricesComputation(null, null, null);
66 });
67 }
68
69 @Test
70 public void testStateJacobian() {
71
72
73
74 Vector3D position = new Vector3D(7.0e6, 1.0e6, 4.0e6);
75 Vector3D velocity = new Vector3D(-500.0, 8000.0, 1000.0);
76
77 AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
78 Orbit initialOrbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
79 FramesFactory.getEME2000(), initDate, Constants.WGS84_EARTH_MU);
80
81
82 KeplerianPropagator propagator = new KeplerianPropagator(initialOrbit);
83 final SpacecraftState initialState = propagator.getInitialState();
84 final double[] stateVector = new double[6];
85 OrbitType.CARTESIAN.mapOrbitToArray(initialState.getOrbit(), PositionAngleType.MEAN, stateVector, null);
86 final AbsoluteDate target = initialState.getDate().shiftedBy(initialState.getOrbit().getKeplerianPeriod());
87 MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, null);
88 final SpacecraftState finalState = propagator.propagate(target);
89 RealMatrix dYdY0 = harvester.getStateTransitionMatrix(finalState);
90 Assertions.assertEquals(OrbitType.CARTESIAN, harvester.getOrbitType());
91 Assertions.assertEquals(PositionAngleType.MEAN, harvester.getPositionAngleType());
92
93
94 double[][] dYdY0Ref = new double[6][6];
95 KeplerianPropagator propagator2;
96 double[] steps = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(initialState.getOrbit(), OrbitType.CARTESIAN)[0];
97 for (int i = 0; i < 6; ++i) {
98 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, -4 * steps[i], i).getOrbit());
99 SpacecraftState sM4h = propagator2.propagate(target);
100 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, -3 * steps[i], i).getOrbit());
101 SpacecraftState sM3h = propagator2.propagate(target);
102 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, -2 * steps[i], i).getOrbit());
103 SpacecraftState sM2h = propagator2.propagate(target);
104 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, -1 * steps[i], i).getOrbit());
105 SpacecraftState sM1h = propagator2.propagate(target);
106 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, +1 * steps[i], i).getOrbit());
107 SpacecraftState sP1h = propagator2.propagate(target);
108 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, +2 * steps[i], i).getOrbit());
109 SpacecraftState sP2h = propagator2.propagate(target);
110 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, +3 * steps[i], i).getOrbit());
111 SpacecraftState sP3h = propagator2.propagate(target);
112 propagator2 = new KeplerianPropagator(shiftState(initialState, OrbitType.CARTESIAN, +4 * steps[i], i).getOrbit());
113 SpacecraftState sP4h = propagator2.propagate(target);
114 fillJacobianColumn(dYdY0Ref, i, OrbitType.CARTESIAN, steps[i],
115 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
116 }
117
118
119 for (int i = 0; i < 6; ++i) {
120 for (int j = 0; j < 6; ++j) {
121 if (stateVector[i] != 0) {
122 double error = FastMath.abs((dYdY0.getEntry(i, j) - dYdY0Ref[i][j]) / stateVector[i]) * steps[j];
123 Assertions.assertEquals(0, error, 7.16e-14);
124 }
125 }
126 }
127
128 }
129
130 private void fillJacobianColumn(double[][] jacobian, int column,
131 OrbitType orbitType, double h,
132 SpacecraftState sM4h, SpacecraftState sM3h,
133 SpacecraftState sM2h, SpacecraftState sM1h,
134 SpacecraftState sP1h, SpacecraftState sP2h,
135 SpacecraftState sP3h, SpacecraftState sP4h) {
136 double[] aM4h = stateToArray(sM4h, orbitType)[0];
137 double[] aM3h = stateToArray(sM3h, orbitType)[0];
138 double[] aM2h = stateToArray(sM2h, orbitType)[0];
139 double[] aM1h = stateToArray(sM1h, orbitType)[0];
140 double[] aP1h = stateToArray(sP1h, orbitType)[0];
141 double[] aP2h = stateToArray(sP2h, orbitType)[0];
142 double[] aP3h = stateToArray(sP3h, orbitType)[0];
143 double[] aP4h = stateToArray(sP4h, orbitType)[0];
144 for (int i = 0; i < jacobian.length; ++i) {
145 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
146 32 * (aP3h[i] - aM3h[i]) -
147 168 * (aP2h[i] - aM2h[i]) +
148 672 * (aP1h[i] - aM1h[i])) / (840 * h);
149 }
150 }
151
152 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
153 double delta, int column) {
154
155 double[][] array = stateToArray(state, orbitType);
156 array[0][column] += delta;
157
158 return arrayToState(array, state.getFrame(), state.getDate(),
159 state.getOrbit().getMu(), state.getAttitude());
160
161 }
162
163 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
164 double[][] array = new double[2][6];
165
166 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngleType.MEAN, array[0], array[1]);
167 return array;
168 }
169
170 private SpacecraftState arrayToState(double[][] array,
171 Frame frame, AbsoluteDate date, double mu,
172 Attitude attitude) {
173 CartesianOrbit orbit = (CartesianOrbit) OrbitType.CARTESIAN.mapArrayToOrbit(array[0], array[1], PositionAngleType.MEAN, date, mu, frame);
174 return new SpacecraftState(orbit, attitude);
175 }
176
177 }