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