1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.semianalytical.dsst;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.Gradient;
22 import org.hipparchus.util.Binary64Field;
23 import org.hipparchus.util.FastMath;
24 import org.hipparchus.util.MathArrays;
25 import org.junit.jupiter.api.Assertions;
26 import org.junit.jupiter.api.BeforeEach;
27 import org.junit.jupiter.api.Test;
28 import org.orekit.Utils;
29 import org.orekit.attitudes.Attitude;
30 import org.orekit.bodies.CelestialBodyFactory;
31 import org.orekit.frames.Frame;
32 import org.orekit.frames.FramesFactory;
33 import org.orekit.orbits.EquinoctialOrbit;
34 import org.orekit.orbits.FieldEquinoctialOrbit;
35 import org.orekit.orbits.FieldOrbit;
36 import org.orekit.orbits.Orbit;
37 import org.orekit.orbits.OrbitType;
38 import org.orekit.orbits.PositionAngleType;
39 import org.orekit.propagation.FieldSpacecraftState;
40 import org.orekit.propagation.PropagationType;
41 import org.orekit.propagation.SpacecraftState;
42 import org.orekit.propagation.ToleranceProvider;
43 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
44 import org.orekit.propagation.semianalytical.dsst.forces.DSSTNewtonianAttraction;
45 import org.orekit.propagation.semianalytical.dsst.forces.DSSTThirdBody;
46 import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
47 import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
48 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
49 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
50 import org.orekit.time.AbsoluteDate;
51 import org.orekit.time.DateComponents;
52 import org.orekit.time.FieldAbsoluteDate;
53 import org.orekit.time.TimeComponents;
54 import org.orekit.time.TimeScalesFactory;
55 import org.orekit.utils.ParameterDriver;
56 import org.orekit.utils.ParameterDriversList;
57
58 import java.io.IOException;
59 import java.text.ParseException;
60 import java.util.ArrayList;
61 import java.util.Arrays;
62 import java.util.Collection;
63 import java.util.List;
64
65 public class FieldDSSTThirdBodyTest {
66
67 private static final double eps = 3.5e-25;
68
69 @Test
70 public void testGetMeanElementRate() {
71 doTestGetMeanElementRate(Binary64Field.getInstance());
72 }
73
74 private <T extends CalculusFieldElement<T>> void doTestGetMeanElementRate(final Field<T> field) {
75
76 final T zero = field.getZero();
77
78 final Frame earthFrame = FramesFactory.getEME2000();
79 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, 2003, 07, 01, 0, 0, 00.000, TimeScalesFactory.getUTC());
80
81 final double mu = 3.986004415E14;
82
83
84
85
86
87
88 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(4.2163393E7),
89 zero.add(-0.25925449177598586),
90 zero.add(-0.06946703170551687),
91 zero.add(0.15995912655021305),
92 zero.add(-0.5969755874197339),
93 zero.add(15.47576793123677),
94 PositionAngleType.TRUE,
95 earthFrame,
96 initDate,
97 zero.add(mu));
98
99 final T mass = zero.add(1000.0);
100 final FieldSpacecraftState<T> state = new FieldSpacecraftState<>(orbit, mass);
101
102 final DSSTForceModel moon = new DSSTThirdBody(CelestialBodyFactory.getMoon(), mu);
103
104 final FieldAuxiliaryElements<T> auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), 1);
105
106
107 final T[] parameters = moon.getParameters(field, state.getDate());
108
109 moon.initializeShortPeriodTerms(auxiliaryElements,
110 PropagationType.MEAN, parameters);
111
112 final T[] elements = MathArrays.buildArray(field, 7);
113 Arrays.fill(elements, zero);
114
115 final T[] daidt = moon.getMeanElementRate(state, auxiliaryElements, parameters);
116 for (int i = 0; i < daidt.length; i++) {
117 elements[i] = daidt[i];
118 }
119
120 Assertions.assertEquals(0.0, elements[0].getReal(), eps);
121 Assertions.assertEquals(4.346622384804537E-10, elements[1].getReal(), eps);
122 Assertions.assertEquals(7.293879548440941E-10, elements[2].getReal(), eps);
123 Assertions.assertEquals(7.465699631747887E-11, elements[3].getReal(), eps);
124 Assertions.assertEquals(3.9170221137233836E-10, elements[4].getReal(), eps);
125 Assertions.assertEquals(-3.178319341840074E-10, elements[5].getReal(), eps);
126
127 }
128
129 @Test
130 public void testShortPeriodTerms() {
131 doTestShortPeriodTerms(Binary64Field.getInstance());
132 }
133
134 @SuppressWarnings("unchecked")
135 private <T extends CalculusFieldElement<T>> void doTestShortPeriodTerms(final Field<T> field) {
136 final T zero = field.getZero();
137
138 final FieldSpacecraftState<T> meanState = getGEOState(field);
139
140 final DSSTForceModel moon = new DSSTThirdBody(CelestialBodyFactory.getMoon(), meanState.getOrbit().getMu().getReal());
141
142 final Collection<DSSTForceModel> forces = new ArrayList<DSSTForceModel>();
143 forces.add(moon);
144
145
146 final FieldAuxiliaryElements<T> aux = new FieldAuxiliaryElements<>(meanState.getOrbit(), 1);
147
148
149 final List<FieldShortPeriodTerms<T>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<T>>();
150
151 for (final DSSTForceModel force : forces) {
152 force.registerAttitudeProvider(null);
153 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(aux, PropagationType.OSCULATING, force.getParameters(field, meanState.getDate())));
154 force.updateShortPeriodTerms(force.getParametersAllValues(field), meanState);
155 }
156
157 T[] y = MathArrays.buildArray(field, 6);
158 Arrays.fill(y, zero);
159 for (final FieldShortPeriodTerms<T> spt : shortPeriodTerms) {
160 final T[] shortPeriodic = spt.value(meanState.getOrbit());
161 for (int i = 0; i < shortPeriodic.length; i++) {
162 y[i] = y[i].add(shortPeriodic[i]);
163 }
164 }
165
166 Assertions.assertEquals(-413.20633326933154, y[0].getReal(), 1.0e-15);
167 Assertions.assertEquals(-1.8060137920197483E-5, y[1].getReal(), 1.0e-20);
168 Assertions.assertEquals(-2.8416367511811057E-5, y[2].getReal(), 1.4e-20);
169 Assertions.assertEquals(-2.791424363476855E-6, y[3].getReal(), 1.0e-21);
170 Assertions.assertEquals(1.8817187527805853E-6, y[4].getReal(), 1.0e-21);
171 Assertions.assertEquals(-3.423664701811889E-5, y[5].getReal(), 1.0e-20);
172
173 }
174
175 @Test
176 @SuppressWarnings("unchecked")
177 public void testShortPeriodTermsStateDerivatives() {
178
179
180 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
181 TimeScalesFactory.getUTC());
182
183 final Orbit orbit = new EquinoctialOrbit(42164000,
184 10e-3,
185 10e-3,
186 FastMath.tan(0.001745329) * FastMath.cos(2 * FastMath.PI / 3),
187 FastMath.tan(0.001745329) * FastMath.sin(2 * FastMath.PI / 3), 0.1,
188 PositionAngleType.TRUE,
189 FramesFactory.getEME2000(),
190 initDate,
191 3.986004415E14);
192
193 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
194
195 final SpacecraftState meanState = new SpacecraftState(orbit);
196
197
198 final Collection<DSSTForceModel> forces = new ArrayList<DSSTForceModel>();
199 final DSSTForceModel moon = new DSSTThirdBody(CelestialBodyFactory.getMoon(), meanState.getOrbit().getMu());
200 final DSSTForceModel sun = new DSSTThirdBody(CelestialBodyFactory.getSun(), meanState.getOrbit().getMu());
201 forces.add(moon);
202 forces.add(sun);
203
204
205 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
206
207
208 final double[][] shortPeriodJacobian = new double[6][6];
209 for (DSSTForceModel force : forces) {
210
211
212 final FieldSpacecraftState<Gradient> dsState = converter.getState(force);
213
214 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
215
216
217 final Gradient[] shortPeriod = new Gradient[6];
218 final Gradient zero = dsState.getOrbit().getA().getField().getZero();
219 Arrays.fill(shortPeriod, zero);
220
221 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
222 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING,
223 converter.getParametersAtStateDate(dsState, force)));
224 force.updateShortPeriodTerms(converter.getParameters(dsState, force), dsState);
225
226 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
227 final Gradient[] spVariation = spt.value(dsState.getOrbit());
228 for (int i = 0; i < spVariation .length; i++) {
229 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
230 }
231 }
232
233 final double[] derivativesASP = shortPeriod[0].getGradient();
234 final double[] derivativesExSP = shortPeriod[1].getGradient();
235 final double[] derivativesEySP = shortPeriod[2].getGradient();
236 final double[] derivativesHxSP = shortPeriod[3].getGradient();
237 final double[] derivativesHySP = shortPeriod[4].getGradient();
238 final double[] derivativesLSP = shortPeriod[5].getGradient();
239
240
241 addToRow(derivativesASP, 0, shortPeriodJacobian);
242 addToRow(derivativesExSP, 1, shortPeriodJacobian);
243 addToRow(derivativesEySP, 2, shortPeriodJacobian);
244 addToRow(derivativesHxSP, 3, shortPeriodJacobian);
245 addToRow(derivativesHySP, 4, shortPeriodJacobian);
246 addToRow(derivativesLSP, 5, shortPeriodJacobian);
247
248 }
249
250
251 double[][] shortPeriodJacobianRef = new double[6][6];
252 double dP = 0.001;
253 double[] steps = ToleranceProvider.getDefaultToleranceProvider(1000000 * dP).getTolerances(orbit, orbitType)[0];
254 for (int i = 0; i < 6; i++) {
255
256 SpacecraftState stateM4 = shiftState(meanState, orbitType, -4 * steps[i], i);
257 double[] shortPeriodM4 = computeShortPeriodTerms(stateM4, forces);
258
259 SpacecraftState stateM3 = shiftState(meanState, orbitType, -3 * steps[i], i);
260 double[] shortPeriodM3 = computeShortPeriodTerms(stateM3, forces);
261
262 SpacecraftState stateM2 = shiftState(meanState, orbitType, -2 * steps[i], i);
263 double[] shortPeriodM2 = computeShortPeriodTerms(stateM2, forces);
264
265 SpacecraftState stateM1 = shiftState(meanState, orbitType, -1 * steps[i], i);
266 double[] shortPeriodM1 = computeShortPeriodTerms(stateM1, forces);
267
268 SpacecraftState stateP1 = shiftState(meanState, orbitType, 1 * steps[i], i);
269 double[] shortPeriodP1 = computeShortPeriodTerms(stateP1, forces);
270
271 SpacecraftState stateP2 = shiftState(meanState, orbitType, 2 * steps[i], i);
272 double[] shortPeriodP2 = computeShortPeriodTerms(stateP2, forces);
273
274 SpacecraftState stateP3 = shiftState(meanState, orbitType, 3 * steps[i], i);
275 double[] shortPeriodP3 = computeShortPeriodTerms(stateP3, forces);
276
277 SpacecraftState stateP4 = shiftState(meanState, orbitType, 4 * steps[i], i);
278 double[] shortPeriodP4 = computeShortPeriodTerms(stateP4, forces);
279
280 fillJacobianColumn(shortPeriodJacobianRef, i, orbitType, steps[i],
281 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
282 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
283
284 }
285
286 for (int m = 0; m < 6; ++m) {
287 for (int n = 0; n < 6; ++n) {
288 double error = FastMath.abs((shortPeriodJacobian[m][n] - shortPeriodJacobianRef[m][n]) / shortPeriodJacobianRef[m][n]);
289 Assertions.assertEquals(0, error, 7.7e-11);
290 }
291 }
292
293 }
294
295 @Test
296 @SuppressWarnings("unchecked")
297 public void testShortPeriodTermsMuParametersDerivatives() {
298
299
300 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
301 TimeScalesFactory.getUTC());
302
303 final Orbit orbit = new EquinoctialOrbit(42164000,
304 10e-3,
305 10e-3,
306 FastMath.tan(0.001745329) * FastMath.cos(2 * FastMath.PI / 3),
307 FastMath.tan(0.001745329) * FastMath.sin(2 * FastMath.PI / 3), 0.1,
308 PositionAngleType.TRUE,
309 FramesFactory.getEME2000(),
310 initDate,
311 3.986004415E14);
312
313 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
314
315 final SpacecraftState meanState = new SpacecraftState(orbit);
316
317
318 final Collection<DSSTForceModel> forces = new ArrayList<DSSTForceModel>();
319 final DSSTForceModel moon = new DSSTThirdBody(CelestialBodyFactory.getMoon(), meanState.getOrbit().getMu());
320 final DSSTForceModel sun = new DSSTThirdBody(CelestialBodyFactory.getSun(), meanState.getOrbit().getMu());
321 forces.add(moon);
322 forces.add(sun);
323
324 for (final DSSTForceModel forceModel : forces) {
325 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
326 driver.setValue(driver.getReferenceValue());
327 driver.setSelected(driver.getName().equals(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT));
328 }
329 }
330
331
332 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
333
334 final double[][] shortPeriodJacobian = new double[6][1];
335
336 for (final DSSTForceModel forceModel : forces) {
337
338
339 final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
340
341 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
342
343
344 final Gradient zero = dsState.getDate().getField().getZero();
345
346
347 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
348 shortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING,
349 converter.getParametersAtStateDate(dsState, forceModel)));
350 forceModel.updateShortPeriodTerms(converter.getParameters(dsState, forceModel), dsState);
351 final Gradient[] shortPeriod = new Gradient[6];
352 Arrays.fill(shortPeriod, zero);
353 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
354 final Gradient[] spVariation = spt.value(dsState.getOrbit());
355 for (int i = 0; i < spVariation .length; i++) {
356 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
357 }
358 }
359
360 final double[] derivativesASP = shortPeriod[0].getGradient();
361 final double[] derivativesExSP = shortPeriod[1].getGradient();
362 final double[] derivativesEySP = shortPeriod[2].getGradient();
363 final double[] derivativesHxSP = shortPeriod[3].getGradient();
364 final double[] derivativesHySP = shortPeriod[4].getGradient();
365 final double[] derivativesLSP = shortPeriod[5].getGradient();
366
367 int index = converter.getFreeStateParameters();
368 for (ParameterDriver driver : forceModel.getParametersDrivers()) {
369 if (driver.isSelected()) {
370 shortPeriodJacobian[0][0] += derivativesASP[index];
371 shortPeriodJacobian[1][0] += derivativesExSP[index];
372 shortPeriodJacobian[2][0] += derivativesEySP[index];
373 shortPeriodJacobian[3][0] += derivativesHxSP[index];
374 shortPeriodJacobian[4][0] += derivativesHySP[index];
375 shortPeriodJacobian[5][0] += derivativesLSP[index];
376 ++index;
377 }
378 }
379 }
380
381
382 double[][] shortPeriodJacobianRef = new double[6][1];
383 ParameterDriversList bound = new ParameterDriversList();
384 for (final DSSTForceModel forceModel : forces) {
385 for (final ParameterDriver driver : forceModel.getParametersDrivers()) {
386 if (driver.getName().equals(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT)) {
387 driver.setSelected(true);
388 bound.add(driver);
389 } else {
390 driver.setSelected(false);
391 }
392 }
393 }
394
395 ParameterDriver selected = bound.getDrivers().get(0);
396 double[] parameters = new double[1];
397 double p0 = selected.getReferenceValue();
398 double h = selected.getScale();
399
400 selected.setValue(p0 - 4 * h);
401 final double[] shortPeriodM4 = computeShortPeriodTerms(meanState, forces);
402
403 selected.setValue(p0 - 3 * h);
404 final double[] shortPeriodM3 = computeShortPeriodTerms(meanState, forces);
405
406 selected.setValue(p0 - 2 * h);
407 final double[] shortPeriodM2 = computeShortPeriodTerms(meanState, forces);
408
409 selected.setValue(p0 - 1 * h);
410 final double[] shortPeriodM1 = computeShortPeriodTerms(meanState, forces);
411
412 selected.setValue(p0 + 1 * h);
413 final double[] shortPeriodP1 = computeShortPeriodTerms(meanState, forces);
414
415 selected.setValue(p0 + 2 * h);
416 final double[] shortPeriodP2 = computeShortPeriodTerms(meanState, forces);
417
418 selected.setValue(p0 + 3 * h);
419 parameters[0] = selected.getValue();
420 final double[] shortPeriodP3 = computeShortPeriodTerms(meanState, forces);
421
422 selected.setValue(p0 + 4 * h, null);
423 final double[] shortPeriodP4 = computeShortPeriodTerms(meanState, forces);
424
425 fillJacobianColumn(shortPeriodJacobianRef, 0, orbitType, h,
426 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
427 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
428
429 for (int i = 0; i < 6; ++i) {
430 Assertions.assertEquals(shortPeriodJacobianRef[i][0],
431 shortPeriodJacobian[i][0],
432 FastMath.abs(shortPeriodJacobianRef[i][0] * 2.5e-11));
433 }
434
435 }
436
437 private <T extends CalculusFieldElement<T>> FieldSpacecraftState<T> getGEOState(final Field<T> field) {
438
439 final T zero = field.getZero();
440
441 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
442 TimeScalesFactory.getUTC());
443 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(42164000),
444 zero.add(10e-3),
445 zero.add(10e-3),
446 zero.add(FastMath.tan(0.001745329) * FastMath.cos(2 * FastMath.PI / 3)),
447 zero.add(FastMath.tan(0.001745329) * FastMath.sin(2 * FastMath.PI / 3)),
448 zero.add(0.1),
449 PositionAngleType.TRUE,
450 FramesFactory.getEME2000(),
451 initDate,
452 zero.add(3.986004415E14));
453 return new FieldSpacecraftState<>(orbit);
454 }
455
456 private double[] computeShortPeriodTerms(SpacecraftState state,
457 Collection<DSSTForceModel> forces) {
458
459 AuxiliaryElements auxiliaryElements = new AuxiliaryElements(state.getOrbit(), 1);
460
461 List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
462 for (final DSSTForceModel force : forces) {
463 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, force.getParameters(state.getDate())));
464 force.updateShortPeriodTerms(force.getParametersAllValues(), state);
465 }
466
467 double[] shortPeriod = new double[6];
468 for (ShortPeriodTerms spt : shortPeriodTerms) {
469 double[] spVariation = spt.value(state.getOrbit());
470 for (int i = 0; i < spVariation.length; i++) {
471 shortPeriod[i] += spVariation[i];
472 }
473 }
474
475 return shortPeriod;
476
477 }
478
479 private void fillJacobianColumn(double[][] jacobian, int column,
480 OrbitType orbitType, double h,
481 double[] M4h, double[] M3h,
482 double[] M2h, double[] M1h,
483 double[] P1h, double[] P2h,
484 double[] P3h, double[] P4h) {
485 for (int i = 0; i < jacobian.length; ++i) {
486 jacobian[i][column] = ( -3 * (P4h[i] - M4h[i]) +
487 32 * (P3h[i] - M3h[i]) -
488 168 * (P2h[i] - M2h[i]) +
489 672 * (P1h[i] - M1h[i])) / (840 * h);
490 }
491 }
492
493 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
494 double delta, int column) {
495
496 double[][] array = stateToArray(state, orbitType);
497 array[0][column] += delta;
498
499 return arrayToState(array, orbitType, state.getFrame(), state.getDate(),
500 state.getOrbit().getMu(), state.getAttitude());
501
502 }
503
504 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
505 double[][] array = new double[2][6];
506
507 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngleType.MEAN, array[0], array[1]);
508 return array;
509 }
510
511 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType,
512 Frame frame, AbsoluteDate date, double mu,
513 Attitude attitude) {
514 EquinoctialOrbit orbit = (EquinoctialOrbit) orbitType.mapArrayToOrbit(array[0], array[1], PositionAngleType.MEAN, date, mu, frame);
515 return new SpacecraftState(orbit, attitude);
516 }
517
518
519
520
521
522
523 private void addToRow(final double[] derivatives, final int index,
524 final double[][] jacobian) {
525
526 for (int i = 0; i < 6; i++) {
527 jacobian[index][i] += derivatives[i];
528 }
529
530 }
531
532 @BeforeEach
533 public void setUp() throws IOException, ParseException {
534 Utils.setDataRoot("regular-data");
535 }
536
537 }