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.Field;
26 import org.hipparchus.CalculusFieldElement;
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.Decimal64Field;
34 import org.hipparchus.util.FastMath;
35 import org.hipparchus.util.MathArrays;
36 import org.junit.Assert;
37 import org.junit.Before;
38 import org.junit.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.InertialProvider;
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.PositionAngle;
63 import org.orekit.propagation.FieldSpacecraftState;
64 import org.orekit.propagation.PropagationType;
65 import org.orekit.propagation.SpacecraftState;
66 import org.orekit.propagation.numerical.NumericalPropagator;
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 public class FieldDSSTAtmosphericDragTest {
86
87 @Test
88 public void testGetMeanElementRate() {
89 doTestGetMeanElementRate(Decimal64Field.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 PositionAngle.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 InertialProvider(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);
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 Assert.assertEquals(-3.415320567871035E-5, elements[0].getReal(), 2.0e-20);
159 Assert.assertEquals(6.276312897745139E-13, elements[1].getReal(), 2.9e-27);
160 Assert.assertEquals(-9.303357008691404E-13, elements[2].getReal(), 2.7e-27);
161 Assert.assertEquals(-7.052316604063199E-14, elements[3].getReal(), 2.0e-28);
162 Assert.assertEquals(-6.793277250493389E-14, elements[4].getReal(), 9.0e-29);
163 Assert.assertEquals(-1.3565284454826392E-15, elements[5].getReal(), 2.0e-28);
164
165 }
166
167 @Test
168 public void testShortPeriodTerms() {
169 doTestShortPeriodTerms(Decimal64Field.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 PositionAngle.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.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)));
218 drag.updateShortPeriodTerms(drag.getParameters(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 Assert.assertEquals(0.03966657233280967, y[0].getReal(), 1.0e-15);
231 Assert.assertEquals(-1.5294381443173415E-8, y[1].getReal(), 1.0e-23);
232 Assert.assertEquals(-2.3614929828516364E-8, y[2].getReal(), 1.4e-23);
233 Assert.assertEquals(-5.901580336558653E-11, y[3].getReal(), 4.0e-25);
234 Assert.assertEquals(1.0287639743124977E-11, y[4].getReal(), 2.0e-24);
235 Assert.assertEquals(2.538427523777691E-8, y[5].getReal(), 1.0e-22);
236 }
237
238 @Test
239 @SuppressWarnings("unchecked")
240 public void testShortPeriodTermsStateDerivatives() {
241
242
243 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
244 TimeScalesFactory.getUTC());
245
246 final Orbit orbit = new EquinoctialOrbit(7204535.84810944,
247 -0.001119677138261611,
248 5.333650671984143E-4,
249 0.847841707880348,
250 0.7998014061193262,
251 3.897842092486239,
252 PositionAngle.TRUE,
253 FramesFactory.getEME2000(),
254 initDate,
255 3.986004415E14);
256
257 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
258
259 final SpacecraftState meanState = new SpacecraftState(orbit);
260
261
262 final AttitudeProvider attitudeProvider = new LofOffset(meanState.getFrame(),
263 LOFType.LVLH_CCSDS, RotationOrder.XYZ,
264 0.0, 0.0, 0.0);
265
266
267 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
268 Constants.WGS84_EARTH_FLATTENING,
269 CelestialBodyFactory.getEarth().getBodyOrientedFrame());
270 final Atmosphere atm = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
271 final double cd = 2.0;
272 final double area = 25.0;
273 final DSSTForceModel drag = new DSSTAtmosphericDrag(atm, cd, area, 3.986004415E14);
274 drag.registerAttitudeProvider(attitudeProvider);
275
276
277 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
278
279
280 final FieldSpacecraftState<Gradient> dsState = converter.getState(drag);
281 final Gradient[] dsParameters = converter.getParameters(dsState, 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, dsParameters));
291 drag.updateShortPeriodTerms(dsParameters, dsState);
292 final Gradient[] shortPeriod = new Gradient[6];
293 Arrays.fill(shortPeriod, zero);
294 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
295 final Gradient[] spVariation = spt.value(dsState.getOrbit());
296 for (int i = 0; i < spVariation .length; i++) {
297 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
298 }
299 }
300
301 final double[][] shortPeriodJacobian = new double[6][6];
302
303 final double[] derivativesASP = shortPeriod[0].getGradient();
304 final double[] derivativesExSP = shortPeriod[1].getGradient();
305 final double[] derivativesEySP = shortPeriod[2].getGradient();
306 final double[] derivativesHxSP = shortPeriod[3].getGradient();
307 final double[] derivativesHySP = shortPeriod[4].getGradient();
308 final double[] derivativesLSP = shortPeriod[5].getGradient();
309
310
311 addToRow(derivativesASP, 0, shortPeriodJacobian);
312 addToRow(derivativesExSP, 1, shortPeriodJacobian);
313 addToRow(derivativesEySP, 2, shortPeriodJacobian);
314 addToRow(derivativesHxSP, 3, shortPeriodJacobian);
315 addToRow(derivativesHySP, 4, shortPeriodJacobian);
316 addToRow(derivativesLSP, 5, shortPeriodJacobian);
317
318
319 double[][] shortPeriodJacobianRef = new double[6][6];
320 double dP = 0.001;
321 double[] steps = NumericalPropagator.tolerances(1000000 * dP, orbit, orbitType)[0];
322 for (int i = 0; i < 6; i++) {
323
324 SpacecraftState stateM4 = shiftState(meanState, orbitType, -4 * steps[i], i);
325 double[] shortPeriodM4 = computeShortPeriodTerms(stateM4, drag);
326
327 SpacecraftState stateM3 = shiftState(meanState, orbitType, -3 * steps[i], i);
328 double[] shortPeriodM3 = computeShortPeriodTerms(stateM3, drag);
329
330 SpacecraftState stateM2 = shiftState(meanState, orbitType, -2 * steps[i], i);
331 double[] shortPeriodM2 = computeShortPeriodTerms(stateM2, drag);
332
333 SpacecraftState stateM1 = shiftState(meanState, orbitType, -1 * steps[i], i);
334 double[] shortPeriodM1 = computeShortPeriodTerms(stateM1, drag);
335
336 SpacecraftState stateP1 = shiftState(meanState, orbitType, 1 * steps[i], i);
337 double[] shortPeriodP1 = computeShortPeriodTerms(stateP1, drag);
338
339 SpacecraftState stateP2 = shiftState(meanState, orbitType, 2 * steps[i], i);
340 double[] shortPeriodP2 = computeShortPeriodTerms(stateP2, drag);
341
342 SpacecraftState stateP3 = shiftState(meanState, orbitType, 3 * steps[i], i);
343 double[] shortPeriodP3 = computeShortPeriodTerms(stateP3, drag);
344
345 SpacecraftState stateP4 = shiftState(meanState, orbitType, 4 * steps[i], i);
346 double[] shortPeriodP4 = computeShortPeriodTerms(stateP4, drag);
347
348 fillJacobianColumn(shortPeriodJacobianRef, i, orbitType, steps[i],
349 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
350 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
351
352 }
353
354 for (int m = 0; m < 6; ++m) {
355 for (int n = 0; n < 6; ++n) {
356 double error = FastMath.abs((shortPeriodJacobian[m][n] - shortPeriodJacobianRef[m][n]) / shortPeriodJacobianRef[m][n]);
357 Assert.assertEquals(0, error, 2.6e-2);
358 }
359 }
360
361 }
362
363 @Test
364 public void testDragParametersDerivatives() throws ParseException, IOException {
365 doTestShortPeriodTermsParametersDerivatives(DragSensitive.DRAG_COEFFICIENT, 6.0e-14);
366 }
367
368 @Test
369 public void testMuParametersDerivatives() throws ParseException, IOException {
370 doTestShortPeriodTermsParametersDerivatives(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT, 3.7e-9);
371 }
372
373 @SuppressWarnings("unchecked")
374 private void doTestShortPeriodTermsParametersDerivatives(String parameterName, double tolerance) {
375
376
377 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
378 TimeScalesFactory.getUTC());
379
380 final Orbit orbit = new EquinoctialOrbit(7204535.84810944,
381 -0.001119677138261611,
382 5.333650671984143E-4,
383 0.847841707880348,
384 0.7998014061193262,
385 3.897842092486239,
386 PositionAngle.TRUE,
387 FramesFactory.getEME2000(),
388 initDate,
389 3.986004415E14);
390
391 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
392
393 final SpacecraftState meanState = new SpacecraftState(orbit);
394
395
396 final AttitudeProvider attitudeProvider = new LofOffset(meanState.getFrame(),
397 LOFType.LVLH_CCSDS, RotationOrder.XYZ,
398 0.0, 0.0, 0.0);
399
400
401 final OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
402 Constants.WGS84_EARTH_FLATTENING,
403 CelestialBodyFactory.getEarth().getBodyOrientedFrame());
404 final Atmosphere atm = new HarrisPriester(CelestialBodyFactory.getSun(), earth, 6);
405 final double cd = 2.0;
406 final double area = 25.0;
407 final DSSTForceModel drag = new DSSTAtmosphericDrag(atm, cd, area, 3.986004415E14);
408 drag.registerAttitudeProvider(attitudeProvider);
409
410 for (final ParameterDriver driver : drag.getParametersDrivers()) {
411 driver.setValue(driver.getReferenceValue());
412 driver.setSelected(driver.getName().equals(parameterName));
413 }
414
415
416 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
417
418
419 final FieldSpacecraftState<Gradient> dsState = converter.getState(drag);
420 final Gradient[] dsParameters = converter.getParameters(dsState, 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, dsParameters));
430 drag.updateShortPeriodTerms(dsParameters, 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 Assert.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 double[] parameters = force.getParameters();
521 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, parameters));
522 force.updateShortPeriodTerms(parameters, state);
523
524 double[] shortPeriod = new double[6];
525 for (ShortPeriodTerms spt : shortPeriodTerms) {
526 double[] spVariation = spt.value(state.getOrbit());
527 for (int i = 0; i < spVariation.length; i++) {
528 shortPeriod[i] += spVariation[i];
529 }
530 }
531
532 return shortPeriod;
533
534 }
535
536 private void fillJacobianColumn(double[][] jacobian, int column,
537 OrbitType orbitType, double h,
538 double[] M4h, double[] M3h,
539 double[] M2h, double[] M1h,
540 double[] P1h, double[] P2h,
541 double[] P3h, double[] P4h) {
542 for (int i = 0; i < jacobian.length; ++i) {
543 jacobian[i][column] = ( -3 * (P4h[i] - M4h[i]) +
544 32 * (P3h[i] - M3h[i]) -
545 168 * (P2h[i] - M2h[i]) +
546 672 * (P1h[i] - M1h[i])) / (840 * h);
547 }
548 }
549
550 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
551 double delta, int column) {
552
553 double[][] array = stateToArray(state, orbitType);
554 array[0][column] += delta;
555
556 return arrayToState(array, orbitType, state.getFrame(), state.getDate(),
557 state.getMu(), state.getAttitude());
558
559 }
560
561 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
562 double[][] array = new double[2][6];
563
564 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngle.MEAN, array[0], array[1]);
565 return array;
566 }
567
568 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType,
569 Frame frame, AbsoluteDate date, double mu,
570 Attitude attitude) {
571 EquinoctialOrbit orbit = (EquinoctialOrbit) orbitType.mapArrayToOrbit(array[0], array[1], PositionAngle.MEAN, date, mu, frame);
572 return new SpacecraftState(orbit, attitude);
573 }
574
575
576
577
578
579
580 private void addToRow(final double[] derivatives, final int index,
581 final double[][] jacobian) {
582
583 for (int i = 0; i < 6; i++) {
584 jacobian[index][i] += derivatives[i];
585 }
586
587 }
588
589 @Before
590 public void setUp() throws IOException, ParseException {
591 Utils.setDataRoot("regular-data:potential/shm-format");
592 }
593
594 }