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 java.io.Serializable;
20 import java.lang.reflect.InvocationTargetException;
21
22 import org.hipparchus.CalculusFieldElement;
23 import org.hipparchus.Field;
24 import org.hipparchus.analysis.differentiation.*;
25 import org.hipparchus.dfp.Dfp;
26 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27 import org.hipparchus.geometry.euclidean.threed.Rotation;
28 import org.hipparchus.geometry.euclidean.threed.SphericalCoordinates;
29 import org.hipparchus.geometry.euclidean.threed.Vector3D;
30 import org.hipparchus.linear.Array2DRowRealMatrix;
31 import org.hipparchus.linear.MatrixUtils;
32 import org.hipparchus.linear.RealMatrix;
33 import org.hipparchus.ode.AbstractIntegrator;
34 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
35 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
36 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
37 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
38 import org.hipparchus.util.FastMath;
39 import org.hipparchus.util.MathArrays;
40 import org.junit.jupiter.api.AfterEach;
41 import org.junit.jupiter.api.Assertions;
42 import org.junit.jupiter.api.BeforeEach;
43 import org.junit.jupiter.api.DisplayName;
44 import org.junit.jupiter.api.Test;
45 import org.mockito.Mockito;
46 import org.orekit.Utils;
47 import org.orekit.attitudes.Attitude;
48 import org.orekit.attitudes.LofOffset;
49 import org.orekit.bodies.CelestialBodyFactory;
50 import org.orekit.errors.OrekitException;
51 import org.orekit.forces.AbstractLegacyForceModelTest;
52 import org.orekit.forces.ForceModel;
53 import org.orekit.forces.gravity.potential.GRGSFormatReader;
54 import org.orekit.forces.gravity.potential.GravityFieldFactory;
55 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
56 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
57 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider.NormalizedSphericalHarmonics;
58 import org.orekit.forces.gravity.potential.TideSystem;
59 import org.orekit.frames.Frame;
60 import org.orekit.frames.FramesFactory;
61 import org.orekit.frames.LOFType;
62 import org.orekit.frames.StaticTransform;
63 import org.orekit.frames.Transform;
64 import org.orekit.orbits.*;
65 import org.orekit.propagation.*;
66 import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
67 import org.orekit.propagation.numerical.FieldNumericalPropagator;
68 import org.orekit.propagation.numerical.NumericalPropagator;
69 import org.orekit.propagation.sampling.OrekitFixedStepHandler;
70 import org.orekit.time.AbsoluteDate;
71 import org.orekit.time.DateComponents;
72 import org.orekit.time.FieldAbsoluteDate;
73 import org.orekit.time.TimeComponents;
74 import org.orekit.time.TimeScalesFactory;
75 import org.orekit.utils.*;
76
77
78 public class HolmesFeatherstoneAttractionModelTest extends AbstractLegacyForceModelTest {
79
80 private static final int SCALING = 930;
81
82 @Override
83 protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel,
84 final FieldSpacecraftState<DerivativeStructure> state) {
85 try {
86 final AbsoluteDate date = state.getDate().toAbsoluteDate();
87 final FieldVector3D<DerivativeStructure> position = state.getPVCoordinates().getPosition();
88 java.lang.reflect.Field bodyFrameField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("bodyFrame");
89 bodyFrameField.setAccessible(true);
90 Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
91
92
93 final StaticTransform fromBodyFrame = bodyFrame.getStaticTransformTo(state.getFrame(), date);
94 final StaticTransform toBodyFrame = fromBodyFrame.getInverse();
95 final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
96
97
98 final GradientHessian gh = gradientHessian((HolmesFeatherstoneAttractionModel) forceModel,
99 date, positionBody);
100
101
102 final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
103
104
105 final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
106 final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
107 final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
108
109
110 final double[] derivatives = new double[1 + state.getMass().getFreeParameters()];
111 final DerivativeStructure[] accDer = new DerivativeStructure[3];
112 for (int i = 0; i < 3; ++i) {
113
114
115 derivatives[0] = gInertial[i];
116
117
118 derivatives[1] = hInertial.getEntry(i, 0);
119 derivatives[2] = hInertial.getEntry(i, 1);
120 derivatives[3] = hInertial.getEntry(i, 2);
121
122
123
124 accDer[i] = state.getMass().getFactory().build(derivatives);
125
126 }
127
128 return new FieldVector3D<>(accDer);
129
130 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
131 return null;
132 }
133 }
134
135 @Override
136 protected FieldVector3D<Gradient> accelerationDerivativesGradient(final ForceModel forceModel,
137 final FieldSpacecraftState<Gradient> state) {
138 try {
139 final AbsoluteDate date = state.getDate().toAbsoluteDate();
140 final FieldVector3D<Gradient> position = state.getPVCoordinates().getPosition();
141 java.lang.reflect.Field bodyFrameField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("bodyFrame");
142 bodyFrameField.setAccessible(true);
143 Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
144
145
146 final Transform fromBodyFrame = bodyFrame.getTransformTo(state.getFrame(), date);
147 final Transform toBodyFrame = fromBodyFrame.getInverse();
148 final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
149
150
151 final GradientHessian gh = gradientHessian((HolmesFeatherstoneAttractionModel) forceModel,
152 date, positionBody);
153
154
155 final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
156
157
158 final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
159 final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
160 final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
161
162
163 final double[] derivatives = new double[state.getMass().getFreeParameters()];
164 final Gradient[] accDer = new Gradient[3];
165 for (int i = 0; i < 3; ++i) {
166
167
168 derivatives[0] = hInertial.getEntry(i, 0);
169 derivatives[1] = hInertial.getEntry(i, 1);
170 derivatives[2] = hInertial.getEntry(i, 2);
171
172
173
174 accDer[i] = new Gradient(gInertial[i], derivatives);
175
176 }
177
178 return new FieldVector3D<>(accDer);
179
180 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
181 return null;
182 }
183 }
184
185 private GradientHessian gradientHessian(final HolmesFeatherstoneAttractionModel hfModel,
186 final AbsoluteDate date, final Vector3D position)
187 {
188 try {
189
190 java.lang.reflect.Field providerField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("provider");
191 providerField.setAccessible(true);
192 NormalizedSphericalHarmonicsProvider provider = (NormalizedSphericalHarmonicsProvider) providerField.get(hfModel);
193 java.lang.reflect.Method createDistancePowersArrayMethod =
194 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createDistancePowersArray", Double.TYPE);
195 createDistancePowersArrayMethod.setAccessible(true);
196 java.lang.reflect.Method createCosSinArraysMethod =
197 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createCosSinArrays", Double.TYPE, Double.TYPE);
198 createCosSinArraysMethod.setAccessible(true);
199 java.lang.reflect.Method computeTesseralMethod =
200 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("computeTesseral",
201 Integer.TYPE, Integer.TYPE, Integer.TYPE,
202 Double.TYPE, Double.TYPE, Double.TYPE,
203 double[].class, double[].class, double[].class,
204 double[].class, double[].class, double[].class);
205 computeTesseralMethod.setAccessible(true);
206
207 final int degree = provider.getMaxDegree();
208 final int order = provider.getMaxOrder();
209 final NormalizedSphericalHarmonics harmonics = provider.onDate(date);
210
211
212 double[] pnm0Plus2 = new double[degree + 1];
213 double[] pnm0Plus1 = new double[degree + 1];
214 double[] pnm0 = new double[degree + 1];
215 double[] pnm1Plus1 = new double[degree + 1];
216 double[] pnm1 = new double[degree + 1];
217 final double[] pnm2 = new double[degree + 1];
218
219
220 final double x = position.getX();
221 final double y = position.getY();
222 final double z = position.getZ();
223 final double x2 = x * x;
224 final double y2 = y * y;
225 final double z2 = z * z;
226 final double r2 = x2 + y2 + z2;
227 final double r = FastMath.sqrt (r2);
228 final double rho2 = x2 + y2;
229 final double rho = FastMath.sqrt(rho2);
230 final double t = z / r;
231 final double u = rho / r;
232 final double tOu = z / rho;
233
234
235 final double[] aOrN = (double[]) createDistancePowersArrayMethod.invoke(hfModel, provider.getAe() / r);
236
237
238 final double[][] cosSinLambda = (double[][]) createCosSinArraysMethod.invoke(hfModel, position.getX() / rho, position.getY() / rho);
239
240
241 int index = 0;
242 double value = 0;
243 final double[] gradient = new double[3];
244 final double[][] hessian = new double[3][3];
245 for (int m = degree; m >= 0; --m) {
246
247
248 index = (Integer) computeTesseralMethod.invoke(hfModel, m, degree, index, t, u, tOu,
249 pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2);
250
251 if (m <= order) {
252
253
254
255 double sumDegreeS = 0;
256 double sumDegreeC = 0;
257 double dSumDegreeSdR = 0;
258 double dSumDegreeCdR = 0;
259 double dSumDegreeSdTheta = 0;
260 double dSumDegreeCdTheta = 0;
261 double d2SumDegreeSdRdR = 0;
262 double d2SumDegreeSdRdTheta = 0;
263 double d2SumDegreeSdThetadTheta = 0;
264 double d2SumDegreeCdRdR = 0;
265 double d2SumDegreeCdRdTheta = 0;
266 double d2SumDegreeCdThetadTheta = 0;
267 for (int n = FastMath.max(2, m); n <= degree; ++n) {
268 final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m);
269 final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m);
270 final double nOr = n / r;
271 final double nnP1Or2 = nOr * (n + 1) / r;
272 final double s0 = pnm0[n] * qSnm;
273 final double c0 = pnm0[n] * qCnm;
274 final double s1 = pnm1[n] * qSnm;
275 final double c1 = pnm1[n] * qCnm;
276 final double s2 = pnm2[n] * qSnm;
277 final double c2 = pnm2[n] * qCnm;
278 sumDegreeS += s0;
279 sumDegreeC += c0;
280 dSumDegreeSdR -= nOr * s0;
281 dSumDegreeCdR -= nOr * c0;
282 dSumDegreeSdTheta += s1;
283 dSumDegreeCdTheta += c1;
284 d2SumDegreeSdRdR += nnP1Or2 * s0;
285 d2SumDegreeSdRdTheta -= nOr * s1;
286 d2SumDegreeSdThetadTheta += s2;
287 d2SumDegreeCdRdR += nnP1Or2 * c0;
288 d2SumDegreeCdRdTheta -= nOr * c1;
289 d2SumDegreeCdThetadTheta += c2;
290 }
291
292
293 final double sML = cosSinLambda[1][m];
294 final double cML = cosSinLambda[0][m];
295 value = value * u + sML * sumDegreeS + cML * sumDegreeC;
296 gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
297 gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
298 gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
299 hessian[0][0] = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
300 hessian[1][0] = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
301 hessian[2][0] = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
302 hessian[1][1] = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
303 hessian[2][1] = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
304 hessian[2][2] = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;
305
306 }
307
308
309 final double[] tmp0 = pnm0Plus2;
310 pnm0Plus2 = pnm0Plus1;
311 pnm0Plus1 = pnm0;
312 pnm0 = tmp0;
313 final double[] tmp1 = pnm1Plus1;
314 pnm1Plus1 = pnm1;
315 pnm1 = tmp1;
316
317 }
318
319
320 value = FastMath.scalb(value, SCALING);
321 for (int i = 0; i < 3; ++i) {
322 gradient[i] = FastMath.scalb(gradient[i], SCALING);
323 for (int j = 0; j <= i; ++j) {
324 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
325 }
326 }
327
328
329
330 final double muOr = provider.getMu() / r;
331 value *= muOr;
332 gradient[0] = muOr * gradient[0] - value / r;
333 gradient[1] *= muOr;
334 gradient[2] *= muOr;
335 hessian[0][0] = muOr * hessian[0][0] - 2 * gradient[0] / r;
336 hessian[1][0] = muOr * hessian[1][0] - gradient[1] / r;
337 hessian[2][0] = muOr * hessian[2][0] - gradient[2] / r;
338 hessian[1][1] *= muOr;
339 hessian[2][1] *= muOr;
340 hessian[2][2] *= muOr;
341
342
343 final SphericalCoordinates sc = new SphericalCoordinates(position);
344 return new GradientHessian(sc.toCartesianGradient(gradient),
345 sc.toCartesianHessian(hessian, gradient));
346
347
348 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException |
349 SecurityException | InvocationTargetException | NoSuchMethodException e) {
350 return null;
351 }
352 }
353
354
355 private static class GradientHessian implements Serializable {
356
357
358 private static final long serialVersionUID = 20130219L;
359
360
361 private final double[] gradient;
362
363
364 private final double[][] hessian;
365
366
367
368
369
370
371
372
373 public GradientHessian(final double[] gradient, final double[][] hessian) {
374 this.gradient = gradient;
375 this.hessian = hessian;
376 }
377
378
379
380
381 public double[] getGradient() {
382 return gradient;
383 }
384
385
386
387
388 public double[][] getHessian() {
389 return hessian;
390 }
391
392 }
393
394 @Test
395 void testRelativeNumericPrecision() {
396
397
398
399
400 int max = 50;
401 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
402 HolmesFeatherstoneAttractionModel model =
403 new HolmesFeatherstoneAttractionModel(itrf, provider);
404 Assertions.assertTrue(model.dependsOnPositionOnly());
405
406
407
408
409
410
411
412
413
414
415 int digits = 30;
416 ReferenceFieldModel refModel = new ReferenceFieldModel(provider, digits);
417
418 double r = 1.25;
419 for (double theta = 0.01; theta < 3.14; theta += 0.1) {
420 Vector3D position = new Vector3D(r * FastMath.sin(theta), 0.0, r * FastMath.cos(theta));
421 Dfp refValue = refModel.nonCentralPart(null, position);
422 double value = model.nonCentralPart(null, position, model.getMu());
423 double relativeError = error(refValue, value).divide(refValue).toDouble();
424 Assertions.assertEquals(0, relativeError, 7.0e-15);
425 }
426
427 }
428
429 @Test
430 void testValue() {
431
432 int max = 50;
433 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
434 HolmesFeatherstoneAttractionModel model =
435 new HolmesFeatherstoneAttractionModel(itrf, provider);
436
437 double r = 1.25;
438 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
439 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
440 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
441 r * FastMath.sin(theta) * FastMath.sin(lambda),
442 r * FastMath.cos(theta));
443 double refValue = provider.getMu() / position.getNorm() +
444 model.nonCentralPart(AbsoluteDate.GPS_EPOCH, position, model.getMu());
445 double value = model.value(AbsoluteDate.GPS_EPOCH, position, model.getMu());
446 Assertions.assertEquals(refValue, value, 1.0e-15 * FastMath.abs(refValue));
447 }
448 }
449
450 }
451
452
453
454
455
456
457 @Test
458 public void RealFieldTest() {
459 DSFactory factory = new DSFactory(6, 4);
460 DerivativeStructure a_0 = factory.variable(0, 7201009.7124401);
461 DerivativeStructure e_0 = factory.variable(1, 5e-3);
462 DerivativeStructure i_0 = factory.variable(2, 98.7 * FastMath.PI / 180);
463 DerivativeStructure R_0 = factory.variable(3, 15.0 * 22.5 * FastMath.PI / 180);
464 DerivativeStructure O_0 = factory.variable(4, 93.0 * FastMath.PI / 180);
465 DerivativeStructure n_0 = factory.variable(5, 0.1);
466
467 Field<DerivativeStructure> field = a_0.getField();
468 DerivativeStructure zero = field.getZero();
469
470 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
471
472 Frame EME = FramesFactory.getEME2000();
473
474 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
475 PositionAngleType.MEAN,
476 EME,
477 J2000,
478 zero.add(Constants.EIGEN5C_EARTH_MU));
479
480 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
481
482 SpacecraftState iSR = initialState.toSpacecraftState();
483 OrbitType type = OrbitType.EQUINOCTIAL;
484 double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
485
486
487 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
488 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
489 integrator.setInitialStepSize(60);
490 AdaptiveStepsizeIntegrator RIntegrator =
491 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
492 RIntegrator.setInitialStepSize(60);
493
494 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
495 FNP.setOrbitType(type);
496 FNP.setPositionAngleType(PositionAngleType.TRUE);
497 FNP.setInitialState(initialState);
498
499 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
500 NP.setOrbitType(FNP.getOrbitType());
501 NP.setPositionAngleType(FNP.getPositionAngleType());
502 NP.setInitialState(iSR);
503
504 double[][] c = new double[3][1];
505 c[0][0] = 0.0;
506 c[2][0] = normalizedC20;
507 double[][] s = new double[3][1];
508 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
509 TideSystem.UNKNOWN,
510 c, s);
511 HolmesFeatherstoneAttractionModel forceModel =
512 new HolmesFeatherstoneAttractionModel(itrf, provider);
513
514 FNP.addForceModel(forceModel);
515 NP.addForceModel(forceModel);
516
517
518 checkRealFieldPropagation(FKO, PositionAngleType.MEAN, 1005., NP, FNP,
519 1.0e-14, 6.0e-8, 1.8e-11, 1.8e-10,
520 1, false);
521 }
522
523
524
525
526
527 @Test
528 public void RealFieldExpectErrorTest() {
529 DSFactory factory = new DSFactory(6, 0);
530 DerivativeStructure a_0 = factory.variable(0, 7201009.7124401);
531 DerivativeStructure e_0 = factory.variable(1, 1e-3);
532 DerivativeStructure i_0 = factory.variable(2, 98.7 * FastMath.PI / 180);
533 DerivativeStructure R_0 = factory.variable(3, 15.0 * 22.5 * FastMath.PI / 180);
534 DerivativeStructure O_0 = factory.variable(4, 93.0 * FastMath.PI / 180);
535 DerivativeStructure n_0 = factory.variable(5, 0.1);
536
537 Field<DerivativeStructure> field = a_0.getField();
538 DerivativeStructure zero = field.getZero();
539
540 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
541
542 Frame EME = FramesFactory.getEME2000();
543 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
544 PositionAngleType.MEAN,
545 EME,
546 J2000,
547 zero.add(Constants.EIGEN5C_EARTH_MU));
548
549 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
550
551 SpacecraftState iSR = initialState.toSpacecraftState();
552 OrbitType type = OrbitType.EQUINOCTIAL;
553 double[][] tolerance = ToleranceProvider.getDefaultToleranceProvider(10.).getTolerances(FKO.toOrbit(), type);
554
555
556 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
557 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
558 integrator.setInitialStepSize(60);
559 AdaptiveStepsizeIntegrator RIntegrator =
560 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
561 RIntegrator.setInitialStepSize(60);
562
563 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
564 FNP.setOrbitType(type);
565 FNP.setInitialState(initialState);
566
567 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
568 NP.setOrbitType(type);
569 NP.setInitialState(iSR);
570
571 double[][] c = new double[3][1];
572 c[0][0] = 0.0;
573 c[2][0] = normalizedC20;
574 double[][] s = new double[3][1];
575 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
576 TideSystem.UNKNOWN,
577 c, s);
578 HolmesFeatherstoneAttractionModel forceModel =
579 new HolmesFeatherstoneAttractionModel(itrf, provider);
580
581
582 NP.addForceModel(forceModel);
583
584 FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(100.);
585 FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
586 SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
587 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
588 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
589 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
590 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
591 Assertions.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
592 }
593 @Test
594 void testGradient() {
595
596 int max = 50;
597 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
598 HolmesFeatherstoneAttractionModel model =
599 new HolmesFeatherstoneAttractionModel(itrf, provider);
600
601 double r = 1.25;
602 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
603 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
604 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
605 r * FastMath.sin(theta) * FastMath.sin(lambda),
606 r * FastMath.cos(theta));
607 double[] refGradient = gradient(model, null, position, 1.0e-3);
608 double norm = FastMath.sqrt(refGradient[0] * refGradient[0] +
609 refGradient[1] * refGradient[1] +
610 refGradient[2] * refGradient[2]);
611 double[] gradient = model.gradient(null, position, model.getMu());
612 double errorX = refGradient[0] - gradient[0];
613 double errorY = refGradient[1] - gradient[1];
614 double errorZ = refGradient[2] - gradient[2];
615 double relativeError = FastMath.sqrt(errorX * errorX + errorY * errorY + errorZ * errorZ) /
616 norm;
617 Assertions.assertEquals(0, relativeError, 3.0e-12);
618 }
619 }
620
621 }
622
623 @Test
624 void testHessian() {
625
626 int max = 50;
627 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
628 HolmesFeatherstoneAttractionModel model =
629 new HolmesFeatherstoneAttractionModel(itrf, provider);
630
631 double r = 1.25;
632 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
633 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
634 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
635 r * FastMath.sin(theta) * FastMath.sin(lambda),
636 r * FastMath.cos(theta));
637 double[][] refHessian = hessian(model, null, position, 1.0e-3);
638 double[][] hessian = gradientHessian(model, null, position).getHessian();
639 double normH2 = 0;
640 double normE2 = 0;
641 for (int i = 0; i < 3; ++i) {
642 for (int j = 0; j < 3; ++j) {
643 double error = refHessian[i][j] - hessian[i][j];
644 normH2 += refHessian[i][j] * refHessian[i][j];
645 normE2 += error * error;
646 }
647 }
648 Assertions.assertEquals(0, FastMath.sqrt(normE2 / normH2), 5.0e-12);
649 }
650 }
651
652 }
653
654 private Dfp error(Dfp refValue, double value) {
655 return refValue.getField().newDfp(value).subtract(refValue);
656 }
657
658 private double[] gradient(final HolmesFeatherstoneAttractionModel model,
659 final AbsoluteDate date, final Vector3D position, final double h)
660 {
661 return new double[] {
662 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_I)), model.getMu()),
663 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_I)), model.getMu()),
664 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_I)), model.getMu()),
665 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_I)), model.getMu()),
666 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_I)), model.getMu()),
667 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_I)), model.getMu()),
668 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_I)), model.getMu()),
669 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_I)), model.getMu()),
670 h),
671 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_J)), model.getMu()),
672 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_J)), model.getMu()),
673 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_J)), model.getMu()),
674 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_J)), model.getMu()),
675 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_J)), model.getMu()),
676 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_J)), model.getMu()),
677 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_J)), model.getMu()),
678 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_J)), model.getMu()),
679 h),
680 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_K)), model.getMu()),
681 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_K)), model.getMu()),
682 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_K)), model.getMu()),
683 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_K)), model.getMu()),
684 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_K)), model.getMu()),
685 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_K)), model.getMu()),
686 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_K)), model.getMu()),
687 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_K)), model.getMu()),
688 h)
689 };
690 }
691
692 private double[][] hessian(final HolmesFeatherstoneAttractionModel model,
693 final AbsoluteDate date, final Vector3D position, final double h)
694 {
695 return new double[][] {
696 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_I)), model.getMu()),
697 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_I)), model.getMu()),
698 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_I)), model.getMu()),
699 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_I)), model.getMu()),
700 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_I)), model.getMu()),
701 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_I)), model.getMu()),
702 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_I)), model.getMu()),
703 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_I)), model.getMu()),
704 h),
705 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_J)), model.getMu()),
706 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_J)), model.getMu()),
707 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_J)), model.getMu()),
708 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_J)), model.getMu()),
709 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_J)), model.getMu()),
710 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_J)), model.getMu()),
711 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_J)), model.getMu()),
712 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_J)), model.getMu()),
713 h),
714 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_K)), model.getMu()),
715 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_K)), model.getMu()),
716 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_K)), model.getMu()),
717 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_K)), model.getMu()),
718 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_K)), model.getMu()),
719 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_K)), model.getMu()),
720 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_K)), model.getMu()),
721 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_K)), model.getMu()),
722 h)
723 };
724 }
725
726
727
728
729
730
731
732 private static class GleasonProvider implements NormalizedSphericalHarmonicsProvider {
733
734 private final int degree;
735 private final int order;
736
737 public GleasonProvider(int degree, int order) {
738 this.degree = degree;
739 this.order = order;
740 }
741
742 public int getMaxDegree() {
743 return degree;
744 }
745
746 public int getMaxOrder() {
747 return order;
748 }
749
750 public double getMu() {
751 return 1;
752 }
753
754 public double getAe() {
755 return 1;
756 }
757
758 public AbsoluteDate getReferenceDate() {
759 return null;
760 }
761
762 public TideSystem getTideSystem() {
763 return TideSystem.UNKNOWN;
764 }
765
766 @Override
767 public NormalizedSphericalHarmonics onDate(final AbsoluteDate date) {
768 return new NormalizedSphericalHarmonics() {
769 @Override
770 public double getNormalizedCnm(int n, int m) {
771 return 1;
772 }
773
774 @Override
775 public double getNormalizedSnm(int n, int m) {
776 return 1;
777 }
778
779 @Override
780 public AbsoluteDate getDate() {
781 return date;
782 }
783 };
784 }
785
786 }
787
788
789 @Test
790 void testHelioSynchronous()
791 {
792
793
794 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 7, 1),
795 new TimeComponents(13, 59, 27.816),
796 TimeScalesFactory.getUTC());
797 Transform itrfToEME2000 = itrf.getTransformTo(FramesFactory.getEME2000(), date);
798 Vector3D pole = itrfToEME2000.transformVector(Vector3D.PLUS_K);
799 Frame poleAligned = new Frame(FramesFactory.getEME2000(),
800 new Transform(date, new Rotation(pole, Vector3D.PLUS_K)),
801 "pole aligned", true);
802
803 double i = FastMath.toRadians(98.7);
804 double omega = FastMath.toRadians(93.0);
805 double OMEGA = FastMath.toRadians(15.0 * 22.5);
806 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
807 0, PositionAngleType.MEAN, poleAligned, date, mu);
808 double[][] c = new double[3][1];
809 c[0][0] = 0.0;
810 c[2][0] = normalizedC20;
811 double[][] s = new double[3][1];
812 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(itrf,
813 GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
814 TideSystem.UNKNOWN,
815 c, s)));
816
817
818 propagator.setStepHandler(Constants.JULIAN_DAY, new SpotStepHandler());
819 propagator.setInitialState(new SpacecraftState(orbit));
820 propagator.propagate(date.shiftedBy(7 * Constants.JULIAN_DAY));
821 Assertions.assertTrue(propagator.getCalls() < 9200);
822
823 }
824
825 private static class SpotStepHandler implements OrekitFixedStepHandler {
826
827 private final PVCoordinatesProvider sun;
828 private double previous;
829
830 public SpotStepHandler() {
831 sun = CelestialBodyFactory.getSun();
832 previous = Double.NaN;
833 }
834
835 public void handleStep(SpacecraftState currentState) {
836
837
838 AbsoluteDate current = currentState.getDate();
839 Vector3D sunPos = sun.getPosition(current , FramesFactory.getEME2000());
840 Vector3D normal = currentState.getPVCoordinates().getMomentum();
841 double angle = Vector3D.angle(sunPos , normal);
842 if (! Double.isNaN(previous)) {
843 Assertions.assertEquals(previous, angle, 0.0013);
844 }
845 previous = angle;
846 }
847
848 }
849
850
851 @Test
852 void testEcksteinHechlerReference() {
853
854
855 AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
856 Vector3D position = new Vector3D(3220103., 69623., 6449822.);
857 Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
858
859 Transform itrfToEME2000 = itrf.getTransformTo(FramesFactory.getEME2000(), date);
860 Vector3D pole = itrfToEME2000.transformVector(Vector3D.PLUS_K);
861 Frame poleAligned = new Frame(FramesFactory.getEME2000(),
862 new Transform(date, new Rotation(pole, Vector3D.PLUS_K)),
863 "pole aligned", true);
864
865 Orbit initialOrbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
866 poleAligned, date, mu);
867
868 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(itrf,
869 GravityFieldFactory.getNormalizedProvider(ae, mu,
870 TideSystem.UNKNOWN,
871 new double[][] {
872 { 0.0 }, { 0.0 }, { normalizedC20 }, { normalizedC30 },
873 { normalizedC40 }, { normalizedC50 }, { normalizedC60 },
874 },
875 new double[][] {
876 { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 },
877 { 0.0 }, { 0.0 }, { 0.0 },
878 })));
879
880
881 propagator.setInitialState(new SpacecraftState(initialOrbit));
882 propagator.setOrbitType(OrbitType.EQUINOCTIAL);
883 propagator.setPositionAngleType(PositionAngleType.TRUE);
884 propagator.setStepHandler(20, new EckStepHandler(initialOrbit, ae,
885 unnormalizedC20, unnormalizedC30, unnormalizedC40,
886 unnormalizedC50, unnormalizedC60));
887 propagator.propagate(date.shiftedBy(50000));
888 Assertions.assertTrue(propagator.getCalls() < 1100);
889
890 }
891
892 private static class EckStepHandler implements OrekitFixedStepHandler {
893
894
895 private static final double mu = 3.986004415e+14;
896
897 private final EcksteinHechlerPropagator referencePropagator;
898
899 private EckStepHandler(Orbit initialOrbit, double ae,
900 double c20, double c30, double c40, double c50, double c60) {
901 referencePropagator =
902 new EcksteinHechlerPropagator(initialOrbit,
903 ae, mu, c20, c30, c40, c50, c60);
904 }
905
906 public void handleStep(SpacecraftState currentState) {
907
908 SpacecraftState EHPOrbit = referencePropagator.propagate(currentState.getDate());
909 Vector3D posEHP = EHPOrbit.getPosition();
910 Vector3D posDROZ = currentState.getPosition();
911 Vector3D velEHP = EHPOrbit.getVelocity();
912 Vector3D dif = posEHP.subtract(posDROZ);
913
914 Vector3D T = new Vector3D(1 / velEHP.getNorm(), velEHP);
915 Vector3D W = EHPOrbit.getPVCoordinates().getMomentum().normalize();
916 Vector3D N = Vector3D.crossProduct(W, T);
917
918 Assertions.assertTrue(dif.getNorm() < 111);
919 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, T)) < 111);
920 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, N)) < 54);
921 Assertions.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, W)) < 12);
922
923 }
924
925 }
926
927 @Test
928 void testParameterDerivative() {
929
930 Utils.setDataRoot("regular-data:potential/grgs-format");
931 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
932
933
934 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
935 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
936 final Frame frame = FramesFactory.getGCRF();
937 final AbsoluteDate date = new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI());
938 final CartesianOrbit orbit = new CartesianOrbit(new PVCoordinates(pos, vel), frame,
939 date, Constants.EIGEN5C_EARTH_MU);
940 final LofOffset lofOffset = new LofOffset(frame, LOFType.LVLH_CCSDS);
941 final Attitude attitude = lofOffset.getAttitude(orbit, date, frame);
942 final SpacecraftState state = new SpacecraftState(orbit, attitude);
943
944 final HolmesFeatherstoneAttractionModel holmesFeatherstoneModel =
945 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true),
946 GravityFieldFactory.getNormalizedProvider(20, 20));
947
948 final String name = NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT;
949 checkParameterDerivative(state, holmesFeatherstoneModel, name, 1.0e-5, 5.0e-12);
950
951 }
952
953 @Test
954 void testParameterDerivativeGradient() {
955
956 Utils.setDataRoot("regular-data:potential/grgs-format");
957 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
958
959
960 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
961 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
962 final SpacecraftState state =
963 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
964 FramesFactory.getGCRF(),
965 new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
966 GravityFieldFactory.getUnnormalizedProvider(1, 1).getMu()));
967
968 final HolmesFeatherstoneAttractionModel holmesFeatherstoneModel =
969 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true),
970 GravityFieldFactory.getNormalizedProvider(20, 20));
971
972 final String name = NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT;
973 checkParameterDerivativeGradient(state, holmesFeatherstoneModel, name, 1.0e-5, 5.0e-12);
974
975 }
976
977 @Test
978 void testTimeDependentField() {
979
980 Utils.setDataRoot("regular-data:potential/icgem-format");
981 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
982
983 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
984 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
985 final SpacecraftState spacecraftState =
986 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
987 FramesFactory.getGCRF(),
988 new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
989 GravityFieldFactory.getUnnormalizedProvider(1, 1).getMu()));
990
991 double dP = 0.1;
992 double duration = 3 * Constants.JULIAN_DAY;
993 BoundedPropagator fixedFieldEphemeris = createEphemeris(dP, spacecraftState, duration,
994 GravityFieldFactory.getConstantNormalizedProvider(8, 8, new AbsoluteDate("2005-01-01T00:00:00.000", TimeScalesFactory.getTAI())));
995 BoundedPropagator varyingFieldEphemeris = createEphemeris(dP, spacecraftState, duration,
996 GravityFieldFactory.getNormalizedProvider(8, 8));
997
998 double step = 60.0;
999 double maxDeltaT = 0;
1000 double maxDeltaN = 0;
1001 double maxDeltaW = 0;
1002 for (AbsoluteDate date = fixedFieldEphemeris.getMinDate();
1003 date.compareTo(fixedFieldEphemeris.getMaxDate()) < 0;
1004 date = date.shiftedBy(step)) {
1005 PVCoordinates pvFixedField = fixedFieldEphemeris.getPVCoordinates(date, FramesFactory.getGCRF());
1006 PVCoordinates pvVaryingField = varyingFieldEphemeris.getPVCoordinates(date, FramesFactory.getGCRF());
1007 Vector3D t = pvFixedField.getVelocity().normalize();
1008 Vector3D w = pvFixedField.getMomentum().normalize();
1009 Vector3D n = Vector3D.crossProduct(w, t);
1010 Vector3D delta = pvVaryingField.getPosition().subtract(pvFixedField.getPosition());
1011 maxDeltaT = FastMath.max(maxDeltaT, FastMath.abs(Vector3D.dotProduct(delta, t)));
1012 maxDeltaN = FastMath.max(maxDeltaN, FastMath.abs(Vector3D.dotProduct(delta, n)));
1013 maxDeltaW = FastMath.max(maxDeltaW, FastMath.abs(Vector3D.dotProduct(delta, w)));
1014 }
1015 Assertions.assertTrue(maxDeltaT > 0.65);
1016 Assertions.assertTrue(maxDeltaT < 0.85);
1017 Assertions.assertTrue(maxDeltaN > 0.02);
1018 Assertions.assertTrue(maxDeltaN < 0.03);
1019 Assertions.assertTrue(maxDeltaW > 0.05);
1020 Assertions.assertTrue(maxDeltaW < 0.10);
1021
1022 }
1023
1024 private BoundedPropagator createEphemeris(double dP, SpacecraftState initialState, double duration,
1025 NormalizedSphericalHarmonicsProvider provider)
1026 {
1027 double[][] tol = ToleranceProvider.getDefaultToleranceProvider(dP).getTolerances(initialState.getOrbit(), OrbitType.CARTESIAN);
1028 AbstractIntegrator integrator =
1029 new DormandPrince853Integrator(0.001, 120.0, tol[0], tol[1]);
1030 NumericalPropagator propagator = new NumericalPropagator(integrator);
1031 final EphemerisGenerator generator = propagator.getEphemerisGenerator();
1032 propagator.setOrbitType(OrbitType.CARTESIAN);
1033 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider));
1034 propagator.setInitialState(initialState);
1035 propagator.propagate(initialState.getDate().shiftedBy(duration));
1036 return generator.getGeneratedEphemeris();
1037 }
1038
1039 @Test
1040 void testStateJacobian()
1041 {
1042
1043 Utils.setDataRoot("regular-data:potential/grgs-format");
1044 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1045
1046
1047 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1048 new TimeComponents(13, 59, 27.816),
1049 TimeScalesFactory.getUTC());
1050 double i = FastMath.toRadians(98.7);
1051 double omega = FastMath.toRadians(93.0);
1052 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1053 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1054 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1055 OrbitType integrationType = OrbitType.CARTESIAN;
1056 double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(0.01).getTolerances(orbit, integrationType);
1057
1058 propagator = new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
1059 tolerances[0], tolerances[1]));
1060 propagator.setOrbitType(integrationType);
1061 HolmesFeatherstoneAttractionModel hfModel =
1062 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1063 Assertions.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1064 propagator.addForceModel(hfModel);
1065 SpacecraftState state0 = new SpacecraftState(orbit);
1066 propagator.setInitialState(state0);
1067
1068 checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
1069 50000, tolerances[0], 7.8e-6);
1070 }
1071
1072 @Test
1073 void testStateJacobianVs80Implementation()
1074 {
1075
1076 Utils.setDataRoot("regular-data:potential/grgs-format");
1077 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1078
1079
1080 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1081 new TimeComponents(13, 59, 27.816),
1082 TimeScalesFactory.getUTC());
1083 double i = FastMath.toRadians(98.7);
1084 double omega = FastMath.toRadians(93.0);
1085 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1086 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1087 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1088
1089 HolmesFeatherstoneAttractionModel hfModel =
1090 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1091 Assertions.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1092 SpacecraftState state = new SpacecraftState(orbit);
1093
1094 checkStateJacobianVs80Implementation(state, hfModel,
1095 new LofOffset(state.getFrame(), LOFType.LVLH_CCSDS),
1096 2.0e-15, false);
1097
1098 }
1099
1100 @Test
1101 void testStateJacobianVs80ImplementationGradient()
1102 {
1103
1104 Utils.setDataRoot("regular-data:potential/grgs-format");
1105 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1106
1107
1108 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1109 new TimeComponents(13, 59, 27.816),
1110 TimeScalesFactory.getUTC());
1111 double i = FastMath.toRadians(98.7);
1112 double omega = FastMath.toRadians(93.0);
1113 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1114 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1115 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1116
1117 HolmesFeatherstoneAttractionModel hfModel =
1118 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1119 Assertions.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1120 SpacecraftState state = new SpacecraftState(orbit);
1121
1122 checkStateJacobianVs80ImplementationGradient(state, hfModel,
1123 new LofOffset(state.getFrame(), LOFType.LVLH_CCSDS),
1124 2.0e-15, false);
1125
1126 }
1127
1128 @Test
1129 void testStateJacobianVsFiniteDifferences()
1130 {
1131
1132 Utils.setDataRoot("regular-data:potential/grgs-format");
1133 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1134
1135
1136 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1137 new TimeComponents(13, 59, 27.816),
1138 TimeScalesFactory.getUTC());
1139 double i = FastMath.toRadians(98.7);
1140 double omega = FastMath.toRadians(93.0);
1141 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1142 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1143 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1144
1145 HolmesFeatherstoneAttractionModel hfModel =
1146 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1147 Assertions.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1148 SpacecraftState state = new SpacecraftState(orbit);
1149
1150 checkStateJacobianVsFiniteDifferences(state, hfModel, Utils.defaultLaw(),
1151 10.0, 2.0e-10, false);
1152
1153 }
1154
1155 @Test
1156 void testStateJacobianVsFiniteDifferencesGradient()
1157 {
1158
1159 Utils.setDataRoot("regular-data:potential/grgs-format");
1160 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1161
1162
1163 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1164 new TimeComponents(13, 59, 27.816),
1165 TimeScalesFactory.getUTC());
1166 double i = FastMath.toRadians(98.7);
1167 double omega = FastMath.toRadians(93.0);
1168 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1169 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1170 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1171
1172 HolmesFeatherstoneAttractionModel hfModel =
1173 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1174 Assertions.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1175 SpacecraftState state = new SpacecraftState(orbit);
1176
1177 checkStateJacobianVsFiniteDifferencesGradient(state, hfModel, Utils.defaultLaw(),
1178 10.0, 2.0e-10, false);
1179
1180 }
1181
1182 @Test
1183 void testIssue996() {
1184
1185 Utils.setDataRoot("regular-data:potential/grgs-format");
1186 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1187
1188
1189 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 7, 1),
1190 new TimeComponents(13, 59, 27.816),
1191 TimeScalesFactory.getUTC());
1192 double i = FastMath.toRadians(98.7);
1193 double omega = FastMath.toRadians(93.0);
1194 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1195 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1196 0, PositionAngleType.MEAN, FramesFactory.getEME2000(), date, mu);
1197
1198 Vector3D pos = orbit.getPosition(itrf);
1199
1200 double[] zeroGradient = new double[] {-0., 0., 0.};
1201
1202 NormalizedSphericalHarmonicsProvider provider00 = GravityFieldFactory.getNormalizedProvider(0, 0);
1203 HolmesFeatherstoneAttractionModel hfModel00 = new HolmesFeatherstoneAttractionModel(itrf, provider00);
1204
1205 Assertions.assertEquals(0., hfModel00.nonCentralPart(date, pos, mu));
1206 Assertions.assertEquals(mu / pos.getNorm(), hfModel00.value(date, pos, mu));
1207 Assertions.assertArrayEquals(zeroGradient, hfModel00.gradient(date, pos, mu));
1208
1209 NormalizedSphericalHarmonicsProvider provider10 = GravityFieldFactory.getNormalizedProvider(1, 0);
1210 HolmesFeatherstoneAttractionModel hfModel10 = new HolmesFeatherstoneAttractionModel(itrf, provider10);
1211
1212 Assertions.assertEquals(0., hfModel10.nonCentralPart(date, pos, mu));
1213 Assertions.assertEquals(mu / pos.getNorm(), hfModel10.value(date, pos, mu));
1214 Assertions.assertArrayEquals(zeroGradient, hfModel10.gradient(date, pos, mu));
1215
1216 NormalizedSphericalHarmonicsProvider provider11 = GravityFieldFactory.getNormalizedProvider(1, 1);
1217 HolmesFeatherstoneAttractionModel hfModel11 = new HolmesFeatherstoneAttractionModel(itrf, provider11);
1218
1219 Assertions.assertEquals(0., hfModel11.nonCentralPart(date, pos, mu));
1220 Assertions.assertEquals(mu / pos.getNorm(), hfModel11.value(date, pos, mu));
1221 Assertions.assertArrayEquals(zeroGradient, hfModel11.gradient(date, pos, mu));
1222
1223 }
1224
1225
1226 @Test
1227 @DisplayName("Test that acceleration derivatives with respect to absolute date are not equal to zero.")
1228 void testIssue1070() {
1229
1230
1231 final int freeParameters = 1;
1232 final GradientField field = GradientField.getField(freeParameters);
1233 final Gradient zero = field.getZero();
1234 final Gradient variable = Gradient.variable(freeParameters, 0, 0.);
1235 final FieldAbsoluteDate<Gradient> fieldAbsoluteDate = new FieldAbsoluteDate<>(field, AbsoluteDate.ARBITRARY_EPOCH).
1236 shiftedBy(variable);
1237
1238
1239 @SuppressWarnings("unchecked")
1240 final FieldSpacecraftState<Gradient> stateMock = Mockito.mock(FieldSpacecraftState.class);
1241 Mockito.when(stateMock.getDate()).thenReturn(fieldAbsoluteDate);
1242 Mockito.when(stateMock.getPosition()).thenReturn(new FieldVector3D<>(zero, zero));
1243 Mockito.when(stateMock.getFrame()).thenReturn(FramesFactory.getGCRF());
1244 Mockito.when(stateMock.getMass()).thenReturn(zero);
1245
1246
1247 final int max = 2;
1248 final NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
1249 final HolmesFeatherstoneAttractionModel model = new HolmesFeatherstoneAttractionModel(itrf, provider);
1250
1251
1252 final Gradient dummyGm = zero.add(1.);
1253 final FieldVector3D<Gradient> accelerationVector = model.acceleration(stateMock, new Gradient[] { dummyGm });
1254
1255
1256 final double[] derivatives = accelerationVector.getNormSq().getGradient();
1257 Assertions.assertNotEquals(0., MatrixUtils.createRealVector(derivatives).getNorm());
1258 }
1259
1260 @Test
1261 void testIssue1866GradientDate() {
1262 doTestIssue1866(true, false, true);
1263 }
1264
1265 @Test
1266 void testIssue1866GradientMu() {
1267 doTestIssue1866(false, true, true);
1268 }
1269
1270 @Test
1271 void testIssue1866DSDate() {
1272 doTestIssue1866(true, false, false);
1273 }
1274
1275 @Test
1276 void testIssue1866DSMu() {
1277 doTestIssue1866(false, true, false);
1278 }
1279
1280 private <T extends CalculusFieldElement<T>> void doTestIssue1866(final boolean partialsWrtDate,
1281 final boolean partialsWrtMu,
1282 final boolean isGradient) {
1283
1284 Utils.setDataRoot("regular-data:potential/icgem-format");
1285 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
1286
1287
1288 final Orbit orbit = new KeplerianOrbit(7e6, 0.2, 0.7, -1.0, 0.5, 0.7,
1289 PositionAngleType.TRUE, FramesFactory.getGCRF(), AbsoluteDate.ARBITRARY_EPOCH, Constants.EIGEN5C_EARTH_MU);
1290
1291
1292 final DSFactory referenceFactory = new DSFactory(8, 2);
1293 final DerivativeStructure referenceMu = getFieldMu(orbit.getMu(), referenceFactory, partialsWrtMu);
1294 final FieldAbsoluteDate<DerivativeStructure> referenceDate = getFieldAbsoluteDate(orbit.getDate(), referenceFactory, partialsWrtDate);
1295 final FieldPVCoordinates<DerivativeStructure> referencePV = getFieldPVCoordinates(orbit.getPVCoordinates(), referenceFactory);
1296 final FieldSpacecraftState<DerivativeStructure> referenceState = new FieldSpacecraftState<>(
1297 new FieldCartesianOrbit<>(referencePV, orbit.getFrame(), referenceDate, referenceMu));
1298
1299
1300 final DSFactory actualFactory = isGradient ? null : new DSFactory(8, 1);
1301 final T actualMu = getFieldMu(orbit.getMu(), actualFactory, partialsWrtMu);
1302 final FieldAbsoluteDate<T> actualDate = getFieldAbsoluteDate(orbit.getDate(), actualFactory, partialsWrtDate);
1303 final FieldPVCoordinates<T> actualPV = getFieldPVCoordinates(orbit.getPVCoordinates(), actualFactory);
1304 final FieldSpacecraftState<T> actualState = new FieldSpacecraftState<>(
1305 new FieldCartesianOrbit<>(actualPV, orbit.getFrame(), actualDate, actualMu));
1306
1307
1308 final HolmesFeatherstoneAttractionModel model = new HolmesFeatherstoneAttractionModel(
1309 FramesFactory.getITRF(IERSConventions.IERS_2010, true),
1310 GravityFieldFactory.getNormalizedProvider(4, 4));
1311
1312 final FieldVector3D<DerivativeStructure> referenceAcceleration = model
1313 .acceleration(referenceState, new DerivativeStructure[]{referenceMu});
1314
1315 final T[] parameters = MathArrays.buildArray(actualMu.getField(), 1);
1316 parameters[0] = actualMu;
1317 final FieldVector3D<T> actualAcceleration = model.acceleration(actualState, parameters);
1318
1319
1320 Assertions.assertArrayEquals(referenceAcceleration.toVector3D().toArray(), actualAcceleration.toVector3D().toArray(), 1e-16);
1321 checkPartials(referenceAcceleration.getX(), actualAcceleration.getX());
1322 checkPartials(referenceAcceleration.getY(), actualAcceleration.getY());
1323 checkPartials(referenceAcceleration.getZ(), actualAcceleration.getZ());
1324 }
1325
1326 @SuppressWarnings("unchecked")
1327 private static <T extends CalculusFieldElement<T>> FieldPVCoordinates<T> getFieldPVCoordinates(
1328 final PVCoordinates pv,
1329 final DSFactory factory) {
1330 if (factory == null) {
1331 return (FieldPVCoordinates<T>) new FieldPVCoordinates<>(
1332 new FieldVector3D<>(
1333 Gradient.variable(8, 0, pv.getPosition().getX()),
1334 Gradient.variable(8, 1, pv.getPosition().getY()),
1335 Gradient.variable(8, 2, pv.getPosition().getZ())),
1336 new FieldVector3D<>(
1337 Gradient.variable(8, 3, pv.getVelocity().getX()),
1338 Gradient.variable(8, 4, pv.getVelocity().getY()),
1339 Gradient.variable(8, 5, pv.getVelocity().getZ())));
1340 } else {
1341 return (FieldPVCoordinates<T>) new FieldPVCoordinates<>(
1342 new FieldVector3D<>(
1343 factory.variable(0, pv.getPosition().getX()),
1344 factory.variable(1, pv.getPosition().getY()),
1345 factory.variable(2, pv.getPosition().getZ())),
1346 new FieldVector3D<>(
1347 factory.variable(3, pv.getVelocity().getX()),
1348 factory.variable(4, pv.getVelocity().getY()),
1349 factory.variable(5, pv.getVelocity().getZ())));
1350 }
1351 }
1352
1353 @SuppressWarnings("unchecked")
1354 private static <T extends CalculusFieldElement<T>> T getFieldMu(
1355 final double mu,
1356 final DSFactory factory,
1357 final boolean partialsWrtMu) {
1358 if (factory == null) {
1359 if (partialsWrtMu) {
1360 return (T) Gradient.variable(8, 7, 0.0).multiply(1e6).add(mu);
1361 } else {
1362 return (T) Gradient.constant(8, mu);
1363 }
1364 } else {
1365 if (partialsWrtMu) {
1366 return (T) factory.variable(7, 0.0).multiply(1e6).add(mu);
1367 } else {
1368 return (T) factory.constant(mu);
1369 }
1370 }
1371 }
1372
1373 @SuppressWarnings("unchecked")
1374 private static <T extends CalculusFieldElement<T>> FieldAbsoluteDate<T> getFieldAbsoluteDate(
1375 final AbsoluteDate date,
1376 final DSFactory factory,
1377 final boolean partialsWrtDate) {
1378 if (factory == null) {
1379 if (partialsWrtDate) {
1380 final Gradient fieldOffset = Gradient.variable(8, 6, 0.0);
1381 return (FieldAbsoluteDate<T>) new FieldAbsoluteDate<>(fieldOffset.getField(), date).shiftedBy(fieldOffset);
1382 } else {
1383 return (FieldAbsoluteDate<T>) new FieldAbsoluteDate<>(GradientField.getField(8), date);
1384 }
1385 } else {
1386 if (partialsWrtDate) {
1387 final DerivativeStructure fieldOffset = factory.variable(6, 0.0);
1388 return (FieldAbsoluteDate<T>) new FieldAbsoluteDate<>(fieldOffset.getField(), date).shiftedBy(fieldOffset);
1389 } else {
1390 return (FieldAbsoluteDate<T>) new FieldAbsoluteDate<>(factory.getDerivativeField(), date);
1391 }
1392 }
1393 }
1394
1395 @SuppressWarnings("unchecked")
1396 private static <T extends CalculusFieldElement<T>> void checkPartials(final DerivativeStructure reference, final T actual) {
1397 final int[] orders = new int[reference.getFreeParameters()];
1398 final Derivative<T> derivatives = (Derivative<T>) actual;
1399 for (int i = 0; i < orders.length; i++) {
1400 orders[i] = 1;
1401 Assertions.assertEquals(reference.getPartialDerivative(orders), derivatives.getPartialDerivative(orders), 1e-16);
1402 orders[i] = 0;
1403 }
1404 }
1405
1406 @BeforeEach
1407 public void setUp() {
1408 itrf = null;
1409 propagator = null;
1410 Utils.setDataRoot("regular-data");
1411 try {
1412
1413 mu = 3.986004415e+14;
1414 ae = 6378136.460;
1415 normalizedC20 = -4.84165299820e-04;
1416 normalizedC30 = 9.57211326674e-07;
1417 normalizedC40 = 5.39990167207e-07;
1418 normalizedC50 = 6.86846073356e-08 ;
1419 normalizedC60 = -1.49953256913e-07;
1420 unnormalizedC20 = FastMath.sqrt( 5) * normalizedC20;
1421 unnormalizedC30 = FastMath.sqrt( 7) * normalizedC30;
1422 unnormalizedC40 = FastMath.sqrt( 9) * normalizedC40;
1423 unnormalizedC50 = FastMath.sqrt(11) * normalizedC50;
1424 unnormalizedC60 = FastMath.sqrt(13) * normalizedC60;
1425
1426 itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
1427 double[] absTolerance = {
1428 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
1429 };
1430 double[] relTolerance = {
1431 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
1432 };
1433 AdaptiveStepsizeIntegrator integrator =
1434 new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
1435 integrator.setInitialStepSize(60);
1436 propagator = new NumericalPropagator(integrator);
1437 } catch (OrekitException oe) {
1438 Assertions.fail(oe.getMessage());
1439 }
1440 }
1441
1442 @AfterEach
1443 public void tearDown() {
1444 itrf = null;
1445 propagator = null;
1446 }
1447
1448 private double unnormalizedC20;
1449 private double unnormalizedC30;
1450 private double unnormalizedC40;
1451 private double unnormalizedC50;
1452 private double unnormalizedC60;
1453 private double normalizedC20;
1454 private double normalizedC30;
1455 private double normalizedC40;
1456 private double normalizedC50;
1457 private double normalizedC60;
1458 private double mu;
1459 private double ae;
1460
1461 private Frame itrf;
1462 private NumericalPropagator propagator;
1463
1464 }