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