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