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