1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.forces.gravity;
18
19
20 import java.io.Serializable;
21 import java.lang.reflect.InvocationTargetException;
22
23 import org.hipparchus.Field;
24 import org.hipparchus.analysis.differentiation.DSFactory;
25 import org.hipparchus.analysis.differentiation.DerivativeStructure;
26 import org.hipparchus.dfp.Dfp;
27 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
28 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
29 import org.hipparchus.geometry.euclidean.threed.Rotation;
30 import org.hipparchus.geometry.euclidean.threed.SphericalCoordinates;
31 import org.hipparchus.geometry.euclidean.threed.Vector3D;
32 import org.hipparchus.linear.Array2DRowRealMatrix;
33 import org.hipparchus.linear.RealMatrix;
34 import org.hipparchus.ode.AbstractIntegrator;
35 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeFieldIntegrator;
36 import org.hipparchus.ode.nonstiff.AdaptiveStepsizeIntegrator;
37 import org.hipparchus.ode.nonstiff.DormandPrince853FieldIntegrator;
38 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
39 import org.hipparchus.random.GaussianRandomGenerator;
40 import org.hipparchus.random.RandomGenerator;
41 import org.hipparchus.random.UncorrelatedRandomVectorGenerator;
42 import org.hipparchus.random.Well19937a;
43 import org.hipparchus.util.FastMath;
44 import org.junit.After;
45 import org.junit.Assert;
46 import org.junit.Before;
47 import org.junit.Test;
48 import org.orekit.Utils;
49 import org.orekit.attitudes.LofOffset;
50 import org.orekit.bodies.CelestialBodyFactory;
51 import org.orekit.errors.OrekitException;
52 import org.orekit.forces.AbstractLegacyForceModelTest;
53 import org.orekit.forces.ForceModel;
54 import org.orekit.forces.gravity.potential.GRGSFormatReader;
55 import org.orekit.forces.gravity.potential.GravityFieldFactory;
56 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
57 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
58 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider.NormalizedSphericalHarmonics;
59 import org.orekit.forces.gravity.potential.TideSystem;
60 import org.orekit.frames.Frame;
61 import org.orekit.frames.FramesFactory;
62 import org.orekit.frames.LOFType;
63 import org.orekit.frames.Transform;
64 import org.orekit.orbits.CartesianOrbit;
65 import org.orekit.orbits.EquinoctialOrbit;
66 import org.orekit.orbits.FieldKeplerianOrbit;
67 import org.orekit.orbits.KeplerianOrbit;
68 import org.orekit.orbits.Orbit;
69 import org.orekit.orbits.OrbitType;
70 import org.orekit.orbits.PositionAngle;
71 import org.orekit.propagation.BoundedPropagator;
72 import org.orekit.propagation.FieldSpacecraftState;
73 import org.orekit.propagation.Propagator;
74 import org.orekit.propagation.SpacecraftState;
75 import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
76 import org.orekit.propagation.numerical.FieldNumericalPropagator;
77 import org.orekit.propagation.numerical.NumericalPropagator;
78 import org.orekit.propagation.sampling.OrekitFixedStepHandler;
79 import org.orekit.time.AbsoluteDate;
80 import org.orekit.time.DateComponents;
81 import org.orekit.time.FieldAbsoluteDate;
82 import org.orekit.time.TimeComponents;
83 import org.orekit.time.TimeScalesFactory;
84 import org.orekit.utils.Constants;
85 import org.orekit.utils.FieldPVCoordinates;
86 import org.orekit.utils.IERSConventions;
87 import org.orekit.utils.PVCoordinates;
88 import org.orekit.utils.PVCoordinatesProvider;
89
90
91 public class HolmesFeatherstoneAttractionModelTest extends AbstractLegacyForceModelTest {
92
93 private static final int SCALING = 930;
94
95 @Override
96 protected FieldVector3D<DerivativeStructure> accelerationDerivatives(final ForceModel forceModel,
97 final AbsoluteDate date, final Frame frame,
98 final FieldVector3D<DerivativeStructure> position,
99 final FieldVector3D<DerivativeStructure> velocity,
100 final FieldRotation<DerivativeStructure> rotation,
101 final DerivativeStructure mass)
102 {
103 try {
104 java.lang.reflect.Field bodyFrameField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("bodyFrame");
105 bodyFrameField.setAccessible(true);
106 Frame bodyFrame = (Frame) bodyFrameField.get(forceModel);
107
108
109 final Transform fromBodyFrame = bodyFrame.getTransformTo(frame, date);
110 final Transform toBodyFrame = fromBodyFrame.getInverse();
111 final Vector3D positionBody = toBodyFrame.transformPosition(position.toVector3D());
112
113
114 final GradientHessian gh = gradientHessian((HolmesFeatherstoneAttractionModel) forceModel,
115 date, positionBody);
116
117
118 final double[] gInertial = fromBodyFrame.transformVector(new Vector3D(gh.getGradient())).toArray();
119
120
121 final RealMatrix hBody = new Array2DRowRealMatrix(gh.getHessian(), false);
122 final RealMatrix rot = new Array2DRowRealMatrix(toBodyFrame.getRotation().getMatrix());
123 final RealMatrix hInertial = rot.transpose().multiply(hBody).multiply(rot);
124
125
126 final double[] derivatives = new double[1 + mass.getFreeParameters()];
127 final DerivativeStructure[] accDer = new DerivativeStructure[3];
128 for (int i = 0; i < 3; ++i) {
129
130
131 derivatives[0] = gInertial[i];
132
133
134 derivatives[1] = hInertial.getEntry(i, 0);
135 derivatives[2] = hInertial.getEntry(i, 1);
136 derivatives[3] = hInertial.getEntry(i, 2);
137
138
139
140 accDer[i] = mass.getFactory().build(derivatives);
141
142 }
143
144 return new FieldVector3D<>(accDer);
145
146 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException | SecurityException e) {
147 return null;
148 }
149 }
150
151 private GradientHessian gradientHessian(final HolmesFeatherstoneAttractionModel hfModel,
152 final AbsoluteDate date, final Vector3D position)
153 {
154 try {
155
156 java.lang.reflect.Field providerField = HolmesFeatherstoneAttractionModel.class.getDeclaredField("provider");
157 providerField.setAccessible(true);
158 NormalizedSphericalHarmonicsProvider provider = (NormalizedSphericalHarmonicsProvider) providerField.get(hfModel);
159 java.lang.reflect.Method createDistancePowersArrayMethod =
160 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createDistancePowersArray", Double.TYPE);
161 createDistancePowersArrayMethod.setAccessible(true);
162 java.lang.reflect.Method createCosSinArraysMethod =
163 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("createCosSinArrays", Double.TYPE, Double.TYPE);
164 createCosSinArraysMethod.setAccessible(true);
165 java.lang.reflect.Method computeTesseralMethod =
166 HolmesFeatherstoneAttractionModel.class.getDeclaredMethod("computeTesseral",
167 Integer.TYPE, Integer.TYPE, Integer.TYPE,
168 Double.TYPE, Double.TYPE, Double.TYPE,
169 double[].class, double[].class, double[].class,
170 double[].class, double[].class, double[].class);
171 computeTesseralMethod.setAccessible(true);
172
173 final int degree = provider.getMaxDegree();
174 final int order = provider.getMaxOrder();
175 final NormalizedSphericalHarmonics harmonics = provider.onDate(date);
176
177
178 double[] pnm0Plus2 = new double[degree + 1];
179 double[] pnm0Plus1 = new double[degree + 1];
180 double[] pnm0 = new double[degree + 1];
181 double[] pnm1Plus1 = new double[degree + 1];
182 double[] pnm1 = new double[degree + 1];
183 final double[] pnm2 = new double[degree + 1];
184
185
186 final double x = position.getX();
187 final double y = position.getY();
188 final double z = position.getZ();
189 final double x2 = x * x;
190 final double y2 = y * y;
191 final double z2 = z * z;
192 final double r2 = x2 + y2 + z2;
193 final double r = FastMath.sqrt (r2);
194 final double rho2 = x2 + y2;
195 final double rho = FastMath.sqrt(rho2);
196 final double t = z / r;
197 final double u = rho / r;
198 final double tOu = z / rho;
199
200
201 final double[] aOrN = (double[]) createDistancePowersArrayMethod.invoke(hfModel, provider.getAe() / r);
202
203
204 final double[][] cosSinLambda = (double[][]) createCosSinArraysMethod.invoke(hfModel, position.getX() / rho, position.getY() / rho);
205
206
207 int index = 0;
208 double value = 0;
209 final double[] gradient = new double[3];
210 final double[][] hessian = new double[3][3];
211 for (int m = degree; m >= 0; --m) {
212
213
214 index = ((Integer) computeTesseralMethod.invoke(hfModel, m, degree, index, t, u, tOu,
215 pnm0Plus2, pnm0Plus1, pnm1Plus1, pnm0, pnm1, pnm2)).intValue();
216
217 if (m <= order) {
218
219
220
221 double sumDegreeS = 0;
222 double sumDegreeC = 0;
223 double dSumDegreeSdR = 0;
224 double dSumDegreeCdR = 0;
225 double dSumDegreeSdTheta = 0;
226 double dSumDegreeCdTheta = 0;
227 double d2SumDegreeSdRdR = 0;
228 double d2SumDegreeSdRdTheta = 0;
229 double d2SumDegreeSdThetadTheta = 0;
230 double d2SumDegreeCdRdR = 0;
231 double d2SumDegreeCdRdTheta = 0;
232 double d2SumDegreeCdThetadTheta = 0;
233 for (int n = FastMath.max(2, m); n <= degree; ++n) {
234 final double qSnm = aOrN[n] * harmonics.getNormalizedSnm(n, m);
235 final double qCnm = aOrN[n] * harmonics.getNormalizedCnm(n, m);
236 final double nOr = n / r;
237 final double nnP1Or2 = nOr * (n + 1) / r;
238 final double s0 = pnm0[n] * qSnm;
239 final double c0 = pnm0[n] * qCnm;
240 final double s1 = pnm1[n] * qSnm;
241 final double c1 = pnm1[n] * qCnm;
242 final double s2 = pnm2[n] * qSnm;
243 final double c2 = pnm2[n] * qCnm;
244 sumDegreeS += s0;
245 sumDegreeC += c0;
246 dSumDegreeSdR -= nOr * s0;
247 dSumDegreeCdR -= nOr * c0;
248 dSumDegreeSdTheta += s1;
249 dSumDegreeCdTheta += c1;
250 d2SumDegreeSdRdR += nnP1Or2 * s0;
251 d2SumDegreeSdRdTheta -= nOr * s1;
252 d2SumDegreeSdThetadTheta += s2;
253 d2SumDegreeCdRdR += nnP1Or2 * c0;
254 d2SumDegreeCdRdTheta -= nOr * c1;
255 d2SumDegreeCdThetadTheta += c2;
256 }
257
258
259 final double sML = cosSinLambda[1][m];
260 final double cML = cosSinLambda[0][m];
261 value = value * u + sML * sumDegreeS + cML * sumDegreeC;
262 gradient[0] = gradient[0] * u + sML * dSumDegreeSdR + cML * dSumDegreeCdR;
263 gradient[1] = gradient[1] * u + m * (cML * sumDegreeS - sML * sumDegreeC);
264 gradient[2] = gradient[2] * u + sML * dSumDegreeSdTheta + cML * dSumDegreeCdTheta;
265 hessian[0][0] = hessian[0][0] * u + sML * d2SumDegreeSdRdR + cML * d2SumDegreeCdRdR;
266 hessian[1][0] = hessian[1][0] * u + m * (cML * dSumDegreeSdR - sML * dSumDegreeCdR);
267 hessian[2][0] = hessian[2][0] * u + sML * d2SumDegreeSdRdTheta + cML * d2SumDegreeCdRdTheta;
268 hessian[1][1] = hessian[1][1] * u - m * m * (sML * sumDegreeS + cML * sumDegreeC);
269 hessian[2][1] = hessian[2][1] * u + m * (cML * dSumDegreeSdTheta - sML * dSumDegreeCdTheta);
270 hessian[2][2] = hessian[2][2] * u + sML * d2SumDegreeSdThetadTheta + cML * d2SumDegreeCdThetadTheta;
271
272 }
273
274
275 final double[] tmp0 = pnm0Plus2;
276 pnm0Plus2 = pnm0Plus1;
277 pnm0Plus1 = pnm0;
278 pnm0 = tmp0;
279 final double[] tmp1 = pnm1Plus1;
280 pnm1Plus1 = pnm1;
281 pnm1 = tmp1;
282
283 }
284
285
286 value = FastMath.scalb(value, SCALING);
287 for (int i = 0; i < 3; ++i) {
288 gradient[i] = FastMath.scalb(gradient[i], SCALING);
289 for (int j = 0; j <= i; ++j) {
290 hessian[i][j] = FastMath.scalb(hessian[i][j], SCALING);
291 }
292 }
293
294
295
296 final double muOr = provider.getMu() / r;
297 value *= muOr;
298 gradient[0] = muOr * gradient[0] - value / r;
299 gradient[1] *= muOr;
300 gradient[2] *= muOr;
301 hessian[0][0] = muOr * hessian[0][0] - 2 * gradient[0] / r;
302 hessian[1][0] = muOr * hessian[1][0] - gradient[1] / r;
303 hessian[2][0] = muOr * hessian[2][0] - gradient[2] / r;
304 hessian[1][1] *= muOr;
305 hessian[2][1] *= muOr;
306 hessian[2][2] *= muOr;
307
308
309 final SphericalCoordinates sc = new SphericalCoordinates(position);
310 return new GradientHessian(sc.toCartesianGradient(gradient),
311 sc.toCartesianHessian(hessian, gradient));
312
313
314 } catch (IllegalArgumentException | IllegalAccessException | NoSuchFieldException |
315 SecurityException | InvocationTargetException | NoSuchMethodException e) {
316 return null;
317 }
318 }
319
320
321 private static class GradientHessian implements Serializable {
322
323
324 private static final long serialVersionUID = 20130219L;
325
326
327 private final double[] gradient;
328
329
330 private final double[][] hessian;
331
332
333
334
335
336
337
338
339 public GradientHessian(final double[] gradient, final double[][] hessian) {
340 this.gradient = gradient;
341 this.hessian = hessian;
342 }
343
344
345
346
347 public double[] getGradient() {
348 return gradient;
349 }
350
351
352
353
354 public double[][] getHessian() {
355 return hessian;
356 }
357
358 }
359
360 @Test
361 public void testRelativeNumericPrecision() {
362
363
364
365
366 int max = 50;
367 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
368 HolmesFeatherstoneAttractionModel model =
369 new HolmesFeatherstoneAttractionModel(itrf, provider);
370 Assert.assertTrue(model.dependsOnPositionOnly());
371
372
373
374
375
376
377
378
379
380
381 int digits = 30;
382 ReferenceFieldModelel.html#ReferenceFieldModel">ReferenceFieldModel refModel = new ReferenceFieldModel(provider, digits);
383
384 double r = 1.25;
385 for (double theta = 0.01; theta < 3.14; theta += 0.1) {
386 Vector3D position = new Vector3D(r * FastMath.sin(theta), 0.0, r * FastMath.cos(theta));
387 Dfp refValue = refModel.nonCentralPart(null, position);
388 double value = model.nonCentralPart(null, position, model.getMu());
389 double relativeError = error(refValue, value).divide(refValue).toDouble();
390 Assert.assertEquals(0, relativeError, 7.0e-15);
391 }
392
393 }
394
395 @Test
396 public void testValue() {
397
398 int max = 50;
399 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
400 HolmesFeatherstoneAttractionModel model =
401 new HolmesFeatherstoneAttractionModel(itrf, provider);
402
403 double r = 1.25;
404 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
405 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
406 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
407 r * FastMath.sin(theta) * FastMath.sin(lambda),
408 r * FastMath.cos(theta));
409 double refValue = provider.getMu() / position.getNorm() +
410 model.nonCentralPart(AbsoluteDate.GPS_EPOCH, position, model.getMu());
411 double value = model.value(AbsoluteDate.GPS_EPOCH, position, model.getMu());
412 Assert.assertEquals(refValue, value, 1.0e-15 * FastMath.abs(refValue));
413 }
414 }
415
416 }
417
418
419
420
421
422 @Test
423 public void RealFieldTest() {
424 DSFactory factory = new DSFactory(6, 4);
425 DerivativeStructure a_0 = factory.variable(0, 7201009.7124401);
426 DerivativeStructure e_0 = factory.variable(1, 1e-3);
427 DerivativeStructure i_0 = factory.variable(2, 98.7 * FastMath.PI / 180);
428 DerivativeStructure R_0 = factory.variable(3, 15.0 * 22.5 * FastMath.PI / 180);
429 DerivativeStructure O_0 = factory.variable(4, 93.0 * FastMath.PI / 180);
430 DerivativeStructure n_0 = factory.variable(5, 0.1);
431
432 Field<DerivativeStructure> field = a_0.getField();
433 DerivativeStructure zero = field.getZero();
434
435 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
436
437 Frame EME = FramesFactory.getEME2000();
438
439 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
440 PositionAngle.MEAN,
441 EME,
442 J2000,
443 zero.add(Constants.EIGEN5C_EARTH_MU));
444
445 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
446
447 SpacecraftState iSR = initialState.toSpacecraftState();
448 OrbitType type = OrbitType.EQUINOCTIAL;
449 double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
450
451
452 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
453 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
454 integrator.setInitialStepSize(zero.add(60));
455 AdaptiveStepsizeIntegrator RIntegrator =
456 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
457 RIntegrator.setInitialStepSize(60);
458
459 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
460 FNP.setOrbitType(type);
461 FNP.setInitialState(initialState);
462
463 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
464 NP.setOrbitType(type);
465 NP.setInitialState(iSR);
466
467 double[][] c = new double[3][1];
468 c[0][0] = 0.0;
469 c[2][0] = normalizedC20;
470 double[][] s = new double[3][1];
471 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
472 TideSystem.UNKNOWN,
473 c, s);
474 HolmesFeatherstoneAttractionModel forceModel =
475 new HolmesFeatherstoneAttractionModel(itrf, provider);
476
477 FNP.addForceModel(forceModel);
478 NP.addForceModel(forceModel);
479
480 FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(1005.);
481 FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
482 SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
483 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
484 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
485
486 Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getX(), finPVC_R.getPosition().getX(), FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
487 Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getY(), finPVC_R.getPosition().getY(), FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
488 Assert.assertEquals(finPVC_DS.toPVCoordinates().getPosition().getZ(), finPVC_R.getPosition().getZ(), FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
489
490 long number = 23091991;
491 RandomGenerator RG = new Well19937a(number);
492 GaussianRandomGenerator NGG = new GaussianRandomGenerator(RG);
493 UncorrelatedRandomVectorGenerator URVG = new UncorrelatedRandomVectorGenerator(new double[] {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 },
494 new double[] {1e1, 0.001, 0.001, 0.001, 0.001, 0.001},
495 NGG);
496 double a_R = a_0.getReal();
497 double e_R = e_0.getReal();
498 double i_R = i_0.getReal();
499 double R_R = R_0.getReal();
500 double O_R = O_0.getReal();
501 double n_R = n_0.getReal();
502 for (int ii = 0; ii < 1; ii++){
503 double[] rand_next = URVG.nextVector();
504 double a_shift = a_R + rand_next[0];
505 double e_shift = e_R + rand_next[1];
506 double i_shift = i_R + rand_next[2];
507 double R_shift = R_R + rand_next[3];
508 double O_shift = O_R + rand_next[4];
509 double n_shift = n_R + rand_next[5];
510
511 KeplerianOrbit shiftedOrb = new KeplerianOrbit(a_shift, e_shift, i_shift, R_shift, O_shift, n_shift,
512 PositionAngle.MEAN,
513 EME,
514 J2000.toAbsoluteDate(),
515 Constants.EIGEN5C_EARTH_MU
516 );
517
518 SpacecraftState shift_iSR = new SpacecraftState(shiftedOrb);
519
520 NumericalPropagator shift_NP = new NumericalPropagator(RIntegrator);
521 shift_NP.setOrbitType(type);
522
523 shift_NP.setInitialState(shift_iSR);
524
525 shift_NP.addForceModel(forceModel);
526
527 SpacecraftState finalState_shift = shift_NP.propagate(target.toAbsoluteDate());
528
529
530 PVCoordinates finPVC_shift = finalState_shift.getPVCoordinates();
531
532
533
534 FieldVector3D<DerivativeStructure> pos_DS = finPVC_DS.getPosition();
535 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]);
536 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]);
537 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]);
538
539
540 double x = finPVC_shift.getPosition().getX();
541 double y = finPVC_shift.getPosition().getY();
542 double z = finPVC_shift.getPosition().getZ();
543 Assert.assertEquals(x_DS, x, FastMath.abs(x - pos_DS.getX().getReal()) * 1e-8);
544 Assert.assertEquals(y_DS, y, FastMath.abs(y - pos_DS.getY().getReal()) * 1e-8);
545 Assert.assertEquals(z_DS, z, FastMath.abs(z - pos_DS.getZ().getReal()) * 1e-8);
546
547
548
549 FieldVector3D<DerivativeStructure> vel_DS = finPVC_DS.getVelocity();
550 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]);
551 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]);
552 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]);
553 double vx = finPVC_shift.getVelocity().getX();
554 double vy = finPVC_shift.getVelocity().getY();
555 double vz = finPVC_shift.getVelocity().getZ();
556 Assert.assertEquals(vx_DS, vx, FastMath.abs(vx) * 1e-9);
557 Assert.assertEquals(vy_DS, vy, FastMath.abs(vy) * 1e-9);
558 Assert.assertEquals(vz_DS, vz, FastMath.abs(vz) * 1e-9);
559
560
561 FieldVector3D<DerivativeStructure> acc_DS = finPVC_DS.getAcceleration();
562 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]);
563 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]);
564 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]);
565 double ax = finPVC_shift.getAcceleration().getX();
566 double ay = finPVC_shift.getAcceleration().getY();
567 double az = finPVC_shift.getAcceleration().getZ();
568 Assert.assertEquals(ax_DS, ax, FastMath.abs(ax) * 1e-9);
569 Assert.assertEquals(ay_DS, ay, FastMath.abs(ay) * 1e-9);
570 Assert.assertEquals(az_DS, az, FastMath.abs(az) * 1e-9);
571 }
572 }
573
574
575
576
577
578 @Test
579 public void RealFieldExpectErrorTest() {
580 DSFactory factory = new DSFactory(6, 0);
581 DerivativeStructure a_0 = factory.variable(0, 7201009.7124401);
582 DerivativeStructure e_0 = factory.variable(1, 1e-3);
583 DerivativeStructure i_0 = factory.variable(2, 98.7 * FastMath.PI / 180);
584 DerivativeStructure R_0 = factory.variable(3, 15.0 * 22.5 * FastMath.PI / 180);
585 DerivativeStructure O_0 = factory.variable(4, 93.0 * FastMath.PI / 180);
586 DerivativeStructure n_0 = factory.variable(5, 0.1);
587
588 Field<DerivativeStructure> field = a_0.getField();
589 DerivativeStructure zero = field.getZero();
590
591 FieldAbsoluteDate<DerivativeStructure> J2000 = new FieldAbsoluteDate<>(field);
592
593 Frame EME = FramesFactory.getEME2000();
594 FieldKeplerianOrbit<DerivativeStructure> FKO = new FieldKeplerianOrbit<>(a_0, e_0, i_0, R_0, O_0, n_0,
595 PositionAngle.MEAN,
596 EME,
597 J2000,
598 zero.add(Constants.EIGEN5C_EARTH_MU));
599
600 FieldSpacecraftState<DerivativeStructure> initialState = new FieldSpacecraftState<>(FKO);
601
602 SpacecraftState iSR = initialState.toSpacecraftState();
603 OrbitType type = OrbitType.EQUINOCTIAL;
604 double[][] tolerance = NumericalPropagator.tolerances(10.0, FKO.toOrbit(), type);
605
606
607 AdaptiveStepsizeFieldIntegrator<DerivativeStructure> integrator =
608 new DormandPrince853FieldIntegrator<>(field, 0.001, 200, tolerance[0], tolerance[1]);
609 integrator.setInitialStepSize(zero.add(60));
610 AdaptiveStepsizeIntegrator RIntegrator =
611 new DormandPrince853Integrator(0.001, 200, tolerance[0], tolerance[1]);
612 RIntegrator.setInitialStepSize(60);
613
614 FieldNumericalPropagator<DerivativeStructure> FNP = new FieldNumericalPropagator<>(field, integrator);
615 FNP.setOrbitType(type);
616 FNP.setInitialState(initialState);
617
618 NumericalPropagator NP = new NumericalPropagator(RIntegrator);
619 NP.setOrbitType(type);
620 NP.setInitialState(iSR);
621
622 double[][] c = new double[3][1];
623 c[0][0] = 0.0;
624 c[2][0] = normalizedC20;
625 double[][] s = new double[3][1];
626 NormalizedSphericalHarmonicsProvider provider = GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
627 TideSystem.UNKNOWN,
628 c, s);
629 HolmesFeatherstoneAttractionModel forceModel =
630 new HolmesFeatherstoneAttractionModel(itrf, provider);
631
632
633 NP.addForceModel(forceModel);
634
635 FieldAbsoluteDate<DerivativeStructure> target = J2000.shiftedBy(100.);
636 FieldSpacecraftState<DerivativeStructure> finalState_DS = FNP.propagate(target);
637 SpacecraftState finalState_R = NP.propagate(target.toAbsoluteDate());
638 FieldPVCoordinates<DerivativeStructure> finPVC_DS = finalState_DS.getPVCoordinates();
639 PVCoordinates finPVC_R = finalState_R.getPVCoordinates();
640 Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getX() - finPVC_R.getPosition().getX()) < FastMath.abs(finPVC_R.getPosition().getX()) * 1e-11);
641 Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getY() - finPVC_R.getPosition().getY()) < FastMath.abs(finPVC_R.getPosition().getY()) * 1e-11);
642 Assert.assertFalse(FastMath.abs(finPVC_DS.toPVCoordinates().getPosition().getZ() - finPVC_R.getPosition().getZ()) < FastMath.abs(finPVC_R.getPosition().getZ()) * 1e-11);
643 }
644 @Test
645 public void testGradient() {
646
647 int max = 50;
648 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
649 HolmesFeatherstoneAttractionModel model =
650 new HolmesFeatherstoneAttractionModel(itrf, provider);
651
652 double r = 1.25;
653 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
654 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
655 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
656 r * FastMath.sin(theta) * FastMath.sin(lambda),
657 r * FastMath.cos(theta));
658 double[] refGradient = gradient(model, null, position, 1.0e-3);
659 double norm = FastMath.sqrt(refGradient[0] * refGradient[0] +
660 refGradient[1] * refGradient[1] +
661 refGradient[2] * refGradient[2]);
662 double[] gradient = model.gradient(null, position, model.getMu());
663 double errorX = refGradient[0] - gradient[0];
664 double errorY = refGradient[1] - gradient[1];
665 double errorZ = refGradient[2] - gradient[2];
666 double relativeError = FastMath.sqrt(errorX * errorX + errorY * errorY + errorZ * errorZ) /
667 norm;
668 Assert.assertEquals(0, relativeError, 3.0e-12);
669 }
670 }
671
672 }
673
674 @Test
675 public void testHessian() {
676
677 int max = 50;
678 NormalizedSphericalHarmonicsProvider provider = new GleasonProvider(max, max);
679 HolmesFeatherstoneAttractionModel model =
680 new HolmesFeatherstoneAttractionModel(itrf, provider);
681
682 double r = 1.25;
683 for (double lambda = 0; lambda < 2 * FastMath.PI; lambda += 0.5) {
684 for (double theta = 0.05; theta < 3.11; theta += 0.03) {
685 Vector3D position = new Vector3D(r * FastMath.sin(theta) * FastMath.cos(lambda),
686 r * FastMath.sin(theta) * FastMath.sin(lambda),
687 r * FastMath.cos(theta));
688 double[][] refHessian = hessian(model, null, position, 1.0e-3);
689 double[][] hessian = gradientHessian(model, null, position).getHessian();
690 double normH2 = 0;
691 double normE2 = 0;
692 for (int i = 0; i < 3; ++i) {
693 for (int j = 0; j < 3; ++j) {
694 double error = refHessian[i][j] - hessian[i][j];
695 normH2 += refHessian[i][j] * refHessian[i][j];
696 normE2 += error * error;
697 }
698 }
699 Assert.assertEquals(0, FastMath.sqrt(normE2 / normH2), 5.0e-12);
700 }
701 }
702
703 }
704
705 private Dfp error(Dfp refValue, double value) {
706 return refValue.getField().newDfp(value).subtract(refValue);
707 }
708
709 private double[] gradient(final HolmesFeatherstoneAttractionModel model,
710 final AbsoluteDate date, final Vector3D position, final double h)
711 {
712 return new double[] {
713 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_I)), model.getMu()),
714 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_I)), model.getMu()),
715 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_I)), model.getMu()),
716 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_I)), model.getMu()),
717 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_I)), model.getMu()),
718 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_I)), model.getMu()),
719 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_I)), model.getMu()),
720 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_I)), model.getMu()),
721 h),
722 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_J)), model.getMu()),
723 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_J)), model.getMu()),
724 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_J)), model.getMu()),
725 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_J)), model.getMu()),
726 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_J)), model.getMu()),
727 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_J)), model.getMu()),
728 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_J)), model.getMu()),
729 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_J)), model.getMu()),
730 h),
731 differential8(model.nonCentralPart(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_K)), model.getMu()),
732 model.nonCentralPart(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_K)), model.getMu()),
733 model.nonCentralPart(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_K)), model.getMu()),
734 model.nonCentralPart(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_K)), model.getMu()),
735 model.nonCentralPart(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_K)), model.getMu()),
736 model.nonCentralPart(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_K)), model.getMu()),
737 model.nonCentralPart(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_K)), model.getMu()),
738 model.nonCentralPart(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_K)), model.getMu()),
739 h)
740 };
741 }
742
743 private double[][] hessian(final HolmesFeatherstoneAttractionModel model,
744 final AbsoluteDate date, final Vector3D position, final double h)
745 {
746 return new double[][] {
747 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_I)), model.getMu()),
748 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_I)), model.getMu()),
749 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_I)), model.getMu()),
750 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_I)), model.getMu()),
751 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_I)), model.getMu()),
752 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_I)), model.getMu()),
753 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_I)), model.getMu()),
754 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_I)), model.getMu()),
755 h),
756 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_J)), model.getMu()),
757 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_J)), model.getMu()),
758 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_J)), model.getMu()),
759 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_J)), model.getMu()),
760 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_J)), model.getMu()),
761 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_J)), model.getMu()),
762 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_J)), model.getMu()),
763 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_J)), model.getMu()),
764 h),
765 differential8(model.gradient(date, position.add(new Vector3D(-4 * h, Vector3D.PLUS_K)), model.getMu()),
766 model.gradient(date, position.add(new Vector3D(-3 * h, Vector3D.PLUS_K)), model.getMu()),
767 model.gradient(date, position.add(new Vector3D(-2 * h, Vector3D.PLUS_K)), model.getMu()),
768 model.gradient(date, position.add(new Vector3D(-1 * h, Vector3D.PLUS_K)), model.getMu()),
769 model.gradient(date, position.add(new Vector3D(+1 * h, Vector3D.PLUS_K)), model.getMu()),
770 model.gradient(date, position.add(new Vector3D(+2 * h, Vector3D.PLUS_K)), model.getMu()),
771 model.gradient(date, position.add(new Vector3D(+3 * h, Vector3D.PLUS_K)), model.getMu()),
772 model.gradient(date, position.add(new Vector3D(+4 * h, Vector3D.PLUS_K)), model.getMu()),
773 h)
774 };
775 }
776
777
778
779
780
781
782
783 private static class GleasonProvider implements NormalizedSphericalHarmonicsProvider {
784
785 private final int degree;
786 private final int order;
787
788 public GleasonProvider(int degree, int order) {
789 this.degree = degree;
790 this.order = order;
791 }
792
793 public int getMaxDegree() {
794 return degree;
795 }
796
797 public int getMaxOrder() {
798 return order;
799 }
800
801 public double getMu() {
802 return 1;
803 }
804
805 public double getAe() {
806 return 1;
807 }
808
809 public AbsoluteDate getReferenceDate() {
810 return null;
811 }
812
813 public double getOffset(AbsoluteDate date) {
814 return 0;
815 }
816
817 public TideSystem getTideSystem() {
818 return TideSystem.UNKNOWN;
819 }
820
821 @Override
822 public NormalizedSphericalHarmonics onDate(final AbsoluteDate date) {
823 return new NormalizedSphericalHarmonics() {
824 @Override
825 public double getNormalizedCnm(int n, int m) {
826 return 1;
827 }
828
829 @Override
830 public double getNormalizedSnm(int n, int m) {
831 return 1;
832 }
833
834 @Override
835 public AbsoluteDate getDate() {
836 return date;
837 }
838 };
839 }
840
841 }
842
843
844 @Test
845 public void testHelioSynchronous()
846 {
847
848
849 AbsoluteDate date = new AbsoluteDate(new DateComponents(1970, 07, 01),
850 new TimeComponents(13, 59, 27.816),
851 TimeScalesFactory.getUTC());
852 Transform itrfToEME2000 = itrf.getTransformTo(FramesFactory.getEME2000(), date);
853 Vector3D pole = itrfToEME2000.transformVector(Vector3D.PLUS_K);
854 Frame poleAligned = new Frame(FramesFactory.getEME2000(),
855 new Transform(date, new Rotation(pole, Vector3D.PLUS_K)),
856 "pole aligned", true);
857
858 double i = FastMath.toRadians(98.7);
859 double omega = FastMath.toRadians(93.0);
860 double OMEGA = FastMath.toRadians(15.0 * 22.5);
861 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
862 0, PositionAngle.MEAN, poleAligned, date, mu);
863 double[][] c = new double[3][1];
864 c[0][0] = 0.0;
865 c[2][0] = normalizedC20;
866 double[][] s = new double[3][1];
867 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(itrf,
868 GravityFieldFactory.getNormalizedProvider(6378136.460, mu,
869 TideSystem.UNKNOWN,
870 c, s)));
871
872
873 propagator.setMasterMode(Constants.JULIAN_DAY, new SpotStepHandler(date, mu));
874 propagator.setInitialState(new SpacecraftState(orbit));
875 propagator.propagate(date.shiftedBy(7 * Constants.JULIAN_DAY));
876 Assert.assertTrue(propagator.getCalls() < 9200);
877
878 }
879
880 private static class SpotStepHandler implements OrekitFixedStepHandler {
881
882 public SpotStepHandler(AbsoluteDate date, double mu) {
883 sun = CelestialBodyFactory.getSun();
884 previous = Double.NaN;
885 }
886
887 private PVCoordinatesProvider sun;
888 private double previous;
889 public void handleStep(SpacecraftState currentState, boolean isLast)
890 {
891
892
893 AbsoluteDate current = currentState.getDate();
894 Vector3D sunPos = sun.getPVCoordinates(current , FramesFactory.getEME2000()).getPosition();
895 Vector3D normal = currentState.getPVCoordinates().getMomentum();
896 double angle = Vector3D.angle(sunPos , normal);
897 if (! Double.isNaN(previous)) {
898 Assert.assertEquals(previous, angle, 0.0013);
899 }
900 previous = angle;
901 }
902
903 }
904
905
906 @Test
907 public void testEcksteinHechlerReference()
908 {
909
910
911 AbsoluteDate date = AbsoluteDate.J2000_EPOCH.shiftedBy(584.);
912 Vector3D position = new Vector3D(3220103., 69623., 6449822.);
913 Vector3D velocity = new Vector3D(6414.7, -2006., -3180.);
914
915 Transform itrfToEME2000 = itrf.getTransformTo(FramesFactory.getEME2000(), date);
916 Vector3D pole = itrfToEME2000.transformVector(Vector3D.PLUS_K);
917 Frame poleAligned = new Frame(FramesFactory.getEME2000(),
918 new Transform(date, new Rotation(pole, Vector3D.PLUS_K)),
919 "pole aligned", true);
920
921 Orbit initialOrbit = new EquinoctialOrbit(new PVCoordinates(position, velocity),
922 poleAligned, date, mu);
923
924 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(itrf,
925 GravityFieldFactory.getNormalizedProvider(ae, mu,
926 TideSystem.UNKNOWN,
927 new double[][] {
928 { 0.0 }, { 0.0 }, { normalizedC20 }, { normalizedC30 },
929 { normalizedC40 }, { normalizedC50 }, { normalizedC60 },
930 },
931 new double[][] {
932 { 0.0 }, { 0.0 }, { 0.0 }, { 0.0 },
933 { 0.0 }, { 0.0 }, { 0.0 },
934 })));
935
936
937 propagator.setInitialState(new SpacecraftState(initialOrbit));
938 propagator.setMasterMode(20, new EckStepHandler(initialOrbit, ae,
939 unnormalizedC20, unnormalizedC30, unnormalizedC40,
940 unnormalizedC50, unnormalizedC60));
941 propagator.propagate(date.shiftedBy(50000));
942 Assert.assertTrue(propagator.getCalls() < 1100);
943
944 }
945
946 private static class EckStepHandler implements OrekitFixedStepHandler {
947
948
949 private static final double mu = 3.986004415e+14;
950
951 private EckStepHandler(Orbit initialOrbit, double ae,
952 double c20, double c30, double c40, double c50, double c60)
953 {
954 referencePropagator =
955 new EcksteinHechlerPropagator(initialOrbit,
956 ae, mu, c20, c30, c40, c50, c60);
957 }
958
959 private EcksteinHechlerPropagator referencePropagator;
960 public void handleStep(SpacecraftState currentState, boolean isLast) {
961
962 SpacecraftState EHPOrbit = referencePropagator.propagate(currentState.getDate());
963 Vector3D posEHP = EHPOrbit.getPVCoordinates().getPosition();
964 Vector3D posDROZ = currentState.getPVCoordinates().getPosition();
965 Vector3D velEHP = EHPOrbit.getPVCoordinates().getVelocity();
966 Vector3D dif = posEHP.subtract(posDROZ);
967
968 Vector3D T = new Vector3D(1 / velEHP.getNorm(), velEHP);
969 Vector3D W = EHPOrbit.getPVCoordinates().getMomentum().normalize();
970 Vector3D N = Vector3D.crossProduct(W, T);
971
972 Assert.assertTrue(dif.getNorm() < 111);
973 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, T)) < 111);
974 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, N)) < 54);
975 Assert.assertTrue(FastMath.abs(Vector3D.dotProduct(dif, W)) < 12);
976
977 }
978
979 }
980
981 @Test
982 public void testParameterDerivative() {
983
984 Utils.setDataRoot("regular-data:potential/grgs-format");
985 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
986
987
988 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
989 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
990 final SpacecraftState state =
991 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
992 FramesFactory.getGCRF(),
993 new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
994 GravityFieldFactory.getUnnormalizedProvider(1, 1).getMu()));
995
996 final HolmesFeatherstoneAttractionModel holmesFeatherstoneModel =
997 new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true),
998 GravityFieldFactory.getNormalizedProvider(20, 20));
999
1000 final String name = NewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT;
1001 checkParameterDerivative(state, holmesFeatherstoneModel, name, 1.0e-5, 5.0e-12);
1002
1003 }
1004
1005 @Test
1006 public void testTimeDependentField() {
1007
1008 Utils.setDataRoot("regular-data:potential/icgem-format");
1009 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
1010
1011 final Vector3D pos = new Vector3D(6.46885878304673824e+06, -1.88050918456274318e+06, -1.32931592294715829e+04);
1012 final Vector3D vel = new Vector3D(2.14718074509906819e+03, 7.38239351251748485e+03, -1.14097953925384523e+01);
1013 final SpacecraftState spacecraftState =
1014 new SpacecraftState(new CartesianOrbit(new PVCoordinates(pos, vel),
1015 FramesFactory.getGCRF(),
1016 new AbsoluteDate(2005, 3, 5, 0, 24, 0.0, TimeScalesFactory.getTAI()),
1017 GravityFieldFactory.getUnnormalizedProvider(1, 1).getMu()));
1018
1019 double dP = 0.1;
1020 double duration = 3 * Constants.JULIAN_DAY;
1021 BoundedPropagator fixedFieldEphemeris = createEphemeris(dP, spacecraftState, duration,
1022 GravityFieldFactory.getConstantNormalizedProvider(8, 8));
1023 BoundedPropagator varyingFieldEphemeris = createEphemeris(dP, spacecraftState, duration,
1024 GravityFieldFactory.getNormalizedProvider(8, 8));
1025
1026 double step = 60.0;
1027 double maxDeltaT = 0;
1028 double maxDeltaN = 0;
1029 double maxDeltaW = 0;
1030 for (AbsoluteDate date = fixedFieldEphemeris.getMinDate();
1031 date.compareTo(fixedFieldEphemeris.getMaxDate()) < 0;
1032 date = date.shiftedBy(step)) {
1033 PVCoordinates pvFixedField = fixedFieldEphemeris.getPVCoordinates(date, FramesFactory.getGCRF());
1034 PVCoordinates pvVaryingField = varyingFieldEphemeris.getPVCoordinates(date, FramesFactory.getGCRF());
1035 Vector3D t = pvFixedField.getVelocity().normalize();
1036 Vector3D w = pvFixedField.getMomentum().normalize();
1037 Vector3D n = Vector3D.crossProduct(w, t);
1038 Vector3D delta = pvVaryingField.getPosition().subtract(pvFixedField.getPosition());
1039 maxDeltaT = FastMath.max(maxDeltaT, FastMath.abs(Vector3D.dotProduct(delta, t)));
1040 maxDeltaN = FastMath.max(maxDeltaN, FastMath.abs(Vector3D.dotProduct(delta, n)));
1041 maxDeltaW = FastMath.max(maxDeltaW, FastMath.abs(Vector3D.dotProduct(delta, w)));
1042 }
1043 Assert.assertTrue(maxDeltaT > 0.15);
1044 Assert.assertTrue(maxDeltaT < 0.25);
1045 Assert.assertTrue(maxDeltaN > 0.01);
1046 Assert.assertTrue(maxDeltaN < 0.02);
1047 Assert.assertTrue(maxDeltaW > 0.05);
1048 Assert.assertTrue(maxDeltaW < 0.10);
1049
1050 }
1051
1052 private BoundedPropagator createEphemeris(double dP, SpacecraftState initialState, double duration,
1053 NormalizedSphericalHarmonicsProvider provider)
1054 {
1055 double[][] tol = NumericalPropagator.tolerances(dP, initialState.getOrbit(), OrbitType.CARTESIAN);
1056 AbstractIntegrator integrator =
1057 new DormandPrince853Integrator(0.001, 120.0, tol[0], tol[1]);
1058 NumericalPropagator propagator = new NumericalPropagator(integrator);
1059 propagator.setEphemerisMode();
1060 propagator.setOrbitType(OrbitType.CARTESIAN);
1061 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), provider));
1062 propagator.setInitialState(initialState);
1063 propagator.propagate(initialState.getDate().shiftedBy(duration));
1064 return propagator.getGeneratedEphemeris();
1065 }
1066
1067 @Test
1068 public void testStateJacobian()
1069 {
1070
1071 Utils.setDataRoot("regular-data:potential/grgs-format");
1072 GravityFieldFactory.addPotentialCoefficientsReader(new GRGSFormatReader("grim4s4_gr", true));
1073
1074
1075 AbsoluteDate date = new AbsoluteDate(new DateComponents(2000, 07, 01),
1076 new TimeComponents(13, 59, 27.816),
1077 TimeScalesFactory.getUTC());
1078 double i = FastMath.toRadians(98.7);
1079 double omega = FastMath.toRadians(93.0);
1080 double OMEGA = FastMath.toRadians(15.0 * 22.5);
1081 Orbit orbit = new KeplerianOrbit(7201009.7124401, 1e-3, i , omega, OMEGA,
1082 0, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu);
1083 OrbitType integrationType = OrbitType.CARTESIAN;
1084 double[][] tolerances = NumericalPropagator.tolerances(0.01, orbit, integrationType);
1085
1086 propagator = new NumericalPropagator(new DormandPrince853Integrator(1.0e-3, 120,
1087 tolerances[0], tolerances[1]));
1088 propagator.setOrbitType(integrationType);
1089 HolmesFeatherstoneAttractionModel hfModel =
1090 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1091 Assert.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1092 propagator.addForceModel(hfModel);
1093 SpacecraftState state0 = new SpacecraftState(orbit);
1094 propagator.setInitialState(state0);
1095
1096 checkStateJacobian(propagator, state0, date.shiftedBy(3.5 * 3600.0),
1097 50000, tolerances[0], 7.8e-6);
1098 }
1099
1100 @Test
1101 public void testStateJacobianVs80Implementation()
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, 07, 01),
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, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu);
1116
1117 HolmesFeatherstoneAttractionModel hfModel =
1118 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1119 Assert.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1120 SpacecraftState state = new SpacecraftState(orbit);
1121
1122 checkStateJacobianVs80Implementation(state, hfModel,
1123 new LofOffset(state.getFrame(), LOFType.VVLH),
1124 2.0e-15, false);
1125
1126 }
1127
1128 @Test
1129 public 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, 07, 01),
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, PositionAngle.MEAN, FramesFactory.getEME2000(), date, mu);
1144
1145 HolmesFeatherstoneAttractionModel hfModel =
1146 new HolmesFeatherstoneAttractionModel(itrf, GravityFieldFactory.getNormalizedProvider(50, 50));
1147 Assert.assertEquals(TideSystem.UNKNOWN, hfModel.getTideSystem());
1148 SpacecraftState state = new SpacecraftState(orbit);
1149
1150 checkStateJacobianVsFiniteDifferences(state, hfModel, Propagator.DEFAULT_LAW,
1151 10.0, 2.0e-10, false);
1152
1153 }
1154
1155 @Before
1156 public void setUp() {
1157 itrf = null;
1158 propagator = null;
1159 Utils.setDataRoot("regular-data");
1160 try {
1161
1162 mu = 3.986004415e+14;
1163 ae = 6378136.460;
1164 normalizedC20 = -4.84165299820e-04;
1165 normalizedC30 = 9.57211326674e-07;
1166 normalizedC40 = 5.39990167207e-07;
1167 normalizedC50 = 6.86846073356e-08 ;
1168 normalizedC60 = -1.49953256913e-07;
1169 unnormalizedC20 = FastMath.sqrt( 5) * normalizedC20;
1170 unnormalizedC30 = FastMath.sqrt( 7) * normalizedC30;
1171 unnormalizedC40 = FastMath.sqrt( 9) * normalizedC40;
1172 unnormalizedC50 = FastMath.sqrt(11) * normalizedC50;
1173 unnormalizedC60 = FastMath.sqrt(13) * normalizedC60;
1174
1175 itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, true);
1176 double[] absTolerance = {
1177 0.001, 1.0e-9, 1.0e-9, 1.0e-6, 1.0e-6, 1.0e-6, 0.001
1178 };
1179 double[] relTolerance = {
1180 1.0e-7, 1.0e-4, 1.0e-4, 1.0e-7, 1.0e-7, 1.0e-7, 1.0e-7
1181 };
1182 AdaptiveStepsizeIntegrator integrator =
1183 new DormandPrince853Integrator(0.001, 1000, absTolerance, relTolerance);
1184 integrator.setInitialStepSize(60);
1185 propagator = new NumericalPropagator(integrator);
1186 } catch (OrekitException oe) {
1187 Assert.fail(oe.getMessage());
1188 }
1189 }
1190
1191 @After
1192 public void tearDown() {
1193 itrf = null;
1194 propagator = null;
1195 }
1196
1197 private double unnormalizedC20;
1198 private double unnormalizedC30;
1199 private double unnormalizedC40;
1200 private double unnormalizedC50;
1201 private double unnormalizedC60;
1202 private double normalizedC20;
1203 private double normalizedC30;
1204 private double normalizedC40;
1205 private double normalizedC50;
1206 private double normalizedC60;
1207 private double mu;
1208 private double ae;
1209
1210 private Frame itrf;
1211 private NumericalPropagator propagator;
1212
1213 }
1214
1215