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.forces.gravity.potential.GravityFieldFactory;
30 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
31 import org.orekit.frames.Frame;
32 import org.orekit.frames.FramesFactory;
33 import org.orekit.orbits.CartesianOrbit;
34 import org.orekit.orbits.Orbit;
35 import org.orekit.orbits.OrbitType;
36 import org.orekit.orbits.PositionAngleType;
37 import org.orekit.propagation.MatricesHarvester;
38 import org.orekit.propagation.SpacecraftState;
39 import org.orekit.propagation.ToleranceProvider;
40 import org.orekit.time.AbsoluteDate;
41 import org.orekit.utils.PVCoordinates;
42
43 public class BrouwerLyddaneStateTransitionMatrixTest {
44
45
46 private UnnormalizedSphericalHarmonicsProvider provider;
47
48
49 private double M2;
50
51 @BeforeEach
52 public void setUp() {
53 Utils.setDataRoot("regular-data:atmosphere:potential/icgem-format");
54 provider = GravityFieldFactory.getUnnormalizedProvider(5, 0);
55 M2 = BrouwerLyddanePropagator.M2;
56 }
57
58 @Test
59 public void testNullStmName() {
60 Assertions.assertThrows(OrekitException.class, () -> {
61
62
63
64
65 AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
66 Vector3D position = new Vector3D(3220103., 69623., 6149822.);
67 Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
68
69 Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
70 FramesFactory.getEME2000(), initDate, provider.getMu());
71
72 final BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, M2);
73
74 propagator.setupMatricesComputation(null, null, null);
75 });
76 }
77
78 @Test
79 public void testStateJacobian() {
80
81
82
83
84
85 AbsoluteDate initDate = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
86 Vector3D position = new Vector3D(3220103., 69623., 6149822.);
87 Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
88 Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(position, velocity),
89 FramesFactory.getEME2000(), initDate, provider.getMu());
90
91
92 BrouwerLyddanePropagator propagator = new BrouwerLyddanePropagator(initialOrbit, provider, M2);
93 final SpacecraftState initialState = propagator.getInitialState();
94 final double[] stateVector = new double[6];
95 OrbitType.CARTESIAN.mapOrbitToArray(initialState.getOrbit(), PositionAngleType.MEAN, stateVector, null);
96 final AbsoluteDate target = initialState.getDate().shiftedBy(initialState.getOrbit().getKeplerianPeriod());
97 MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, null);
98 final SpacecraftState finalState = propagator.propagate(target);
99 RealMatrix dYdY0 = harvester.getStateTransitionMatrix(finalState);
100 Assertions.assertEquals(OrbitType.CARTESIAN, harvester.getOrbitType());
101 Assertions.assertEquals(PositionAngleType.MEAN, harvester.getPositionAngleType());
102
103
104 double[][] dYdY0Ref = new double[6][6];
105 BrouwerLyddanePropagator propagator2;
106 double[] steps = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(initialState.getOrbit(), OrbitType.CARTESIAN)[0];
107 for (int i = 0; i < 6; ++i) {
108 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -4 * steps[i], i).getOrbit(), provider, M2);
109 SpacecraftState sM4h = propagator2.propagate(target);
110 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -3 * steps[i], i).getOrbit(), provider, M2);
111 SpacecraftState sM3h = propagator2.propagate(target);
112 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -2 * steps[i], i).getOrbit(), provider, M2);
113 SpacecraftState sM2h = propagator2.propagate(target);
114 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, -1 * steps[i], i).getOrbit(), provider, M2);
115 SpacecraftState sM1h = propagator2.propagate(target);
116 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +1 * steps[i], i).getOrbit(), provider, M2);
117 SpacecraftState sP1h = propagator2.propagate(target);
118 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +2 * steps[i], i).getOrbit(), provider, M2);
119 SpacecraftState sP2h = propagator2.propagate(target);
120 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +3 * steps[i], i).getOrbit(), provider, M2);
121 SpacecraftState sP3h = propagator2.propagate(target);
122 propagator2 = new BrouwerLyddanePropagator(shiftState(initialState, OrbitType.CARTESIAN, +4 * steps[i], i).getOrbit(), provider, M2);
123 SpacecraftState sP4h = propagator2.propagate(target);
124 fillJacobianColumn(dYdY0Ref, i, OrbitType.CARTESIAN, steps[i],
125 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
126 }
127
128
129 for (int i = 0; i < 6; ++i) {
130 for (int j = 0; j < 6; ++j) {
131 if (stateVector[i] != 0) {
132 double error = FastMath.abs((dYdY0.getEntry(i, j) - dYdY0Ref[i][j]) / stateVector[i]) * steps[j];
133 Assertions.assertEquals(0, error, 3.0e-13);
134 }
135 }
136 }
137
138 }
139
140 private void fillJacobianColumn(double[][] jacobian, int column,
141 OrbitType orbitType, double h,
142 SpacecraftState sM4h, SpacecraftState sM3h,
143 SpacecraftState sM2h, SpacecraftState sM1h,
144 SpacecraftState sP1h, SpacecraftState sP2h,
145 SpacecraftState sP3h, SpacecraftState sP4h) {
146 double[] aM4h = stateToArray(sM4h, orbitType)[0];
147 double[] aM3h = stateToArray(sM3h, orbitType)[0];
148 double[] aM2h = stateToArray(sM2h, orbitType)[0];
149 double[] aM1h = stateToArray(sM1h, orbitType)[0];
150 double[] aP1h = stateToArray(sP1h, orbitType)[0];
151 double[] aP2h = stateToArray(sP2h, orbitType)[0];
152 double[] aP3h = stateToArray(sP3h, orbitType)[0];
153 double[] aP4h = stateToArray(sP4h, orbitType)[0];
154 for (int i = 0; i < jacobian.length; ++i) {
155 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
156 32 * (aP3h[i] - aM3h[i]) -
157 168 * (aP2h[i] - aM2h[i]) +
158 672 * (aP1h[i] - aM1h[i])) / (840 * h);
159 }
160 }
161
162 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
163 double delta, int column) {
164
165 double[][] array = stateToArray(state, orbitType);
166 array[0][column] += delta;
167
168 return arrayToState(array, state.getFrame(), state.getDate(),
169 state.getOrbit().getMu(), state.getAttitude());
170
171 }
172
173 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
174 double[][] array = new double[2][6];
175
176 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngleType.MEAN, array[0], array[1]);
177 return array;
178 }
179
180 private SpacecraftState arrayToState(double[][] array,
181 Frame frame, AbsoluteDate date, double mu,
182 Attitude attitude) {
183 CartesianOrbit orbit = (CartesianOrbit) OrbitType.CARTESIAN.mapArrayToOrbit(array[0], array[1], PositionAngleType.MEAN, date, mu, frame);
184 return new SpacecraftState(orbit, attitude);
185 }
186
187
188 }