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 java.io.IOException;
20 import java.text.ParseException;
21 import java.util.ArrayList;
22 import java.util.Arrays;
23 import java.util.List;
24
25 import org.hipparchus.CalculusFieldElement;
26 import org.hipparchus.Field;
27 import org.hipparchus.analysis.differentiation.Gradient;
28 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
29 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
30 import org.hipparchus.geometry.euclidean.threed.Rotation;
31 import org.hipparchus.geometry.euclidean.threed.RotationOrder;
32 import org.hipparchus.geometry.euclidean.threed.Vector3D;
33 import org.hipparchus.util.Binary64Field;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.MathArrays;
36 import org.junit.jupiter.api.Assertions;
37 import org.junit.jupiter.api.BeforeEach;
38 import org.junit.jupiter.api.Test;
39 import org.orekit.Utils;
40 import org.orekit.attitudes.Attitude;
41 import org.orekit.attitudes.AttitudeProvider;
42 import org.orekit.attitudes.FieldAttitude;
43 import org.orekit.attitudes.FrameAlignedProvider;
44 import org.orekit.attitudes.LofOffset;
45 import org.orekit.bodies.CelestialBody;
46 import org.orekit.bodies.CelestialBodyFactory;
47 import org.orekit.bodies.OneAxisEllipsoid;
48 import org.orekit.forces.BoxAndSolarArraySpacecraft;
49 import org.orekit.forces.drag.DragSensitive;
50 import org.orekit.forces.gravity.potential.GravityFieldFactory;
51 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
52 import org.orekit.frames.Frame;
53 import org.orekit.frames.FramesFactory;
54 import org.orekit.frames.LOFType;
55 import org.orekit.models.earth.atmosphere.Atmosphere;
56 import org.orekit.models.earth.atmosphere.HarrisPriester;
57 import org.orekit.orbits.EquinoctialOrbit;
58 import org.orekit.orbits.FieldEquinoctialOrbit;
59 import org.orekit.orbits.FieldOrbit;
60 import org.orekit.orbits.Orbit;
61 import org.orekit.orbits.OrbitType;
62 import org.orekit.orbits.PositionAngleType;
63 import org.orekit.propagation.FieldSpacecraftState;
64 import org.orekit.propagation.PropagationType;
65 import org.orekit.propagation.SpacecraftState;
66 import org.orekit.propagation.ToleranceProvider;
67 import org.orekit.propagation.semianalytical.dsst.forces.DSSTAtmosphericDrag;
68 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
69 import org.orekit.propagation.semianalytical.dsst.forces.DSSTNewtonianAttraction;
70 import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
71 import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
72 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
73 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
74 import org.orekit.time.AbsoluteDate;
75 import org.orekit.time.DateComponents;
76 import org.orekit.time.FieldAbsoluteDate;
77 import org.orekit.time.TimeComponents;
78 import org.orekit.time.TimeScalesFactory;
79 import org.orekit.utils.Constants;
80 import org.orekit.utils.IERSConventions;
81 import org.orekit.utils.ParameterDriver;
82 import org.orekit.utils.ParameterDriversList;
83 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
84
85 class FieldDSSTAtmosphericDragTest {
86
87 @Test
88 void testGetMeanElementRate() {
89 doTestGetMeanElementRate(Binary64Field.getInstance());
90 }
91
92 private <T extends CalculusFieldElement<T>> void doTestGetMeanElementRate(Field<T> field) {
93
94 final T zero = field.getZero();
95
96 final UnnormalizedSphericalHarmonicsProvider provider =
97 GravityFieldFactory.getUnnormalizedProvider(2, 0);
98
99 final Frame earthFrame = FramesFactory.getEME2000();
100 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, 2003, 07, 01, 0, 0, 0, TimeScalesFactory.getUTC());
101 final double mu = 3.986004415E14;
102
103
104
105
106
107
108 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(7204535.84810944),
109 zero.add(-0.001119677138261611),
110 zero.add(5.333650671984143E-4),
111 zero.add(0.847841707880348),
112 zero.add(0.7998014061193262),
113 zero.add(3.897842092486239),
114 PositionAngleType.TRUE,
115 earthFrame,
116 initDate,
117 zero.add(mu));
118
119
120 final OneAxisEllipsoid earth = new OneAxisEllipsoid(provider.getAe(),
121 Constants.WGS84_EARTH_FLATTENING,
122 CelestialBodyFactory.getEarth().getBodyOrientedFrame());
123 final Atmosphere atm = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
124 final double cd = 2.0;
125 final double area = 25.0;
126 DSSTForceModel drag = new DSSTAtmosphericDrag(atm, cd, area, mu);
127
128
129 Rotation rotation = new Rotation(1., 0., 0., 0., false);
130 AttitudeProvider attitudeProvider = new FrameAlignedProvider(rotation);
131 drag.registerAttitudeProvider(attitudeProvider);
132
133
134 FieldRotation<T> fieldRotation = new FieldRotation<>(field, rotation);
135 FieldVector3D<T> rotationRate = new FieldVector3D<>(zero, zero, zero);
136 FieldVector3D<T> rotationAcceleration = new FieldVector3D<>(zero, zero, zero);
137 TimeStampedFieldAngularCoordinates<T> orientation = new TimeStampedFieldAngularCoordinates<>(initDate, fieldRotation, rotationRate, rotationAcceleration);
138 final FieldAttitude<T> att = new FieldAttitude<>(earthFrame, orientation);
139
140 final T mass = zero.add(1000.0);
141 final FieldSpacecraftState<T> state = new FieldSpacecraftState<>(orbit, att, mass);
142 final FieldAuxiliaryElements<T> auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), 1);
143
144
145 final T[] parameters = drag.getParameters(field, state.getDate());
146
147 drag.initializeShortPeriodTerms(auxiliaryElements,
148 PropagationType.MEAN, parameters);
149
150
151 final T[] elements = MathArrays.buildArray(field, 7);
152 Arrays.fill(elements, zero);
153 final T[] daidt = drag.getMeanElementRate(state, auxiliaryElements, parameters);
154 for (int i = 0; i < daidt.length; i++) {
155 elements[i] = daidt[i];
156 }
157
158 Assertions.assertEquals(-3.415320567871035E-5, elements[0].getReal(), 2.0e-20);
159 Assertions.assertEquals(6.276312897745139E-13, elements[1].getReal(), 2.9e-27);
160 Assertions.assertEquals(-9.303357008691404E-13, elements[2].getReal(), 2.7e-27);
161 Assertions.assertEquals(-7.052316604063199E-14, elements[3].getReal(), 2.0e-28);
162 Assertions.assertEquals(-6.793277250493389E-14, elements[4].getReal(), 9.0e-29);
163 Assertions.assertEquals(-1.3565284454826392E-15, elements[5].getReal(), 2.0e-28);
164
165 }
166
167 @Test
168 void testShortPeriodTerms() {
169 doTestShortPeriodTerms(Binary64Field.getInstance());
170 }
171
172 @SuppressWarnings("unchecked")
173 private <T extends CalculusFieldElement<T>> void doTestShortPeriodTerms(final Field<T> field) {
174
175 final T zero = field.getZero();
176 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, new DateComponents(2003, 03, 21), new TimeComponents(1, 0, 0.), TimeScalesFactory.getUTC());
177
178 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(7069219.9806427825),
179 zero.add(-4.5941811292223825E-4),
180 zero.add(1.309932339472599E-4),
181 zero.add(-1.002996107003202),
182 zero.add(0.570979900577994),
183 zero.add(2.62038786211518),
184 PositionAngleType.TRUE,
185 FramesFactory.getEME2000(),
186 initDate,
187 zero.add(3.986004415E14));
188
189 final FieldSpacecraftState<T> meanState = new FieldSpacecraftState<>(orbit);
190
191 final CelestialBody sun = CelestialBodyFactory.getSun();
192 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
193 Constants.WGS84_EARTH_FLATTENING,
194 FramesFactory.getITRF(IERSConventions.IERS_2010,
195 true));
196
197 final BoxAndSolarArraySpacecraft boxAndWing = new BoxAndSolarArraySpacecraft(5.0, 2.0, 2.0,
198 sun,
199 50.0, Vector3D.PLUS_J,
200 2.0, 0.1,
201 0.2, 0.6);
202
203 final Atmosphere atmosphere = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
204 final AttitudeProvider attitudeProvider = new LofOffset(meanState.getFrame(),
205 LOFType.LVLH_CCSDS, RotationOrder.XYZ,
206 0.0, 0.0, 0.0);
207
208 final DSSTForceModel drag = new DSSTAtmosphericDrag(atmosphere, boxAndWing, meanState.getOrbit().getMu().getReal());
209
210
211 final FieldAuxiliaryElements<T> aux = new FieldAuxiliaryElements<>(meanState.getOrbit(), 1);
212
213
214 final List<FieldShortPeriodTerms<T>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<T>>();
215
216 drag.registerAttitudeProvider(attitudeProvider);
217 shortPeriodTerms.addAll(drag.initializeShortPeriodTerms(aux, PropagationType.OSCULATING, drag.getParameters(field, orbit.getDate())));
218 drag.updateShortPeriodTerms(drag.getParametersAllValues(field), meanState);
219
220 T[] y = MathArrays.buildArray(field, 6);
221 Arrays.fill(y, zero);
222
223 for (final FieldShortPeriodTerms<T> spt : shortPeriodTerms) {
224 final T[] shortPeriodic = spt.value(meanState.getOrbit());
225 for (int i = 0; i < shortPeriodic.length; i++) {
226 y[i] = y[i].add(shortPeriodic[i]);
227 }
228 }
229
230 Assertions.assertEquals( 0.03966657233267546, y[0].getReal(), 1.0e-12);
231 Assertions.assertEquals(-1.52943814431705860e-8, y[1].getReal(), 1.0e-20);
232 Assertions.assertEquals(-2.36149298285122150e-8, y[2].getReal(), 1.0e-20);
233 Assertions.assertEquals(-5.90158033654432200e-11, y[3].getReal(), 1.0e-20);
234 Assertions.assertEquals( 1.02876397430619780e-11, y[4].getReal(), 1.0e-20);
235 Assertions.assertEquals( 2.53842752377756140e-8, y[5].getReal(), 1.0e-20);
236
237 }
238
239 @Test
240 @SuppressWarnings("unchecked")
241 void testShortPeriodTermsStateDerivatives() {
242
243
244 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
245 TimeScalesFactory.getUTC());
246
247 final Orbit orbit = new EquinoctialOrbit(7204535.84810944,
248 -0.001119677138261611,
249 5.333650671984143E-4,
250 0.847841707880348,
251 0.7998014061193262,
252 3.897842092486239,
253 PositionAngleType.TRUE,
254 FramesFactory.getEME2000(),
255 initDate,
256 3.986004415E14);
257
258 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
259
260 final SpacecraftState meanState = new SpacecraftState(orbit);
261
262
263 final AttitudeProvider attitudeProvider = new LofOffset(meanState.getFrame(),
264 LOFType.LVLH_CCSDS, RotationOrder.XYZ,
265 0.0, 0.0, 0.0);
266
267
268 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
269 Constants.WGS84_EARTH_FLATTENING,
270 CelestialBodyFactory.getEarth().getBodyOrientedFrame());
271 final Atmosphere atm = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
272 final double cd = 2.0;
273 final double area = 25.0;
274 final DSSTForceModel drag = new DSSTAtmosphericDrag(atm, cd, area, 3.986004415E14);
275 drag.registerAttitudeProvider(attitudeProvider);
276
277
278 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
279
280
281 final FieldSpacecraftState<Gradient> dsState = converter.getState(drag);
282
283 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
284
285
286 final Gradient zero = dsState.getDate().getField().getZero();
287
288
289 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
290 shortPeriodTerms.addAll(drag.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING,
291 converter.getParametersAtStateDate(dsState, drag)));
292 drag.updateShortPeriodTerms(converter.getParameters(dsState, drag), dsState);
293 final Gradient[] shortPeriod = new Gradient[6];
294 Arrays.fill(shortPeriod, zero);
295 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
296 final Gradient[] spVariation = spt.value(dsState.getOrbit());
297 for (int i = 0; i < spVariation .length; i++) {
298 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
299 }
300 }
301
302 final double[][] shortPeriodJacobian = new double[6][6];
303
304 final double[] derivativesASP = shortPeriod[0].getGradient();
305 final double[] derivativesExSP = shortPeriod[1].getGradient();
306 final double[] derivativesEySP = shortPeriod[2].getGradient();
307 final double[] derivativesHxSP = shortPeriod[3].getGradient();
308 final double[] derivativesHySP = shortPeriod[4].getGradient();
309 final double[] derivativesLSP = shortPeriod[5].getGradient();
310
311
312 addToRow(derivativesASP, 0, shortPeriodJacobian);
313 addToRow(derivativesExSP, 1, shortPeriodJacobian);
314 addToRow(derivativesEySP, 2, shortPeriodJacobian);
315 addToRow(derivativesHxSP, 3, shortPeriodJacobian);
316 addToRow(derivativesHySP, 4, shortPeriodJacobian);
317 addToRow(derivativesLSP, 5, shortPeriodJacobian);
318
319
320 double[][] shortPeriodJacobianRef = new double[6][6];
321 double dP = 0.001;
322 double[] steps = ToleranceProvider.getDefaultToleranceProvider(1000000 * dP).getTolerances(orbit, orbitType)[0];
323 for (int i = 0; i < 6; i++) {
324
325 SpacecraftState stateM4 = shiftState(meanState, orbitType, -4 * steps[i], i);
326 double[] shortPeriodM4 = computeShortPeriodTerms(stateM4, drag);
327
328 SpacecraftState stateM3 = shiftState(meanState, orbitType, -3 * steps[i], i);
329 double[] shortPeriodM3 = computeShortPeriodTerms(stateM3, drag);
330
331 SpacecraftState stateM2 = shiftState(meanState, orbitType, -2 * steps[i], i);
332 double[] shortPeriodM2 = computeShortPeriodTerms(stateM2, drag);
333
334 SpacecraftState stateM1 = shiftState(meanState, orbitType, -1 * steps[i], i);
335 double[] shortPeriodM1 = computeShortPeriodTerms(stateM1, drag);
336
337 SpacecraftState stateP1 = shiftState(meanState, orbitType, 1 * steps[i], i);
338 double[] shortPeriodP1 = computeShortPeriodTerms(stateP1, drag);
339
340 SpacecraftState stateP2 = shiftState(meanState, orbitType, 2 * steps[i], i);
341 double[] shortPeriodP2 = computeShortPeriodTerms(stateP2, drag);
342
343 SpacecraftState stateP3 = shiftState(meanState, orbitType, 3 * steps[i], i);
344 double[] shortPeriodP3 = computeShortPeriodTerms(stateP3, drag);
345
346 SpacecraftState stateP4 = shiftState(meanState, orbitType, 4 * steps[i], i);
347 double[] shortPeriodP4 = computeShortPeriodTerms(stateP4, drag);
348
349 fillJacobianColumn(shortPeriodJacobianRef, i, orbitType, steps[i],
350 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
351 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
352
353 }
354
355 for (int m = 0; m < 6; ++m) {
356 for (int n = 0; n < 6; ++n) {
357 double error = FastMath.abs((shortPeriodJacobian[m][n] - shortPeriodJacobianRef[m][n]) / shortPeriodJacobianRef[m][n]);
358 Assertions.assertEquals(0, error, 2.6e-2);
359 }
360 }
361
362 }
363
364 @Test
365 void testDragParametersDerivatives() throws ParseException, IOException {
366 doTestShortPeriodTermsParametersDerivatives(DragSensitive.DRAG_COEFFICIENT, 6.0e-14);
367 }
368
369 @Test
370 void testMuParametersDerivatives() throws ParseException, IOException {
371 doTestShortPeriodTermsParametersDerivatives(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT, 3.7e-9);
372 }
373
374 @SuppressWarnings("unchecked")
375 private void doTestShortPeriodTermsParametersDerivatives(String parameterName, double tolerance) {
376
377
378 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
379 TimeScalesFactory.getUTC());
380
381 final Orbit orbit = new EquinoctialOrbit(7204535.84810944,
382 -0.001119677138261611,
383 5.333650671984143E-4,
384 0.847841707880348,
385 0.7998014061193262,
386 3.897842092486239,
387 PositionAngleType.TRUE,
388 FramesFactory.getEME2000(),
389 initDate,
390 3.986004415E14);
391
392 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
393
394 final SpacecraftState meanState = new SpacecraftState(orbit);
395
396
397 final AttitudeProvider attitudeProvider = new LofOffset(meanState.getFrame(),
398 LOFType.LVLH_CCSDS, RotationOrder.XYZ,
399 0.0, 0.0, 0.0);
400
401
402 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
403 Constants.WGS84_EARTH_FLATTENING,
404 CelestialBodyFactory.getEarth().getBodyOrientedFrame());
405 final Atmosphere atm = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
406 final double cd = 2.0;
407 final double area = 25.0;
408 final DSSTForceModel drag = new DSSTAtmosphericDrag(atm, cd, area, 3.986004415E14);
409 drag.registerAttitudeProvider(attitudeProvider);
410
411 for (final ParameterDriver driver : drag.getParametersDrivers()) {
412 driver.setValue(driver.getReferenceValue());
413 driver.setSelected(driver.getName().equals(parameterName));
414 }
415
416
417 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
418
419
420 final FieldSpacecraftState<Gradient> dsState = converter.getState(drag);
421
422 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
423
424
425 final Gradient zero = dsState.getDate().getField().getZero();
426
427
428 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
429 shortPeriodTerms.addAll(drag.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING, converter.getParametersAtStateDate(dsState, drag)));
430 drag.updateShortPeriodTerms(converter.getParameters(dsState, drag), dsState);
431 final Gradient[] shortPeriod = new Gradient[6];
432 Arrays.fill(shortPeriod, zero);
433 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
434 final Gradient[] spVariation = spt.value(dsState.getOrbit());
435 for (int i = 0; i < spVariation .length; i++) {
436 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
437 }
438 }
439
440 final double[][] shortPeriodJacobian = new double[6][1];
441
442 final double[] derivativesASP = shortPeriod[0].getGradient();
443 final double[] derivativesExSP = shortPeriod[1].getGradient();
444 final double[] derivativesEySP = shortPeriod[2].getGradient();
445 final double[] derivativesHxSP = shortPeriod[3].getGradient();
446 final double[] derivativesHySP = shortPeriod[4].getGradient();
447 final double[] derivativesLSP = shortPeriod[5].getGradient();
448
449 int index = converter.getFreeStateParameters();
450 for (ParameterDriver driver : drag.getParametersDrivers()) {
451 if (driver.isSelected()) {
452 shortPeriodJacobian[0][0] += derivativesASP[index];
453 shortPeriodJacobian[1][0] += derivativesExSP[index];
454 shortPeriodJacobian[2][0] += derivativesEySP[index];
455 shortPeriodJacobian[3][0] += derivativesHxSP[index];
456 shortPeriodJacobian[4][0] += derivativesHySP[index];
457 shortPeriodJacobian[5][0] += derivativesLSP[index];
458 ++index;
459 }
460 }
461
462
463 double[][] shortPeriodJacobianRef = new double[6][1];
464 ParameterDriversList bound = new ParameterDriversList();
465 for (final ParameterDriver driver : drag.getParametersDrivers()) {
466 if (driver.getName().equals(parameterName)) {
467 driver.setSelected(true);
468 bound.add(driver);
469 } else {
470 driver.setSelected(false);
471 }
472 }
473
474 ParameterDriver selected = bound.getDrivers().get(0);
475 double p0 = selected.getReferenceValue();
476 double h = selected.getScale();
477
478 selected.setValue(p0 - 4 * h);
479 final double[] shortPeriodM4 = computeShortPeriodTerms(meanState, drag);
480
481 selected.setValue(p0 - 3 * h);
482 final double[] shortPeriodM3 = computeShortPeriodTerms(meanState, drag);
483
484 selected.setValue(p0 - 2 * h);
485 final double[] shortPeriodM2 = computeShortPeriodTerms(meanState, drag);
486
487 selected.setValue(p0 - 1 * h);
488 final double[] shortPeriodM1 = computeShortPeriodTerms(meanState, drag);
489
490 selected.setValue(p0 + 1 * h);
491 final double[] shortPeriodP1 = computeShortPeriodTerms(meanState, drag);
492
493 selected.setValue(p0 + 2 * h);
494 final double[] shortPeriodP2 = computeShortPeriodTerms(meanState, drag);
495
496 selected.setValue(p0 + 3 * h);
497 final double[] shortPeriodP3 = computeShortPeriodTerms(meanState, drag);
498
499 selected.setValue(p0 + 4 * h);
500 final double[] shortPeriodP4 = computeShortPeriodTerms(meanState, drag);
501
502 fillJacobianColumn(shortPeriodJacobianRef, 0, orbitType, h,
503 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
504 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
505
506 for (int i = 0; i < 6; ++i) {
507 Assertions.assertEquals(shortPeriodJacobianRef[i][0],
508 shortPeriodJacobian[i][0],
509 FastMath.abs(shortPeriodJacobianRef[i][0] * tolerance));
510 }
511
512 }
513
514 private double[] computeShortPeriodTerms(SpacecraftState state,
515 DSSTForceModel force) {
516
517 AuxiliaryElements auxiliaryElements = new AuxiliaryElements(state.getOrbit(), 1);
518
519 List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
520 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, force.getParameters(state.getDate())));
521 force.updateShortPeriodTerms(force.getParametersAllValues(), state);
522
523 double[] shortPeriod = new double[6];
524 for (ShortPeriodTerms spt : shortPeriodTerms) {
525 double[] spVariation = spt.value(state.getOrbit());
526 for (int i = 0; i < spVariation.length; i++) {
527 shortPeriod[i] += spVariation[i];
528 }
529 }
530
531 return shortPeriod;
532
533 }
534
535 private void fillJacobianColumn(double[][] jacobian, int column,
536 OrbitType orbitType, double h,
537 double[] M4h, double[] M3h,
538 double[] M2h, double[] M1h,
539 double[] P1h, double[] P2h,
540 double[] P3h, double[] P4h) {
541 for (int i = 0; i < jacobian.length; ++i) {
542 jacobian[i][column] = ( -3 * (P4h[i] - M4h[i]) +
543 32 * (P3h[i] - M3h[i]) -
544 168 * (P2h[i] - M2h[i]) +
545 672 * (P1h[i] - M1h[i])) / (840 * h);
546 }
547 }
548
549 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
550 double delta, int column) {
551
552 double[][] array = stateToArray(state, orbitType);
553 array[0][column] += delta;
554
555 return arrayToState(array, orbitType, state.getFrame(), state.getDate(),
556 state.getOrbit().getMu(), state.getAttitude());
557
558 }
559
560 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
561 double[][] array = new double[2][6];
562
563 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngleType.MEAN, array[0], array[1]);
564 return array;
565 }
566
567 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType,
568 Frame frame, AbsoluteDate date, double mu,
569 Attitude attitude) {
570 EquinoctialOrbit orbit = (EquinoctialOrbit) orbitType.mapArrayToOrbit(array[0], array[1], PositionAngleType.MEAN, date, mu, frame);
571 return new SpacecraftState(orbit, attitude);
572 }
573
574
575
576
577
578
579 private void addToRow(final double[] derivatives, final int index,
580 final double[][] jacobian) {
581
582 for (int i = 0; i < 6; i++) {
583 jacobian[index][i] += derivatives[i];
584 }
585
586 }
587
588 @BeforeEach
589 public void setUp() throws IOException, ParseException {
590 Utils.setDataRoot("regular-data:potential/shm-format");
591 }
592
593 }