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