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.util.Decimal64Field;
29 import org.hipparchus.util.FastMath;
30 import org.hipparchus.util.MathArrays;
31 import org.junit.Assert;
32 import org.junit.Before;
33 import org.junit.Test;
34 import org.orekit.Utils;
35 import org.orekit.attitudes.Attitude;
36 import org.orekit.bodies.CelestialBodyFactory;
37 import org.orekit.bodies.OneAxisEllipsoid;
38 import org.orekit.forces.gravity.potential.GravityFieldFactory;
39 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
40 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
41 import org.orekit.frames.Frame;
42 import org.orekit.frames.FramesFactory;
43 import org.orekit.orbits.EquinoctialOrbit;
44 import org.orekit.orbits.FieldEquinoctialOrbit;
45 import org.orekit.orbits.FieldKeplerianOrbit;
46 import org.orekit.orbits.FieldOrbit;
47 import org.orekit.orbits.Orbit;
48 import org.orekit.orbits.OrbitType;
49 import org.orekit.orbits.PositionAngle;
50 import org.orekit.propagation.FieldSpacecraftState;
51 import org.orekit.propagation.PropagationType;
52 import org.orekit.propagation.SpacecraftState;
53 import org.orekit.propagation.numerical.NumericalPropagator;
54 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
55 import org.orekit.propagation.semianalytical.dsst.forces.DSSTNewtonianAttraction;
56 import org.orekit.propagation.semianalytical.dsst.forces.DSSTTesseral;
57 import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
58 import org.orekit.propagation.semianalytical.dsst.forces.ShortPeriodTerms;
59 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
60 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
61 import org.orekit.time.AbsoluteDate;
62 import org.orekit.time.DateComponents;
63 import org.orekit.time.FieldAbsoluteDate;
64 import org.orekit.time.TimeComponents;
65 import org.orekit.time.TimeScalesFactory;
66 import org.orekit.utils.Constants;
67 import org.orekit.utils.ParameterDriver;
68 import org.orekit.utils.ParameterDriversList;
69
70 public class FieldDSSTTesseralTest {
71
72 @Test
73 public void testGetMeanElementRate(){
74 doTestGetMeanElementRate(Decimal64Field.getInstance());
75 }
76
77 private <T extends CalculusFieldElement<T>> void doTestGetMeanElementRate(final Field<T> field) {
78
79 final T zero = field.getZero();
80
81 final UnnormalizedSphericalHarmonicsProvider provider =
82 GravityFieldFactory.getUnnormalizedProvider(4, 4);
83
84 final Frame frame = FramesFactory.getEME2000();
85 final Frame earthFrame = CelestialBodyFactory.getEarth().getBodyOrientedFrame();
86 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, 2007, 04, 16, 0, 46, 42.400, TimeScalesFactory.getUTC());
87
88
89
90
91
92
93
94 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(2.655989E7),
95 zero.add(2.719455286199036E-4),
96 zero.add(0.0041543085910249414),
97 zero.add(-0.3412974060023717),
98 zero.add(0.3960084733107685),
99 zero.add(8.566537840341699),
100 PositionAngle.TRUE,
101 frame,
102 initDate,
103 zero.add(3.986004415E14));
104
105 final T mass = zero.add(1000.0);
106 final FieldSpacecraftState<T> state = new FieldSpacecraftState<>(orbit, mass);
107
108 final DSSTForceModel tesseral = new DSSTTesseral(earthFrame,
109 Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider,
110 4, 4, 4, 8, 4, 4, 2);
111
112 final FieldAuxiliaryElements<T> auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), 1);
113
114
115 final T[] parameters = tesseral.getParameters(field);
116
117 tesseral.initializeShortPeriodTerms(auxiliaryElements,
118 PropagationType.MEAN, parameters);
119
120 final T[] elements = MathArrays.buildArray(field, 7);
121 Arrays.fill(elements, zero);
122
123 final T[] daidt = tesseral.getMeanElementRate(state, auxiliaryElements, parameters);
124 for (int i = 0; i < daidt.length; i++) {
125 elements[i] = daidt[i];
126 }
127
128 Assert.assertEquals(7.120011500375922E-5, elements[0].getReal(), 6.0e-19);
129 Assert.assertEquals(-1.109767646425212E-11, elements[1].getReal(), 2.0e-26);
130 Assert.assertEquals(2.3036711391089307E-11, elements[2].getReal(), 1.5e-26);
131 Assert.assertEquals(2.499304852807308E-12, elements[3].getReal(), 1.0e-27);
132 Assert.assertEquals(1.3899097178558372E-13, elements[4].getReal(), 3.0e-27);
133 Assert.assertEquals(5.795522421338584E-12, elements[5].getReal(), 1.0e-26);
134
135 }
136
137 @Test
138 public void testShortPeriodTerms() {
139 doTestShortPeriodTerms(Decimal64Field.getInstance());
140 }
141
142 @SuppressWarnings("unchecked")
143 private <T extends CalculusFieldElement<T>> void doTestShortPeriodTerms(final Field<T> field) {
144
145 final T zero = field.getZero();
146 Utils.setDataRoot("regular-data:potential/icgem-format");
147 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("^eigen-6s-truncated$", false));
148 UnnormalizedSphericalHarmonicsProvider nshp = GravityFieldFactory.getUnnormalizedProvider(8, 8);
149 FieldOrbit<T> orbit = new FieldKeplerianOrbit<>(zero.add(13378000),
150 zero.add(0.05),
151 zero,
152 zero,
153 zero.add(FastMath.PI),
154 zero,
155 PositionAngle.MEAN,
156 FramesFactory.getTOD(false),
157 new FieldAbsoluteDate<>(field, 2003, 5, 6, TimeScalesFactory.getUTC()),
158 zero.add(nshp.getMu()));
159
160 OneAxisEllipsoid earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
161 Constants.WGS84_EARTH_FLATTENING,
162 FramesFactory.getGTOD(false));
163
164
165 final DSSTForceModel force = new DSSTTesseral(earth.getBodyFrame(),
166 Constants.WGS84_EARTH_ANGULAR_VELOCITY,
167 nshp, 8, 8, 4, 12, 8, 8, 4);
168
169
170 final FieldSpacecraftState<T> meanState = new FieldSpacecraftState<>(orbit, zero.add(45.0));
171
172
173 final FieldAuxiliaryElements<T> aux = new FieldAuxiliaryElements<>(orbit, 1);
174
175 final List<FieldShortPeriodTerms<T>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<T>>();
176
177 force.registerAttitudeProvider(null);
178 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(aux, PropagationType.OSCULATING, force.getParameters(field)));
179 force.updateShortPeriodTerms(force.getParameters(field), meanState);
180
181 T[] y = MathArrays.buildArray(field, 6);
182 Arrays.fill(y, zero);
183 for (final FieldShortPeriodTerms<T> spt : shortPeriodTerms) {
184 final T[] shortPeriodic = spt.value(meanState.getOrbit());
185 for (int i = 0; i < shortPeriodic.length; i++) {
186 y[i] = y[i].add(shortPeriodic[i]);
187 }
188 }
189
190 Assert.assertEquals(5.192409957353236, y[0].getReal(), 1.e-15);
191 Assert.assertEquals(9.660364749662076E-7, y[1].getReal(), 1.e-22);
192 Assert.assertEquals(1.542008987162059E-6, y[2].getReal(), 1.e-21);
193 Assert.assertEquals(-4.9944146013126755E-8, y[3].getReal(), 1.e-22);
194 Assert.assertEquals(-4.500974242661177E-8, y[4].getReal(), 1.e-22);
195 Assert.assertEquals(-2.785213556107612E-7, y[5].getReal(), 1.e-21);
196 }
197
198 @Test
199 public void testIssue625() {
200 doTestIssue625(Decimal64Field.getInstance());
201 }
202
203 private <T extends CalculusFieldElement<T>> void doTestIssue625(final Field<T> field) {
204
205 final T zero = field.getZero();
206
207 final UnnormalizedSphericalHarmonicsProvider provider =
208 GravityFieldFactory.getUnnormalizedProvider(4, 4);
209
210 final Frame frame = FramesFactory.getEME2000();
211 final Frame earthFrame = CelestialBodyFactory.getEarth().getBodyOrientedFrame();
212 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, 2007, 04, 16, 0, 46, 42.400, TimeScalesFactory.getUTC());
213
214
215
216
217
218
219
220 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(2.655989E7),
221 zero.add(2.719455286199036E-4),
222 zero.add(0.0041543085910249414),
223 zero.add(-0.3412974060023717),
224 zero.add(0.3960084733107685),
225 zero.add(8.566537840341699),
226 PositionAngle.TRUE,
227 frame,
228 initDate,
229 zero.add(3.986004415E14));
230
231 final FieldSpacecraftState<T> state = new FieldSpacecraftState<>(orbit, zero.add(1000.0));
232
233 final FieldAuxiliaryElements<T> auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), 1);
234
235
236 final DSSTForceModel tesseral = new DSSTTesseral(earthFrame,
237 Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider,
238 4, 4, 4, 8, 4, 4, 2);
239 tesseral.initializeShortPeriodTerms(auxiliaryElements, PropagationType.MEAN, tesseral.getParameters(field));
240
241
242 final DSSTForceModel tesseralDefault = new DSSTTesseral(earthFrame,
243 Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider);
244 tesseralDefault.initializeShortPeriodTerms(auxiliaryElements, PropagationType.MEAN, tesseralDefault.getParameters(field));
245
246
247 final T[] elements = tesseral.getMeanElementRate(state, auxiliaryElements, tesseral.getParameters(field));
248
249
250 final T[] elementsDefault = tesseralDefault.getMeanElementRate(state, auxiliaryElements, tesseralDefault.getParameters(field));
251
252
253 for (int i = 0; i < 6; i++) {
254 Assert.assertEquals(elements[i].getReal(), elementsDefault[i].getReal(), Double.MIN_VALUE);
255 }
256
257 }
258
259 @Test
260 public void testIssue736() {
261 doTestIssue736(Decimal64Field.getInstance());
262 }
263
264 private <T extends CalculusFieldElement<T>> void doTestIssue736(final Field<T> field) {
265
266
267 final UnnormalizedSphericalHarmonicsProvider provider =
268 GravityFieldFactory.getUnnormalizedProvider(4, 4);
269
270
271 final Frame frame = FramesFactory.getEME2000();
272 final Frame earthFrame = CelestialBodyFactory.getEarth().getBodyOrientedFrame();
273 final FieldAbsoluteDate<T> initDate = new FieldAbsoluteDate<>(field, 2007, 04, 16, 0, 46, 42.400, TimeScalesFactory.getUTC());
274
275
276 final T zero = field.getZero();
277 final FieldOrbit<T> orbit = new FieldEquinoctialOrbit<>(zero.add(2.655989E7), zero.add(2.719455286199036E-4),
278 zero.add(0.0041543085910249414), zero.add(-0.3412974060023717),
279 zero.add(0.3960084733107685), zero.add(8.566537840341699),
280 PositionAngle.TRUE, frame, initDate, zero.add(3.986004415E14));
281
282
283 final DSSTForceModel tesseral = new DSSTTesseral(earthFrame, Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider);
284 final T[] parameters = tesseral.getParameters(field);
285
286
287 tesseral.initializeShortPeriodTerms(new FieldAuxiliaryElements<>(orbit, 1), PropagationType.MEAN, parameters);
288
289
290 final FieldOrbit<T> shfitedOrbit = new FieldEquinoctialOrbit<>(zero.add(2.655989E7), zero.add(0.02),
291 zero.add(0.0041543085910249414), zero.add(-0.3412974060023717),
292 zero.add(0.3960084733107685), zero.add(8.566537840341699),
293 PositionAngle.TRUE, frame, initDate, zero.add(3.986004415E14));
294
295 final T[] elements = tesseral.getMeanElementRate(new FieldSpacecraftState<>(shfitedOrbit), new FieldAuxiliaryElements<>(shfitedOrbit, 1), parameters);
296
297
298
299
300 for (int i = 0; i < elements.length; i++) {
301 Assert.assertTrue(elements[i].getReal() != 0);
302 }
303
304 }
305
306 @Test
307 @SuppressWarnings("unchecked")
308 public void testShortPeriodTermsStateDerivatives() {
309
310
311 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
312 TimeScalesFactory.getUTC());
313
314 final Orbit orbit = new EquinoctialOrbit(42164000,
315 10e-3,
316 10e-3,
317 FastMath.tan(0.001745329) * FastMath.cos(2 * FastMath.PI / 3),
318 FastMath.tan(0.001745329) * FastMath.sin(2 * FastMath.PI / 3), 0.1,
319 PositionAngle.TRUE,
320 FramesFactory.getEME2000(),
321 initDate,
322 3.986004415E14);
323
324 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
325
326 final SpacecraftState meanState = new SpacecraftState(orbit);
327
328
329 final UnnormalizedSphericalHarmonicsProvider provider =
330 GravityFieldFactory.getUnnormalizedProvider(4, 4);
331 final Frame earthFrame = CelestialBodyFactory.getEarth().getBodyOrientedFrame();
332 final DSSTForceModel tesseral =
333 new DSSTTesseral(earthFrame,
334 Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider,
335 4, 4, 4, 8, 4, 4, 2);
336
337
338 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
339
340
341 final FieldSpacecraftState<Gradient> dsState = converter.getState(tesseral);
342 final Gradient[] dsParameters = converter.getParameters(dsState, tesseral);
343
344 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
345
346
347 final Gradient zero = dsState.getDate().getField().getZero();
348
349
350 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
351 shortPeriodTerms.addAll(tesseral.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING, dsParameters));
352 tesseral.updateShortPeriodTerms(dsParameters, dsState);
353 final Gradient[] shortPeriod = new Gradient[6];
354 Arrays.fill(shortPeriod, zero);
355 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
356 final Gradient[] spVariation = spt.value(dsState.getOrbit());
357 for (int i = 0; i < spVariation .length; i++) {
358 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
359 }
360 }
361
362 final double[][] shortPeriodJacobian = new double[6][6];
363
364 final double[] derivativesASP = shortPeriod[0].getGradient();
365 final double[] derivativesExSP = shortPeriod[1].getGradient();
366 final double[] derivativesEySP = shortPeriod[2].getGradient();
367 final double[] derivativesHxSP = shortPeriod[3].getGradient();
368 final double[] derivativesHySP = shortPeriod[4].getGradient();
369 final double[] derivativesLSP = shortPeriod[5].getGradient();
370
371
372 addToRow(derivativesASP, 0, shortPeriodJacobian);
373 addToRow(derivativesExSP, 1, shortPeriodJacobian);
374 addToRow(derivativesEySP, 2, shortPeriodJacobian);
375 addToRow(derivativesHxSP, 3, shortPeriodJacobian);
376 addToRow(derivativesHySP, 4, shortPeriodJacobian);
377 addToRow(derivativesLSP, 5, shortPeriodJacobian);
378
379
380 double[][] shortPeriodJacobianRef = new double[6][6];
381 double dP = 0.001;
382 double[] steps = NumericalPropagator.tolerances(1000000 * dP, orbit, orbitType)[0];
383 for (int i = 0; i < 6; i++) {
384
385 SpacecraftState stateM4 = shiftState(meanState, orbitType, -4 * steps[i], i);
386 double[] shortPeriodM4 = computeShortPeriodTerms(stateM4, tesseral);
387
388 SpacecraftState stateM3 = shiftState(meanState, orbitType, -3 * steps[i], i);
389 double[] shortPeriodM3 = computeShortPeriodTerms(stateM3, tesseral);
390
391 SpacecraftState stateM2 = shiftState(meanState, orbitType, -2 * steps[i], i);
392 double[] shortPeriodM2 = computeShortPeriodTerms(stateM2, tesseral);
393
394 SpacecraftState stateM1 = shiftState(meanState, orbitType, -1 * steps[i], i);
395 double[] shortPeriodM1 = computeShortPeriodTerms(stateM1, tesseral);
396
397 SpacecraftState stateP1 = shiftState(meanState, orbitType, 1 * steps[i], i);
398 double[] shortPeriodP1 = computeShortPeriodTerms(stateP1, tesseral);
399
400 SpacecraftState stateP2 = shiftState(meanState, orbitType, 2 * steps[i], i);
401 double[] shortPeriodP2 = computeShortPeriodTerms(stateP2, tesseral);
402
403 SpacecraftState stateP3 = shiftState(meanState, orbitType, 3 * steps[i], i);
404 double[] shortPeriodP3 = computeShortPeriodTerms(stateP3, tesseral);
405
406 SpacecraftState stateP4 = shiftState(meanState, orbitType, 4 * steps[i], i);
407 double[] shortPeriodP4 = computeShortPeriodTerms(stateP4, tesseral);
408
409 fillJacobianColumn(shortPeriodJacobianRef, i, orbitType, steps[i],
410 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
411 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
412
413 }
414
415 for (int m = 0; m < 6; ++m) {
416 for (int n = 0; n < 6; ++n) {
417 double error = FastMath.abs((shortPeriodJacobian[m][n] - shortPeriodJacobianRef[m][n]) / shortPeriodJacobianRef[m][n]);
418 Assert.assertEquals(0, error, 7.6e-10);
419 }
420 }
421
422 }
423
424 @Test
425 @SuppressWarnings("unchecked")
426 public void testShortPeriodTermsMuParametersDerivatives() {
427
428
429 final AbsoluteDate initDate = new AbsoluteDate(new DateComponents(2003, 05, 21), new TimeComponents(1, 0, 0.),
430 TimeScalesFactory.getUTC());
431
432 final Orbit orbit = new EquinoctialOrbit(42164000,
433 10e-3,
434 10e-3,
435 FastMath.tan(0.001745329) * FastMath.cos(2 * FastMath.PI / 3),
436 FastMath.tan(0.001745329) * FastMath.sin(2 * FastMath.PI / 3), 0.1,
437 PositionAngle.TRUE,
438 FramesFactory.getEME2000(),
439 initDate,
440 3.986004415E14);
441
442 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
443
444 final SpacecraftState meanState = new SpacecraftState(orbit);
445
446
447 final UnnormalizedSphericalHarmonicsProvider provider =
448 GravityFieldFactory.getUnnormalizedProvider(4, 4);
449 final Frame earthFrame = CelestialBodyFactory.getEarth().getBodyOrientedFrame();
450 final DSSTForceModel tesseral =
451 new DSSTTesseral(earthFrame,
452 Constants.WGS84_EARTH_ANGULAR_VELOCITY, provider,
453 4, 4, 4, 8, 4, 4, 2);
454
455 for (final ParameterDriver driver : tesseral.getParametersDrivers()) {
456 driver.setValue(driver.getReferenceValue());
457 driver.setSelected(driver.getName().equals(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT));
458 }
459
460
461 final DSSTGradientConverter converter = new DSSTGradientConverter(meanState, Utils.defaultLaw());
462
463
464 final FieldSpacecraftState<Gradient> dsState = converter.getState(tesseral);
465 final Gradient[] dsParameters = converter.getParameters(dsState, tesseral);
466
467 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
468
469
470 final Gradient zero = dsState.getDate().getField().getZero();
471
472
473 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<FieldShortPeriodTerms<Gradient>>();
474 shortPeriodTerms.addAll(tesseral.initializeShortPeriodTerms(fieldAuxiliaryElements, PropagationType.OSCULATING, dsParameters));
475 tesseral.updateShortPeriodTerms(dsParameters, dsState);
476 final Gradient[] shortPeriod = new Gradient[6];
477 Arrays.fill(shortPeriod, zero);
478 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
479 final Gradient[] spVariation = spt.value(dsState.getOrbit());
480 for (int i = 0; i < spVariation .length; i++) {
481 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
482 }
483 }
484
485 final double[][] shortPeriodJacobian = new double[6][1];
486
487 final double[] derivativesASP = shortPeriod[0].getGradient();
488 final double[] derivativesExSP = shortPeriod[1].getGradient();
489 final double[] derivativesEySP = shortPeriod[2].getGradient();
490 final double[] derivativesHxSP = shortPeriod[3].getGradient();
491 final double[] derivativesHySP = shortPeriod[4].getGradient();
492 final double[] derivativesLSP = shortPeriod[5].getGradient();
493
494 int index = converter.getFreeStateParameters();
495 for (ParameterDriver driver : tesseral.getParametersDrivers()) {
496 if (driver.isSelected()) {
497 shortPeriodJacobian[0][0] += derivativesASP[index];
498 shortPeriodJacobian[1][0] += derivativesExSP[index];
499 shortPeriodJacobian[2][0] += derivativesEySP[index];
500 shortPeriodJacobian[3][0] += derivativesHxSP[index];
501 shortPeriodJacobian[4][0] += derivativesHySP[index];
502 shortPeriodJacobian[5][0] += derivativesLSP[index];
503 ++index;
504 }
505 }
506
507
508 double[][] shortPeriodJacobianRef = new double[6][1];
509 ParameterDriversList bound = new ParameterDriversList();
510 for (final ParameterDriver driver : tesseral.getParametersDrivers()) {
511 if (driver.getName().equals(DSSTNewtonianAttraction.CENTRAL_ATTRACTION_COEFFICIENT)) {
512 driver.setSelected(true);
513 bound.add(driver);
514 } else {
515 driver.setSelected(false);
516 }
517 }
518
519 ParameterDriver selected = bound.getDrivers().get(0);
520 double p0 = selected.getReferenceValue();
521 double h = selected.getScale();
522
523 selected.setValue(p0 - 4 * h);
524 final double[] shortPeriodM4 = computeShortPeriodTerms(meanState, tesseral);
525
526 selected.setValue(p0 - 3 * h);
527 final double[] shortPeriodM3 = computeShortPeriodTerms(meanState, tesseral);
528
529 selected.setValue(p0 - 2 * h);
530 final double[] shortPeriodM2 = computeShortPeriodTerms(meanState, tesseral);
531
532 selected.setValue(p0 - 1 * h);
533 final double[] shortPeriodM1 = computeShortPeriodTerms(meanState, tesseral);
534
535 selected.setValue(p0 + 1 * h);
536 final double[] shortPeriodP1 = computeShortPeriodTerms(meanState, tesseral);
537
538 selected.setValue(p0 + 2 * h);
539 final double[] shortPeriodP2 = computeShortPeriodTerms(meanState, tesseral);
540
541 selected.setValue(p0 + 3 * h);
542 final double[] shortPeriodP3 = computeShortPeriodTerms(meanState, tesseral);
543
544 selected.setValue(p0 + 4 * h);
545 final double[] shortPeriodP4 = computeShortPeriodTerms(meanState, tesseral);
546
547 fillJacobianColumn(shortPeriodJacobianRef, 0, orbitType, h,
548 shortPeriodM4, shortPeriodM3, shortPeriodM2, shortPeriodM1,
549 shortPeriodP1, shortPeriodP2, shortPeriodP3, shortPeriodP4);
550
551 for (int i = 0; i < 6; ++i) {
552 Assert.assertEquals(shortPeriodJacobianRef[i][0],
553 shortPeriodJacobian[i][0],
554 FastMath.abs(shortPeriodJacobianRef[i][0] * 2e-10));
555 }
556
557 }
558
559 private double[] computeShortPeriodTerms(SpacecraftState state,
560 DSSTForceModel force) {
561
562 AuxiliaryElements auxiliaryElements = new AuxiliaryElements(state.getOrbit(), 1);
563
564 List<ShortPeriodTerms> shortPeriodTerms = new ArrayList<ShortPeriodTerms>();
565 double[] parameters = force.getParameters();
566 shortPeriodTerms.addAll(force.initializeShortPeriodTerms(auxiliaryElements, PropagationType.OSCULATING, parameters));
567 force.updateShortPeriodTerms(parameters, state);
568
569 double[] shortPeriod = new double[6];
570 for (ShortPeriodTerms spt : shortPeriodTerms) {
571 double[] spVariation = spt.value(state.getOrbit());
572 for (int i = 0; i < spVariation.length; i++) {
573 shortPeriod[i] += spVariation[i];
574 }
575 }
576
577 return shortPeriod;
578
579 }
580
581 private void fillJacobianColumn(double[][] jacobian, int column,
582 OrbitType orbitType, double h,
583 double[] M4h, double[] M3h,
584 double[] M2h, double[] M1h,
585 double[] P1h, double[] P2h,
586 double[] P3h, double[] P4h) {
587 for (int i = 0; i < jacobian.length; ++i) {
588 jacobian[i][column] = ( -3 * (P4h[i] - M4h[i]) +
589 32 * (P3h[i] - M3h[i]) -
590 168 * (P2h[i] - M2h[i]) +
591 672 * (P1h[i] - M1h[i])) / (840 * h);
592 }
593 }
594
595 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
596 double delta, int column) {
597
598 double[][] array = stateToArray(state, orbitType);
599 array[0][column] += delta;
600
601 return arrayToState(array, orbitType, state.getFrame(), state.getDate(),
602 state.getMu(), state.getAttitude());
603
604 }
605
606 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
607 double[][] array = new double[2][6];
608
609 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngle.MEAN, array[0], array[1]);
610 return array;
611 }
612
613 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType,
614 Frame frame, AbsoluteDate date, double mu,
615 Attitude attitude) {
616 EquinoctialOrbit orbit = (EquinoctialOrbit) orbitType.mapArrayToOrbit(array[0], array[1], PositionAngle.MEAN, date, mu, frame);
617 return new SpacecraftState(orbit, attitude);
618 }
619
620
621
622
623
624
625 private void addToRow(final double[] derivatives, final int index,
626 final double[][] jacobian) {
627
628 for (int i = 0; i < 6; i++) {
629 jacobian[index][i] += derivatives[i];
630 }
631
632 }
633
634 @Before
635 public void setUp() throws IOException, ParseException {
636 Utils.setDataRoot("regular-data:potential/shm-format");
637 }
638
639 }