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