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.hipparchus.geometry.euclidean.threed.Rotation;
20 import org.hipparchus.geometry.euclidean.threed.Vector3D;
21 import org.hipparchus.linear.RealMatrix;
22 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
23 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
24 import org.hipparchus.util.FastMath;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.BeforeEach;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.Utils;
29 import org.orekit.attitudes.Attitude;
30 import org.orekit.attitudes.AttitudeProvider;
31 import org.orekit.attitudes.FrameAlignedProvider;
32 import org.orekit.bodies.CelestialBodyFactory;
33 import org.orekit.bodies.OneAxisEllipsoid;
34 import org.orekit.forces.ForceModel;
35 import org.orekit.forces.drag.DragForce;
36 import org.orekit.forces.drag.DragSensitive;
37 import org.orekit.forces.drag.IsotropicDrag;
38 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
39 import org.orekit.forces.gravity.NewtonianAttraction;
40 import org.orekit.forces.gravity.potential.GravityFieldFactory;
41 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
42 import org.orekit.forces.gravity.potential.SHMFormatReader;
43 import org.orekit.forces.maneuvers.ConstantThrustManeuver;
44 import org.orekit.frames.Frame;
45 import org.orekit.frames.FramesFactory;
46 import org.orekit.models.earth.atmosphere.HarrisPriester;
47 import org.orekit.orbits.KeplerianOrbit;
48 import org.orekit.orbits.Orbit;
49 import org.orekit.orbits.OrbitType;
50 import org.orekit.orbits.PositionAngleType;
51 import org.orekit.propagation.SpacecraftState;
52 import org.orekit.propagation.ToleranceProvider;
53 import org.orekit.time.AbsoluteDate;
54 import org.orekit.time.DateComponents;
55 import org.orekit.time.TimeComponents;
56 import org.orekit.time.TimeScalesFactory;
57 import org.orekit.utils.Constants;
58 import org.orekit.utils.IERSConventions;
59 import org.orekit.utils.ParameterDriver;
60 import org.orekit.utils.ParameterDriversList;
61
62 import java.io.IOException;
63 import java.text.ParseException;
64
65
66 public class IntegrableJacobianColumnGeneratorTest {
67
68 @BeforeEach
69 public void setUp() {
70 Utils.setDataRoot("regular-data:potential/shm-format");
71 GravityFieldFactory.addPotentialCoefficientsReader(new SHMFormatReader("^eigen_cg03c_coef$", false));
72 }
73
74 @Test
75 public void testDragParametersDerivatives() throws ParseException, IOException {
76 doTestParametersDerivatives(DragSensitive.DRAG_COEFFICIENT, 2.4e-3, OrbitType.values());
77 }
78
79 @Test
80 public void testMuParametersDerivatives() throws ParseException, IOException {
81
82
83
84 doTestParametersDerivatives(NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT, 5e-7,
85 OrbitType.CARTESIAN);
86 }
87
88 private void doTestParametersDerivatives(String parameterName, double tolerance,
89 OrbitType... orbitTypes) {
90
91 OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
92 Constants.WGS84_EARTH_FLATTENING,
93 FramesFactory.getITRF(IERSConventions.IERS_2010, true));
94 ForceModel drag = new DragForce(new HarrisPriester(CelestialBodyFactory.getSun(), earth),
95 new IsotropicDrag(2.5, 1.2));
96
97 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(5, 5);
98 ForceModel gravityField =
99 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider);
100 Orbit baseOrbit =
101 new KeplerianOrbit(7000000.0, 0.01, 0.1, 0.7, 0, 1.2, PositionAngleType.TRUE,
102 FramesFactory.getEME2000(), AbsoluteDate.J2000_EPOCH,
103 provider.getMu());
104
105 double dt = 900;
106 double dP = 1.0;
107 for (OrbitType orbitType : orbitTypes) {
108 final Orbit initialOrbit = orbitType.convertType(baseOrbit);
109 for (PositionAngleType angleType : PositionAngleType.values()) {
110
111 NumericalPropagator propagator =
112 setUpPropagator(initialOrbit, dP, orbitType, angleType, drag, gravityField);
113 propagator.setMu(provider.getMu());
114 ParameterDriver selected = null;
115 for (final ForceModel forceModel : propagator.getAllForceModels()) {
116 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
117 driver.setValue(driver.getReferenceValue(), initialOrbit.getDate());
118 if (driver.getName().equals(parameterName)) {
119 driver.setSelected(true);
120 selected = driver;
121 } else {
122 driver.setSelected(false);
123 }
124 }
125 }
126
127 SpacecraftState initialState = new SpacecraftState(initialOrbit);
128 propagator.setInitialState(initialState);
129 PickUpHandler pickUp = new PickUpHandler(propagator, null, null, selected.getNameSpan(new AbsoluteDate()));
130 propagator.setStepHandler(pickUp);
131 propagator.propagate(initialState.getDate().shiftedBy(dt));
132 RealMatrix dYdP = pickUp.getdYdP();
133
134
135 double[][] dYdPRef = new double[6][1];
136 NumericalPropagator propagator2 = setUpPropagator(initialOrbit, dP, orbitType, angleType,
137 drag, gravityField);
138 propagator2.setMu(provider.getMu());
139 ParameterDriversList bound = new ParameterDriversList();
140 for (final ForceModel forceModel : propagator2.getAllForceModels()) {
141 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
142 if (driver.getName().equals(parameterName)) {
143 driver.setSelected(true);
144 bound.add(driver);
145 } else {
146 driver.setSelected(false);
147 }
148 }
149 }
150 ParameterDriver selected2 = bound.getDrivers().get(0);
151 double p0 = selected2.getReferenceValue();
152 double h = selected2.getScale();
153 selected2.setValue(p0 - 4 * h);
154 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
155 orbitType, angleType,
156 initialState.getFrame(), initialState.getDate(),
157 propagator2.getMu(),
158 initialState.getAttitude()));
159 SpacecraftState sM4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
160 selected2.setValue(p0 - 3 * h);
161 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
162 orbitType, angleType,
163 initialState.getFrame(), initialState.getDate(),
164 propagator2.getMu(),
165 initialState.getAttitude()));
166 SpacecraftState sM3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
167 selected2.setValue(p0 - 2 * h);
168 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
169 orbitType, angleType,
170 initialState.getFrame(), initialState.getDate(),
171 propagator2.getMu(),
172 initialState.getAttitude()));
173 SpacecraftState sM2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
174 selected2.setValue(p0 - 1 * h);
175 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
176 orbitType, angleType,
177 initialState.getFrame(), initialState.getDate(),
178 propagator2.getMu(),
179 initialState.getAttitude()));
180 SpacecraftState sM1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
181 selected2.setValue(p0 + 1 * h);
182 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
183 orbitType, angleType,
184 initialState.getFrame(), initialState.getDate(),
185 propagator2.getMu(),
186 initialState.getAttitude()));
187 SpacecraftState sP1h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
188 selected2.setValue(p0 + 2 * h);
189 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
190 orbitType, angleType,
191 initialState.getFrame(), initialState.getDate(),
192 propagator2.getMu(),
193 initialState.getAttitude()));
194 SpacecraftState sP2h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
195 selected2.setValue(p0 + 3 * h);
196 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
197 orbitType, angleType,
198 initialState.getFrame(), initialState.getDate(),
199 propagator2.getMu(),
200 initialState.getAttitude()));
201 SpacecraftState sP3h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
202 selected2.setValue(p0 + 4 * h);
203 propagator2.resetInitialState(arrayToState(stateToArray(initialState, orbitType, angleType, true),
204 orbitType, angleType,
205 initialState.getFrame(), initialState.getDate(),
206 propagator2.getMu(),
207 initialState.getAttitude()));
208 SpacecraftState sP4h = propagator2.propagate(initialOrbit.getDate().shiftedBy(dt));
209 fillJacobianColumn(dYdPRef, 0, orbitType, angleType, h,
210 sM4h, sM3h, sM2h, sM1h, sP1h, sP2h, sP3h, sP4h);
211
212 for (int i = 0; i < 6; ++i) {
213 Assertions.assertEquals(dYdPRef[i][0], dYdP.getEntry(i, 0), FastMath.abs(dYdPRef[i][0] * tolerance));
214 }
215
216 }
217 }
218
219 }
220
221 @Test
222 public void testJacobianIssue18() {
223
224
225 final double mu = 3.9860047e14;
226
227 final double isp = 318;
228 final double mass = 2500;
229 final double a = 24396159;
230 final double e = 0.72831215;
231 final double i = FastMath.toRadians(7);
232 final double omega = FastMath.toRadians(180);
233 final double OMEGA = FastMath.toRadians(261);
234 final double lv = 0;
235
236 final double duration = 3653.99;
237 final double f = 420;
238 final double delta = FastMath.toRadians(-7.4978);
239 final double alpha = FastMath.toRadians(351);
240 final AttitudeProvider law = new FrameAlignedProvider(new Rotation(new Vector3D(alpha, delta),
241 Vector3D.PLUS_I));
242
243 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2004, 01, 01),
244 new TimeComponents(23, 30, 00.000),
245 TimeScalesFactory.getUTC());
246 final Orbit orbit =
247 new KeplerianOrbit(a, e, i, omega, OMEGA, lv, PositionAngleType.TRUE,
248 FramesFactory.getEME2000(), initDate, mu);
249 SpacecraftState initialState =
250 new SpacecraftState(orbit, law.getAttitude(orbit, orbit.getDate(), orbit.getFrame()), mass);
251
252 final AbsoluteDate fireDate = new AbsoluteDate(new DateComponents(2004, 01, 02),
253 new TimeComponents(04, 15, 34.080),
254 TimeScalesFactory.getUTC());
255 final ConstantThrustManeuver maneuver =
256 new ConstantThrustManeuver(fireDate, duration, f, isp, Vector3D.PLUS_I);
257
258 double[] absTolerance = {
259 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
260 };
261 double[] relTolerance = {
262 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
263 };
264 AdaptiveStepsizeIntegrator integrator =
265 new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
266 integrator.setInitialStepSize(60);
267 final NumericalPropagator propagator = new NumericalPropagator(integrator);
268
269 propagator.setAttitudeProvider(law);
270 propagator.addForceModel(maneuver);
271 ParameterDriver selected = maneuver.getParameterDriver("thrust");
272 selected.setSelected(true);
273
274 final OrbitType orbitType = OrbitType.CARTESIAN;
275 final PositionAngleType angleType = PositionAngleType.TRUE;
276 propagator.setOrbitType(orbitType);
277 StateTransitionMatrixGenerator stmGenerator =
278 new StateTransitionMatrixGenerator("stm",
279 propagator.getAllForceModels(),
280 propagator.getAttitudeProvider());
281 IntegrableJacobianColumnGenerator columnGenerator = new IntegrableJacobianColumnGenerator(stmGenerator, selected.getName());
282 propagator.addAdditionalDerivativesProvider(columnGenerator);
283 propagator.addAdditionalDerivativesProvider(stmGenerator);
284
285 initialState = stmGenerator.setInitialStateTransitionMatrix(initialState, null, orbitType, angleType);
286 initialState = initialState.addAdditionalData(columnGenerator.getName(), new double[6]);
287 propagator.setInitialState(initialState);
288
289 final AbsoluteDate finalDate = fireDate.shiftedBy(3800);
290 final SpacecraftState finalorb = propagator.propagate(finalDate);
291 Assertions.assertEquals(0, finalDate.durationFrom(finalorb.getDate()), 1.0e-11);
292
293 }
294
295 private void fillJacobianColumn(double[][] jacobian, int column,
296 OrbitType orbitType, PositionAngleType angleType, double h,
297 SpacecraftState sM4h, SpacecraftState sM3h,
298 SpacecraftState sM2h, SpacecraftState sM1h,
299 SpacecraftState sP1h, SpacecraftState sP2h,
300 SpacecraftState sP3h, SpacecraftState sP4h) {
301 boolean withMass = jacobian.length > 6;
302 double[] aM4h = stateToArray(sM4h, orbitType, angleType, withMass)[0];
303 double[] aM3h = stateToArray(sM3h, orbitType, angleType, withMass)[0];
304 double[] aM2h = stateToArray(sM2h, orbitType, angleType, withMass)[0];
305 double[] aM1h = stateToArray(sM1h, orbitType, angleType, withMass)[0];
306 double[] aP1h = stateToArray(sP1h, orbitType, angleType, withMass)[0];
307 double[] aP2h = stateToArray(sP2h, orbitType, angleType, withMass)[0];
308 double[] aP3h = stateToArray(sP3h, orbitType, angleType, withMass)[0];
309 double[] aP4h = stateToArray(sP4h, orbitType, angleType, withMass)[0];
310 for (int i = 0; i < jacobian.length; ++i) {
311 jacobian[i][column] = ( -3 * (aP4h[i] - aM4h[i]) +
312 32 * (aP3h[i] - aM3h[i]) -
313 168 * (aP2h[i] - aM2h[i]) +
314 672 * (aP1h[i] - aM1h[i])) / (840 * h);
315 }
316 }
317
318 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType, PositionAngleType angleType,
319 boolean withMass) {
320 double[][] array = new double[2][withMass ? 7 : 6];
321 orbitType.mapOrbitToArray(state.getOrbit(), angleType, array[0], array[1]);
322 if (withMass) {
323 array[0][6] = state.getMass();
324 }
325 return array;
326 }
327
328 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType, PositionAngleType angleType,
329 Frame frame, AbsoluteDate date, double mu,
330 Attitude attitude) {
331 Orbit orbit = orbitType.mapArrayToOrbit(array[0], array[1], angleType, date, mu, frame);
332 return (array.length > 6) ?
333 new SpacecraftState(orbit, attitude) :
334 new SpacecraftState(orbit, attitude, array[0][6]);
335 }
336
337 private NumericalPropagator setUpPropagator(Orbit orbit, double dP,
338 OrbitType orbitType, PositionAngleType angleType,
339 ForceModel... models)
340 {
341
342 final double minStep = 0.001;
343 final double maxStep = 1000;
344
345 double[][] tol = ToleranceProvider.getDefaultToleranceProvider(dP).getTolerances(orbit, orbitType);
346 NumericalPropagator propagator =
347 new NumericalPropagator(new DormandPrince853Integrator(minStep, maxStep, tol[0], tol[1]));
348 propagator.setOrbitType(orbitType);
349 propagator.setPositionAngleType(angleType);
350 for (ForceModel model : models) {
351 propagator.addForceModel(model);
352 }
353 return propagator;
354 }
355
356 }