1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces.inertia;
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.geometry.euclidean.threed.FieldRotation;
24 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
25 import org.hipparchus.geometry.euclidean.threed.Vector3D;
26 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
27 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
28 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
29 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
30 import org.hipparchus.random.GaussianRandomGenerator;
31 import org.hipparchus.random.RandomGenerator;
32 import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
33 import org.hipparchus.random.Well19937a;
34 import org.hipparchus.util.FastMath;
35 import org.junit.jupiter.api.Assertions;
36 import org.junit.jupiter.api.BeforeEach;
37 import org.junit.jupiter.api.Test;
38 import org.orekit.Utils;
39 import org.orekit.errors.OrekitException;
40 import org.orekit.errors.OrekitIllegalArgumentException;
41 import org.orekit.errors.OrekitMessages;
42 import org.orekit.forces.AbstractLegacyForceModelTest;
43 import org.orekit.forces.ForceModel;
44 import org.orekit.frames.FieldTransform;
45 import org.orekit.frames.Frame;
46 import org.orekit.frames.FramesFactory;
47 import org.orekit.orbits.KeplerianOrbit;
48 import org.orekit.orbits.Orbit;
49 import org.orekit.orbits.PositionAngleType;
50 import org.orekit.propagation.FieldSpacecraftState;
51 import org.orekit.propagation.SpacecraftState;
52 import org.orekit.propagation.numerical.FieldNumericalPropagator;
53 import org.orekit.propagation.numerical.NumericalPropagator;
54 import org.orekit.time.AbsoluteDate;
55 import org.orekit.time.DateComponents;
56 import org.orekit.time.FieldAbsoluteDate;
57 import org.orekit.time.TimeComponents;
58 import org.orekit.time.TimeScalesFactory;
59 import org.orekit.utils.AbsolutePVCoordinates;
60 import org.orekit.utils.Constants;
61 import org.orekit.utils.FieldAbsolutePVCoordinates;
62 import org.orekit.utils.FieldPVCoordinates;
63 import org.orekit.utils.IERSConventions;
64 import org.orekit.utils.PVCoordinates;
65
66
67 public class InertialForcesTest extends AbstractLegacyForceModelTest {
68
69 @Override
70 protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel, final FieldSpacecraftState<DerivativeStructure> state) {
71 try {
72 final FieldVector3D<DerivativeStructure> position = state.getPVCoordinates().getPosition();
73 final FieldVector3D<DerivativeStructure> velocity = state.getPVCoordinates().getVelocity();
74 java.lang.reflect.Field refInertialFrameField = InertialForces.class.getDeclaredField("referenceInertialFrame");
75 refInertialFrameField.setAccessible(true);
76 Frame refInertialFrame = (Frame) refInertialFrameField.get(forceModel);
77
78 final FieldTransform<DerivativeStructure> inertToStateFrame = refInertialFrame.getTransformTo(state.getFrame(),
79 state.getDate());
80 final FieldVector3D<DerivativeStructure> a1 = inertToStateFrame.getCartesian().getAcceleration();
81 final FieldRotation<DerivativeStructure> r1 = inertToStateFrame.getAngular().getRotation();
82 final FieldVector3D<DerivativeStructure> o1 = inertToStateFrame.getAngular().getRotationRate();
83 final FieldVector3D<DerivativeStructure> oDot1 = inertToStateFrame.getAngular().getRotationAcceleration();
84
85 final FieldVector3D<DerivativeStructure> p2 = position;
86 final FieldVector3D<DerivativeStructure> v2 = velocity;
87
88 final FieldVector3D<DerivativeStructure> crossCrossP = FieldVector3D.crossProduct(o1, FieldVector3D.crossProduct(o1, p2));
89 final FieldVector3D<DerivativeStructure> crossV = FieldVector3D.crossProduct(o1, v2);
90 final FieldVector3D<DerivativeStructure> crossDotP = FieldVector3D.crossProduct(oDot1, p2);
91
92
93
94 return r1.applyTo(a1).subtract(new FieldVector3D<>(2, crossV, 1, crossCrossP, 1, crossDotP));
95
96 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
97 return null;
98 }
99 }
100
101 @Override
102 protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel, final FieldSpacecraftState<Gradient> state) {
103 try {
104 final FieldVector3D<Gradient> position = state.getPVCoordinates().getPosition();
105 final FieldVector3D<Gradient> velocity = state.getPVCoordinates().getVelocity();
106 java.lang.reflect.Field refInertialFrameField = InertialForces.class.getDeclaredField("referenceInertialFrame");
107 refInertialFrameField.setAccessible(true);
108 Frame refInertialFrame = (Frame) refInertialFrameField.get(forceModel);
109
110 final FieldTransform<Gradient> inertToStateFrame = refInertialFrame.getTransformTo(state.getFrame(),
111 state.getDate());
112 final FieldVector3D<Gradient> a1 = inertToStateFrame.getCartesian().getAcceleration();
113 final FieldRotation<Gradient> r1 = inertToStateFrame.getAngular().getRotation();
114 final FieldVector3D<Gradient> o1 = inertToStateFrame.getAngular().getRotationRate();
115 final FieldVector3D<Gradient> oDot1 = inertToStateFrame.getAngular().getRotationAcceleration();
116
117 final FieldVector3D<Gradient> p2 = position;
118 final FieldVector3D<Gradient> v2 = velocity;
119
120 final FieldVector3D<Gradient> crossCrossP = FieldVector3D.crossProduct(o1, FieldVector3D.crossProduct(o1, p2));
121 final FieldVector3D<Gradient> crossV = FieldVector3D.crossProduct(o1, v2);
122 final FieldVector3D<Gradient> crossDotP = FieldVector3D.crossProduct(oDot1, p2);
123
124
125
126 return r1.applyTo(a1).subtract(new FieldVector3D<>(2, crossV, 1, crossCrossP, 1, crossDotP));
127
128 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
129 return null;
130 }
131 }
132
133 @Test
134 void testJacobianVs80Implementation() {
135
136 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
137 new TimeComponents(13, 59, 27.816),
138 TimeScalesFactory.getUTC());
139 double i = FastMath.toRadians(98.7);
140 double omega = FastMath.toRadians(93.0);
141 double OMEGA = FastMath.toRadians(15.0 * 22.5);
142 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
143 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
144 Constants.EIGEN5C_EARTH_MU);
145 final AbsolutePVCoordinates pva = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getPVCoordinates());
146 final InertialForces forceModel = new InertialForces(pva.getFrame());
147 Assertions.assertFalse(forceModel.dependsOnPositionOnly());
148 checkStateJacobianVs80Implementation(new SpacecraftState(pva), forceModel,
149 Utils.defaultLaw(),
150 1.0e-50, false);
151 }
152
153 @Test
154 void testJacobianVs80ImplementationGradient() {
155
156 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
157 new TimeComponents(13, 59, 27.816),
158 TimeScalesFactory.getUTC());
159 double i = FastMath.toRadians(98.7);
160 double omega = FastMath.toRadians(93.0);
161 double OMEGA = FastMath.toRadians(15.0 * 22.5);
162 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
163 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
164 Constants.EIGEN5C_EARTH_MU);
165 final AbsolutePVCoordinates pva = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getPVCoordinates());
166 final InertialForces forceModel = new InertialForces(pva.getFrame());
167 Assertions.assertFalse(forceModel.dependsOnPositionOnly());
168 checkStateJacobianVs80ImplementationGradient(new SpacecraftState(pva), forceModel,
169 Utils.defaultLaw(),
170 1.0e-50, false);
171 }
172
173 @Test
174 public void RealFieldTest() {
175 DSFactory factory = new DSFactory(6, 5);
176 DerivativeStructure fpx = factory.variable(0, 0.8);
177 DerivativeStructure fpy = factory.variable(1, 0.2);
178 DerivativeStructure fpz = factory.variable(2, 0.0);
179 DerivativeStructure fvx = factory.variable(3, 0.0);
180 DerivativeStructure fvy = factory.variable(4, 0.0);
181 DerivativeStructure fvz = factory.variable(5, 0.1);
182
183 final FieldPVCoordinates<DerivativeStructure> initialConditions =
184 new FieldPVCoordinates<>(new FieldVector3D<>(fpx, fpy, fpz),
185 new FieldVector3D<>(fvx, fvy, fvz));
186
187 final double minStep = 0.00001;
188 final double maxstep = 3600.0;
189
190 Field<DerivativeStructure> field = fpx.getField();
191
192 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
193
194 Frame EME = FramesFactory.getEME2000();
195
196
197 final FieldAbsolutePVCoordinates<DerivativeStructure> initialAbsPV =
198 new FieldAbsolutePVCoordinates<>(EME, J2000, initialConditions);
199
200
201 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(initialAbsPV);
202
203 SpacecraftState iSR = initialState.toSpacecraftState();
204
205 final double positionTolerance = 0.01;
206 final double velocityTolerance = 0.01;
207 final double massTolerance = 1.0e-6;
208 final double[] vecAbsoluteTolerances =
209 {
210 positionTolerance, positionTolerance, positionTolerance,
211 velocityTolerance, velocityTolerance, velocityTolerance,
212 massTolerance
213 };
214 final double[] vecRelativeTolerances =
215 new double[vecAbsoluteTolerances.length];
216
217 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
218 new DormandPrince853FieldIntegrator<>(field,minStep, maxstep,
219 vecAbsoluteTolerances,
220 vecRelativeTolerances);
221 integrator.setInitialStepSize(60);
222 AdaptiveStepsizeIntegrator RIntegrator =
223 new DormandPrince853Integrator(minStep, maxstep,
224 vecAbsoluteTolerances,
225 vecRelativeTolerances);
226 RIntegrator.setInitialStepSize(60);
227
228 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
229 FNP.setOrbitType(null);
230 FNP.setIgnoreCentralAttraction(true);
231 FNP.setInitialState(initialState);
232
233 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
234 NP.setOrbitType(null);
235 NP.setIgnoreCentralAttraction(true);
236 NP.setInitialState(iSR);
237
238 final InertialForces forceModel = new InertialForces(EME);
239
240 FNP.addForceModel(forceModel);
241 NP.addForceModel(forceModel);
242
243
244 FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1000.);
245 FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
246 SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
247 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
248 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
249
250 Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
251 Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
252 Assertions.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
253
254 long number = 23091991;
255 RandomGenerator RG = new Well19937a(number);
256 GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
257 UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 },
258 new double[] {1e3, 0.01, 0.01, 0.01, 0.01, 0.01},
259 NGG);
260 double px_R = fpx.getReal();
261 double py_R = fpy.getReal();
262 double pz_R = fpz.getReal();
263 double vx_R = fvx.getReal();
264 double vy_R = fvy.getReal();
265 double vz_R = fvz.getReal();
266 double maxP = 0;
267 double maxV = 0;
268 double maxA = 0;
269 for (int ii = 0; ii < 1; ii++){
270 double[] rand_next = URVG.nextVector();
271 double px_shift = px_R + rand_next[0];
272 double py_shift = py_R + rand_next[1];
273 double pz_shift = pz_R + rand_next[2];
274 double vx_shift = vx_R + rand_next[3];
275 double vy_shift = vy_R + rand_next[4];
276 double vz_shift = vz_R + rand_next[5];
277
278 final PVCoordinates shiftedConditions =
279 new PVCoordinates(new Vector3D(px_shift, py_shift, pz_shift),
280 new Vector3D(vx_shift, vy_shift, vz_shift));
281
282 final AbsolutePVCoordinates shiftedAbsPV =
283 new AbsolutePVCoordinates(EME, J2000.toAbsoluteDate(), shiftedConditions);
284
285 SpacecraftState shift_iSR = new SpacecraftState(shiftedAbsPV);
286
287
288
289 NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator);
290
291 shift_NP.setInitialState(shift_iSR);
292
293 shift_NP.setOrbitType(null);
294 shift_NP.setIgnoreCentralAttraction(true);
295 shift_NP.addForceModel(forceModel);
296
297
298 SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate());
299
300
301 PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
302
303
304 FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
305 double x_DS = pos_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
306 double y_DS = pos_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
307 double z_DS = pos_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
308 double x = finPVC_shift.getPosition().getX();
309 double y = finPVC_shift.getPosition().getY();
310 double z = finPVC_shift.getPosition().getZ();
311 maxP = FastMath.max(maxP, FastMath.abs((x_DS - x) / (x - pos_DS.getX().getReal())));
312 maxP = FastMath.max(maxP, FastMath.abs((y_DS - y) / (y - pos_DS.getY().getReal())));
313 maxP = FastMath.max(maxP, FastMath.abs((z_DS - z) / (z - pos_DS.getZ().getReal())));
314
315
316 FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
317 double vx_DS = vel_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
318 double vy_DS = vel_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
319 double vz_DS = vel_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
320 double vx = finPVC_shift.getVelocity().getX();
321 double vy = finPVC_shift.getVelocity().getY();
322 double vz = finPVC_shift.getVelocity().getZ();
323 maxV = FastMath.max(maxV, FastMath.abs((vx_DS - vx) / vx));
324 maxV = FastMath.max(maxV, FastMath.abs((vy_DS - vy) / vy));
325 maxV = FastMath.max(maxV, FastMath.abs((vz_DS - vz) / vz));
326
327
328 FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
329 double ax_DS = acc_DS.getX().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
330 double ay_DS = acc_DS.getY().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
331 double az_DS = acc_DS.getZ().taylor(rand_next[0], rand_next[1], rand_next[2], rand_next[3], rand_next[4], rand_next[5]);
332 double ax = finPVC_shift.getAcceleration().getX();
333 double ay = finPVC_shift.getAcceleration().getY();
334 double az = finPVC_shift.getAcceleration().getZ();
335 if (ax != 0 || ay !=0 || az != 0) {
336 maxA = FastMath.max(maxA, FastMath.abs((ax_DS - ax) / ax));
337 maxA = FastMath.max(maxA, FastMath.abs((ay_DS - ay) / ay));
338 maxA = FastMath.max(maxA, FastMath.abs((az_DS - az) / az));
339 } else {
340 maxA = 0;
341 }
342 }
343 Assertions.assertEquals(0, maxP, 1.1e-14);
344 Assertions.assertEquals(0, maxV, 1.0e-15);
345 Assertions.assertEquals(0, maxA, 1.0e-15);
346 }
347
348 @Test
349 void testNoParametersDrivers() {
350 try {
351
352 AbsoluteDate date = new AbsoluteDate(new DateComponents(2003, 03, 01),
353 new TimeComponents(13, 59, 27.816),
354 TimeScalesFactory.getUTC());
355 double i = FastMath.toRadians(98.7);
356 double omega = FastMath.toRadians(93.0);
357 double OMEGA = FastMath.toRadians(15.0 * 22.5);
358 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
359 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date,
360 Constants.EIGEN5C_EARTH_MU);
361 final AbsolutePVCoordinates pva = new AbsolutePVCoordinates(orbit.getFrame(), orbit.getPVCoordinates());
362 final InertialForces forceModel = new InertialForces(pva.getFrame());
363 forceModel.getParameterDriver(" ");
364 } catch (OrekitException oe) {
365 Assertions.assertEquals(OrekitMessages.UNSUPPORTED_PARAMETER_NAME, oe.getSpecifier());
366 }
367 }
368
369 @Test
370 void testNonInertialFrame() {
371 try {
372
373 final Frame ecef = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
374
375 final InertialForces force = new InertialForces(ecef);
376 force.getParametersDrivers();
377 } catch (OrekitIllegalArgumentException oe) {
378 Assertions.assertEquals(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME_NOT_SUITABLE_AS_REFERENCE_FOR_INERTIAL_FORCES,
379 oe.getSpecifier());
380 }
381 }
382
383 @BeforeEach
384 public void setUp() {
385 Utils.setDataRoot("regular-data");
386 }
387
388 }