1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces.gravity;
18
19 import org.hipparchus.Field;
20 import org.hipparchus.analysis.differentiation.DSFactory;
21 import org.hipparchus.analysis.differentiation.DerivativeStructure;
22 import org.hipparchus.analysis.differentiation.Gradient;
23 import org.hipparchus.analysis.differentiation.GradientField;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
28 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
29 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
30 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
31 import org.hipparchus.util.FastMath;
32 import org.junit.jupiter.api.Assertions;
33 import org.junit.jupiter.api.BeforeEach;
34 import org.junit.jupiter.api.DisplayName;
35 import org.junit.jupiter.api.Test;
36 import org.mockito.Mockito;
37 import org.orekit.Utils;
38 import org.orekit.attitudes.LofOffset;
39 import org.orekit.bodies.CelestialBody;
40 import org.orekit.bodies.CelestialBodyFactory;
41 import org.orekit.forces.AbstractLegacyForceModelTest;
42 import org.orekit.forces.ForceModel;
43 import org.orekit.frames.Frame;
44 import org.orekit.frames.FramesFactory;
45 import org.orekit.frames.LOFType;
46 import org.orekit.orbits.CartesianOrbit;
47 import org.orekit.orbits.FieldKeplerianOrbit;
48 import org.orekit.orbits.KeplerianOrbit;
49 import org.orekit.orbits.Orbit;
50 import org.orekit.orbits.OrbitType;
51 import org.orekit.orbits.PositionAngleType;
52 import org.orekit.propagation.FieldSpacecraftState;
53 import org.orekit.propagation.SpacecraftState;
54 import org.orekit.propagation.ToleranceProvider;
55 import org.orekit.propagation.numerical.FieldNumericalPropagator;
56 import org.orekit.propagation.numerical.NumericalPropagator;
57 import org.orekit.time.AbsoluteDate;
58 import org.orekit.time.DateComponents;
59 import org.orekit.time.FieldAbsoluteDate;
60 import org.orekit.time.TimeComponents;
61 import org.orekit.time.TimeScalesFactory;
62 import org.orekit.utils.Constants;
63 import org.orekit.utils.FieldPVCoordinates;
64 import org.orekit.utils.PVCoordinates;
65
66 public class SingleBodyAbsoluteAttractionTest extends AbstractLegacyForceModelTest {
67
68 @BeforeEach
69 public void setUp() {
70 Utils.setDataRoot("regular-data");
71 }
72
73 @Override
74 protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel, final FieldSpacecraftState<DerivativeStructure> state) {
75
76 try {
77 final AbsoluteDate date = state.getDate().toAbsoluteDate();
78 final FieldVector3D<DerivativeStructure> position = state.getPVCoordinates().getPosition();
79 java.lang.reflect.Field bodyField = AbstractBodyAttraction.class.getDeclaredField("positionProvider");
80 bodyField.setAccessible(true);
81 CelestialBody body = (CelestialBody) bodyField.get(forceModel);
82 double gm = forceModel.
83 getParameterDriver(body.getName() + SingleBodyAbsoluteAttraction.ATTRACTION_COEFFICIENT_SUFFIX).
84 getValue(date);
85
86
87 final Vector3D centralToBody = body.getPosition(date, state.getFrame());
88 final FieldVector3D<DerivativeStructure> satToBody = position.subtract(centralToBody).negate();
89 final DerivativeStructure r2Sat = satToBody.getNormSq();
90
91
92 final FieldVector3D<DerivativeStructure> satAcc =
93 new FieldVector3D<>(r2Sat.sqrt().multiply(r2Sat).reciprocal().multiply(gm), satToBody);
94 return satAcc;
95
96
97 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
98 return null;
99 }
100
101 }
102
103 @Override
104 protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel, final FieldSpacecraftState<Gradient> state) {
105
106 try {
107 final AbsoluteDate date = state.getDate().toAbsoluteDate();
108 final FieldVector3D<Gradient> position = state.getPVCoordinates().getPosition();
109 java.lang.reflect.Field bodyField = AbstractBodyAttraction.class.getDeclaredField("positionProvider");
110 bodyField.setAccessible(true);
111 CelestialBody body = (CelestialBody) bodyField.get(forceModel);
112 double gm = forceModel.
113 getParameterDriver(body.getName() + SingleBodyAbsoluteAttraction.ATTRACTION_COEFFICIENT_SUFFIX).
114 getValue(date);
115
116
117 final Vector3D centralToBody = body.getPosition(date, state.getFrame());
118 final FieldVector3D<Gradient> satToBody = position.subtract(centralToBody).negate();
119 final Gradient r2Sat = satToBody.getNormSq();
120
121
122 final FieldVector3D<Gradient> satAcc =
123 new FieldVector3D<>(r2Sat.sqrt().multiply(r2Sat).reciprocal().multiply(gm), satToBody);
124 return satAcc;
125
126
127 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
128 return null;
129 }
130
131 }
132
133 @Test
134 public void RealFieldTest() {
135 DSFactory factory = new DSFactory(6, 5);
136 DerivativeStructure a_0 = factory.variable(0, 7e7);
137 DerivativeStructure e_0 = factory.variable(1, 0.4);
138 DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
139 DerivativeStructure R_0 = factory.variable(3, 0.7);
140 DerivativeStructure O_0 = factory.variable(4, 0.5);
141 DerivativeStructure n_0 = factory.variable(5, 0.1);
142 DerivativeStructure mu = factory.constant(Constants.EIGEN5C_EARTH_MU);
143
144 Field<DerivativeStructure> field = a_0.getField();
145
146 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
147
148 Frame EME = FramesFactory.getEME2000();
149
150 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
151 PositionAngleType.MEAN,
152 EME,
153 J2000,
154 mu);
155
156 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
157
158 SpacecraftState iSR = initialState.toSpacecraftState();
159 OrbitType type = OrbitType.KEPLERIAN;
160 double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
161
162
163 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
164 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
165 integrator.setInitialStepSize(60);
166 AdaptiveStepsizeIntegrator RIntegrator =
167 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
168 RIntegrator.setInitialStepSize(60);
169
170 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
171 FNP.setOrbitType(type);
172 FNP.setInitialState(initialState);
173
174 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
175 NP.setOrbitType(type);
176 NP.setInitialState(iSR);
177
178 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(CelestialBodyFactory.getSun());
179
180 FNP.addForceModel(forceModel);
181 NP.addForceModel(forceModel);
182
183
184 checkRealFieldPropagation(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
185 1.0e-16, 5.0e-09, 3.0e-10, 7.0e-10,
186 1, false);
187 }
188
189 @Test
190 public void RealFieldGradientTest() {
191 int freeParameters = 6;
192 Gradient a_0 = Gradient.variable(freeParameters, 0, 7e7);
193 Gradient e_0 = Gradient.variable(freeParameters, 1, 0.4);
194 Gradient i_0 = Gradient.variable(freeParameters, 2, 85 * FastMath.PI / 180);
195 Gradient R_0 = Gradient.variable(freeParameters, 3, 0.7);
196 Gradient O_0 = Gradient.variable(freeParameters, 4, 0.5);
197 Gradient n_0 = Gradient.variable(freeParameters, 5, 0.1);
198 Gradient mu = Gradient.constant(freeParameters, Constants.EIGEN5C_EARTH_MU);
199
200 Field<Gradient> field = a_0.getField();
201
202 FieldAbsoluteDate<Gradient> J2000 = new FieldAbsoluteDate<>(field);
203
204 Frame EME = FramesFactory.getEME2000();
205
206 FieldKeplerianOrbit<Gradient> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
207 PositionAngleType.MEAN,
208 EME,
209 J2000,
210 mu);
211
212 FieldSpacecraftState<Gradient> initialState = new FieldSpacecraftState<>(FKO);
213
214 SpacecraftState iSR = initialState.toSpacecraftState();
215 OrbitType type = OrbitType.KEPLERIAN;
216 double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
217
218
219 AdaptiveStepsizeFieldIntegrator<Gradient> integrator =
220 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
221 integrator.setInitialStepSize(60);
222 AdaptiveStepsizeIntegrator RIntegrator =
223 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
224 RIntegrator.setInitialStepSize(60);
225
226 FieldNumericalPropagator<Gradient> FNP = new FieldNumericalPropagator<>(field, integrator);
227 FNP.setOrbitType(type);
228 FNP.setInitialState(initialState);
229
230 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
231 NP.setOrbitType(type);
232 NP.setInitialState(iSR);
233
234 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(CelestialBodyFactory.getSun());
235
236 FNP.addForceModel(forceModel);
237 NP.addForceModel(forceModel);
238
239
240 checkRealFieldPropagationGradient(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
241 1.0e-16, 1.3e-02, 2.9e-4, 1.3e-3,
242 1, false);
243 }
244
245 @Test
246 public void RealFieldExpectErrorTest() {
247 DSFactory factory = new DSFactory(6, 5);
248 DerivativeStructure a_0 = factory.variable(0, 7e7);
249 DerivativeStructure e_0 = factory.variable(1, 0.4);
250 DerivativeStructure i_0 = factory.variable(2, 85 * FastMath.PI / 180);
251 DerivativeStructure R_0 = factory.variable(3, 0.7);
252 DerivativeStructure O_0 = factory.variable(4, 0.5);
253 DerivativeStructure n_0 = factory.variable(5, 0.1);
254
255 Field<DerivativeStructure> field = a_0.getField();
256 DerivativeStructure zero = field.getZero();
257
258 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
259
260 Frame EME = FramesFactory.getEME2000();
261
262 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
263 PositionAngleType.MEAN,
264 EME,
265 J2000,
266 zero.add(Constants.EIGEN5C_EARTH_MU));
267
268 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
269
270 SpacecraftState iSR = initialState.toSpacecraftState();
271 OrbitType type = OrbitType.KEPLERIAN;
272 double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(0.001).getTolerances(FKO.toOrbit(), type);
273
274
275 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
276 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
277 integrator.setInitialStepSize(60);
278 AdaptiveStepsizeIntegrator RIntegrator =
279 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
280 RIntegrator.setInitialStepSize(60);
281
282 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
283 FNP.setOrbitType(type);
284 FNP.setInitialState(initialState);
285
286 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
287 NP.setOrbitType(type);
288 NP.setInitialState(iSR);
289
290 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(CelestialBodyFactory.getSun());
291
292 FNP.addForceModel(forceModel);
293
294
295 FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
296 FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
297 SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
298 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
299 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
300
301 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
302 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
303 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
304 }
305
306 @Test
307 void testParameterDerivative() {
308
309 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
310 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
311 final SpacecraftState state =
312 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
313 FramesFactory.getGCRF(),
314 new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
315 Constants.EIGEN5C_EARTH_MU));
316
317 final CelestialBody moon = CelestialBodyFactory.getMoon();
318 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
319 Assertions.assertTrue(forceModel.dependsOnPositionOnly());
320 final String name = moon.getName() + SingleBodyAbsoluteAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
321 checkParameterDerivative(state, forceModel, name, 1.0, 7.0e-15);
322
323 }
324
325 @Test
326 void testParameterDerivativeGradient() {
327
328 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
329 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
330 final SpacecraftState state =
331 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
332 FramesFactory.getGCRF(),
333 new AbsoluteDate(2003, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
334 Constants.EIGEN5C_EARTH_MU));
335
336 final CelestialBody moon = CelestialBodyFactory.getMoon();
337 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
338 Assertions.assertTrue(forceModel.dependsOnPositionOnly());
339 final String name = moon.getName() + SingleBodyAbsoluteAttraction.ATTRACTION_COEFFICIENT_SUFFIX;
340 checkParameterDerivativeGradient(state, forceModel, name, 1.0, 7.0e-15);
341
342 }
343
344 @Test
345 void testJacobianVs80Implementation() {
346
347 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
348 new TimeComponents(13, 59, 27.816),
349 TimeScalesFactory.getUTC());
350 double i = FastMath.toRadians(98.7);
351 double omega = FastMath.toRadians(93.0);
352 double OMEGA = FastMath.toRadians(15.0 * 22.5);
353 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
354 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
355 Constants.EIGEN5C_EARTH_MU);
356 final CelestialBody moon = CelestialBodyFactory.getMoon();
357 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
358 checkStateJacobianVs80Implementation(new SpacecraftState(orbit), forceModel,
359 new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
360 1.0e-15, false);
361 }
362
363 @Test
364 void testJacobianVs80ImplementationGradient() {
365
366 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
367 new TimeComponents(13, 59, 27.816),
368 TimeScalesFactory.getUTC());
369 double i = FastMath.toRadians(98.7);
370 double omega = FastMath.toRadians(93.0);
371 double OMEGA = FastMath.toRadians(15.0 * 22.5);
372 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
373 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
374 Constants.EIGEN5C_EARTH_MU);
375 final CelestialBody moon = CelestialBodyFactory.getMoon();
376 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
377 checkStateJacobianVs80ImplementationGradient(new SpacecraftState(orbit), forceModel,
378 new LofOffset(orbit.getFrame(), LOFType.LVLH_CCSDS),
379 1.0e-15, false);
380 }
381
382 @Test
383 void testGlobalStateJacobian()
384 {
385
386
387 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
388 new TimeComponents(13, 59, 27.816),
389 TimeScalesFactory.getUTC());
390 double i = FastMath.toRadians(98.7);
391 double omega = FastMath.toRadians(93.0);
392 double OMEGA = FastMath.toRadians(15.0 * 22.5);
393 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
394 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
395 Constants.EIGEN5C_EARTH_MU);
396 OrbitType integrationType = OrbitType.CARTESIAN;
397 double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(0.01).getTolerances(orbit, integrationType);
398
399 NumericalPropagator propagator =
400 new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
401 tolerances[0], tolerances[1]));
402 propagator.setOrbitType(integrationType);
403 final CelestialBody moon = CelestialBodyFactory.getMoon();
404 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
405 propagator.addForceModel(forceModel);
406 SpacecraftState state0 = new SpacecraftState(orbit);
407
408 checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
409 1e4, tolerances[0], 2.0e-9);
410
411 }
412
413
414 @Test
415 @DisplayName("Test that acceleration derivatives with respect to absolute date are not equal to zero.")
416 void testIssue1070() {
417
418
419 final int freeParameters = 1;
420 final GradientField field = GradientField.getField(freeParameters);
421 final Gradient zero = field.getZero();
422 final Gradient variable = Gradient.variable(freeParameters, 0, 0.);
423 final FieldAbsoluteDate<Gradient> fieldAbsoluteDate = new FieldAbsoluteDate<>(field, AbsoluteDate.ARBITRARY_EPOCH).
424 shiftedBy(variable);
425
426
427 @SuppressWarnings("unchecked")
428 final FieldSpacecraftState<Gradient> stateMock = Mockito.mock(FieldSpacecraftState.class);
429 Mockito.when(stateMock.getDate()).thenReturn(fieldAbsoluteDate);
430 Mockito.when(stateMock.getPosition()).thenReturn(new FieldVector3D<>(zero, zero));
431 Mockito.when(stateMock.getFrame()).thenReturn(FramesFactory.getGCRF());
432
433
434 final CelestialBody moon = CelestialBodyFactory.getMoon();
435 final SingleBodyAbsoluteAttraction forceModel = new SingleBodyAbsoluteAttraction(moon);
436
437
438 final Gradient gm = zero.add(moon.getGM());
439 final FieldVector3D<Gradient> accelerationVector = forceModel.acceleration(stateMock, new Gradient[] { gm });
440
441
442 final double[] derivatives = accelerationVector.getNormSq().getGradient();
443 Assertions.assertNotEquals(0., MatrixUtils.createRealVector(derivatives).getNorm());
444 }
445
446 }