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 org.hipparchus.Field;
20 import org.hipparchus.analysis.differentiation.Gradient;
21 import org.hipparchus.geometry.euclidean.threed.Vector3D;
22 import org.hipparchus.ode.ODEIntegrator;
23 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
24 import org.hipparchus.util.Binary64;
25 import org.hipparchus.util.Binary64Field;
26 import org.hipparchus.util.FastMath;
27 import org.hipparchus.util.MathArrays;
28 import org.junit.jupiter.api.Assertions;
29 import org.junit.jupiter.api.BeforeEach;
30 import org.junit.jupiter.api.Test;
31 import org.mockito.Mockito;
32 import org.orekit.Utils;
33 import org.orekit.attitudes.Attitude;
34 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
35 import org.orekit.forces.gravity.potential.GravityFieldFactory;
36 import org.orekit.forces.gravity.potential.UnnormalizedSphericalHarmonicsProvider;
37 import org.orekit.frames.Frame;
38 import org.orekit.frames.FramesFactory;
39 import org.orekit.orbits.*;
40 import org.orekit.propagation.FieldSpacecraftState;
41 import org.orekit.propagation.PropagationType;
42 import org.orekit.propagation.SpacecraftState;
43 import org.orekit.propagation.ToleranceProvider;
44 import org.orekit.propagation.numerical.NumericalPropagator;
45 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
46 import org.orekit.propagation.semianalytical.dsst.forces.DSSTJ2SquaredClosedForm;
47 import org.orekit.propagation.semianalytical.dsst.forces.DSSTZonal;
48 import org.orekit.propagation.semianalytical.dsst.forces.ZeisModel;
49 import org.orekit.propagation.semianalytical.dsst.utilities.AuxiliaryElements;
50 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
51 import org.orekit.time.AbsoluteDate;
52 import org.orekit.time.FieldAbsoluteDate;
53 import org.orekit.time.TimeScalesFactory;
54 import org.orekit.utils.Constants;
55 import org.orekit.utils.IERSConventions;
56
57 import java.io.IOException;
58 import java.text.ParseException;
59
60 public class DSSTJ2SquaredClosedFormTest {
61
62 private UnnormalizedSphericalHarmonicsProvider provider;
63
64 @BeforeEach
65 public void setUp() throws IOException, ParseException {
66 Utils.setDataRoot("regular-data:potential/shm-format");
67 provider = GravityFieldFactory.getUnnormalizedProvider(2, 0);
68 }
69
70 @SuppressWarnings("unchecked")
71 @Test
72 public void testShortPeriodZeis() {
73 DSSTJ2SquaredClosedForm j2Squared = new DSSTJ2SquaredClosedForm(new ZeisModel(), provider);
74 AuxiliaryElements auxiliaryElements = Mockito.mock(AuxiliaryElements.class);
75 FieldAuxiliaryElements<Binary64> fAuxiliaryElements = Mockito.mock(FieldAuxiliaryElements.class);
76 Binary64[] emptyArray = MathArrays.buildArray(Binary64Field.getInstance(), 0);
77
78 Assertions.assertTrue(j2Squared.initializeShortPeriodTerms(auxiliaryElements, PropagationType.MEAN, new double[0]).isEmpty());
79 Assertions.assertTrue(j2Squared.initializeShortPeriodTerms(fAuxiliaryElements, PropagationType.MEAN, emptyArray).isEmpty());
80 j2Squared.updateShortPeriodTerms(new double[0]);
81 j2Squared.updateShortPeriodTerms(emptyArray);
82 Assertions.assertTrue(j2Squared.initializeShortPeriodTerms(auxiliaryElements, PropagationType.MEAN, new double[0]).isEmpty());
83 Assertions.assertTrue(j2Squared.initializeShortPeriodTerms(fAuxiliaryElements, PropagationType.MEAN, emptyArray).isEmpty());
84 }
85
86
87
88 @Test
89 public void testGetMeanElementRateZeis() {
90
91
92 final Orbit orbit = createOrbit(200000.0, 210000.0);
93 final SpacecraftState state = new SpacecraftState(orbit, 1000.0);
94
95
96 final DSSTForceModel j2Squared = new DSSTJ2SquaredClosedForm(new ZeisModel(), provider);
97
98
99 final AuxiliaryElements auxiliaryElements = new AuxiliaryElements(state.getOrbit(), 1);
100
101
102 final double[] elements = j2Squared.getMeanElementRate(state, auxiliaryElements, j2Squared.getParameters());
103
104
105 Assertions.assertEquals(0.0, elements[0], 1.e-25);
106 Assertions.assertEquals(2.5668006482691996E-14, elements[1], 1.e-25);
107 Assertions.assertEquals(7.052226821361117E-14, elements[2], 1.e-25);
108 Assertions.assertEquals(3.6576370779863025E-10, elements[3], 1.e-25);
109 Assertions.assertEquals(-4.3590021280959657E-10, elements[4], 1.e-25);
110 Assertions.assertEquals(1.2618917692354564E-8, elements[5], 1.e-25);
111 }
112
113
114
115
116 @Test
117 public void testFieldGetMeanElementRateZeis() {
118
119
120 final Field<Binary64> field = Binary64Field.getInstance();
121 final Binary64 zero = field.getZero();
122
123
124 final FieldOrbit<Binary64> orbit = createOrbit(field, 200000.0, 210000.0);
125 final FieldSpacecraftState<Binary64> state = new FieldSpacecraftState<>(orbit, zero.add(1000.0));
126
127
128 final DSSTForceModel j2Squared = new DSSTJ2SquaredClosedForm(new ZeisModel(), provider);
129
130
131 final FieldAuxiliaryElements<Binary64> auxiliaryElements = new FieldAuxiliaryElements<>(state.getOrbit(), 1);
132
133
134 final Binary64[] elements = j2Squared.getMeanElementRate(state, auxiliaryElements, j2Squared.getParameters(field));
135
136
137 Assertions.assertEquals(0.0, elements[0].getReal(), 1.e-25);
138 Assertions.assertEquals(2.5668006482691996E-14, elements[1].getReal(), 1.e-25);
139 Assertions.assertEquals(7.052226821361117E-14, elements[2].getReal(), 1.e-25);
140 Assertions.assertEquals(3.6576370779863025E-10, elements[3].getReal(), 1.e-25);
141 Assertions.assertEquals(-4.3590021280959657E-10, elements[4].getReal(), 1.e-25);
142 Assertions.assertEquals(1.2618917692354564E-8, elements[5].getReal(), 1.e-25);
143
144 }
145
146 private void doTestComparisonWithNumerical(final double perigeeAltitude, final double apogeeAltitude,
147 final double currentDifferenceWithoutJ2Squared,
148 final double currentDifferenceWithJ2Squared) {
149
150
151 final Orbit initialOrbit = createOrbit(perigeeAltitude, apogeeAltitude);
152 final SpacecraftState initialState = new SpacecraftState(initialOrbit, 1000.0);
153
154
155 final double duration = 2.0 * Constants.JULIAN_DAY;
156 final AbsoluteDate end = initialOrbit.getDate().shiftedBy(duration);
157
158
159 final double[][] tolerances = ToleranceProvider.getDefaultToleranceProvider(1.).getTolerances(initialOrbit, initialOrbit.getType());
160 final ODEIntegrator numIntegrator = new DormandPrince853Integrator(0.001, 300.0, tolerances[0], tolerances[1]);
161 final NumericalPropagator numPropagator = new NumericalPropagator(numIntegrator);
162 numPropagator.addForceModel(new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), GravityFieldFactory.getNormalizedProvider(provider)));
163 numPropagator.setInitialState(initialState);
164
165
166 final ODEIntegrator dsstIntegrator = new DormandPrince853Integrator(initialOrbit.getKeplerianPeriod(), 10.0 * initialOrbit.getKeplerianPeriod(), tolerances[0], tolerances[1]);
167 final DSSTPropagator dsstPropagatorWithoutJ2Squared = new DSSTPropagator(dsstIntegrator, PropagationType.OSCULATING);
168 dsstPropagatorWithoutJ2Squared.addForceModel(new DSSTZonal(provider));
169 dsstPropagatorWithoutJ2Squared.setInitialState(initialState, PropagationType.OSCULATING);
170
171
172 final DSSTPropagator dsstPropagatorWithJ2Squared = new DSSTPropagator(dsstIntegrator, PropagationType.OSCULATING);
173 dsstPropagatorWithJ2Squared.addForceModel(new DSSTZonal(provider));
174 dsstPropagatorWithJ2Squared.addForceModel(new DSSTJ2SquaredClosedForm(new ZeisModel(), provider));
175 dsstPropagatorWithJ2Squared.setInitialState(initialState, PropagationType.OSCULATING);
176
177
178 final Vector3D propagatedNum = numPropagator.propagate(end).getPosition();
179 final Vector3D propagatedDsstWitoutJ2Squared = dsstPropagatorWithoutJ2Squared.propagate(end).getPosition();
180 final Vector3D propagatedDsstWithJ2Squared = dsstPropagatorWithJ2Squared.propagate(end).getPosition();
181
182
183 final double differenceWithoutJ2Squared = FastMath.abs(Vector3D.distance(propagatedNum, propagatedDsstWitoutJ2Squared));
184 final double differenceWithJ2Squared = FastMath.abs(Vector3D.distance(propagatedNum, propagatedDsstWithJ2Squared));
185
186
187 Assertions.assertTrue(differenceWithJ2Squared < differenceWithoutJ2Squared);
188 Assertions.assertEquals(0.0, differenceWithoutJ2Squared, currentDifferenceWithoutJ2Squared);
189 Assertions.assertEquals(0.0, differenceWithJ2Squared, currentDifferenceWithJ2Squared);
190
191 }
192
193
194
195
196
197
198
199 @Test
200 public void testComparisonWithNumericalVeryLeo() {
201 doTestComparisonWithNumerical(200000.0, 210000.0, 20461.1, 6013.1);
202 }
203
204
205
206
207
208
209
210 @Test
211 public void testComparisonWithNumericalLeo() {
212 doTestComparisonWithNumerical(650000.0, 680000.0, 15291.5, 4653.4);
213 }
214
215
216
217
218
219
220
221 @Test
222 public void testComparisonWithNumericalMeo() {
223 doTestComparisonWithNumerical(5622000.0, 5959000.0, 1595.6, 689.6);
224 }
225
226
227
228
229 @Test
230 public void testMeanElementRateDerivatives() {
231
232
233 final OrbitType orbitType = OrbitType.EQUINOCTIAL;
234 final Orbit orbit = createOrbit(650000.0, 680000.0);
235 final SpacecraftState state = new SpacecraftState(orbit, 1000.0);
236
237
238 final DSSTForceModel j2Squared = new DSSTJ2SquaredClosedForm(new ZeisModel(), provider);
239
240
241 final DSSTGradientConverter converter = new DSSTGradientConverter(state, Utils.defaultLaw());
242
243
244 final FieldSpacecraftState<Gradient> dsState = converter.getState(j2Squared);
245 final Gradient[] dsParameters = converter.getParameters(dsState, j2Squared);
246
247 final FieldAuxiliaryElements<Gradient> fieldAuxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), 1);
248
249
250 final Gradient[] meanRates = j2Squared.getMeanElementRate(dsState, fieldAuxiliaryElements, dsParameters);
251 final double[][] meanElementRatesJacobian = new double[6][6];
252
253 final double[] derivativesA = meanRates[0].getGradient();
254 final double[] derivativesEx = meanRates[1].getGradient();
255 final double[] derivativesEy = meanRates[2].getGradient();
256 final double[] derivativesHx = meanRates[3].getGradient();
257 final double[] derivativesHy = meanRates[4].getGradient();
258 final double[] derivativesL = meanRates[5].getGradient();
259
260
261 addToRow(derivativesA, 0, meanElementRatesJacobian);
262 addToRow(derivativesEx, 1, meanElementRatesJacobian);
263 addToRow(derivativesEy, 2, meanElementRatesJacobian);
264 addToRow(derivativesHx, 3, meanElementRatesJacobian);
265 addToRow(derivativesHy, 4, meanElementRatesJacobian);
266 addToRow(derivativesL, 5, meanElementRatesJacobian);
267
268
269 double[][] meanElementRatesJacobianRef = new double[6][6];
270 double dP = 1.0;
271 double[] steps = ToleranceProvider.getDefaultToleranceProvider(dP * 1000).getTolerances(orbit, orbitType)[0];
272 for (int i = 0; i < 6; i++) {
273
274 SpacecraftState stateM4 = shiftState(state, orbitType, -4 * steps[i], i);
275 double[] meanRatesM4 = meanElementsRates(stateM4, j2Squared);
276
277 SpacecraftState stateM3 = shiftState(state, orbitType, -3 * steps[i], i);
278 double[] meanRatesM3 = meanElementsRates(stateM3, j2Squared);
279
280 SpacecraftState stateM2 = shiftState(state, orbitType, -2 * steps[i], i);
281 double[] meanRatesM2 = meanElementsRates(stateM2, j2Squared);
282
283 SpacecraftState stateM1 = shiftState(state, orbitType, -1 * steps[i], i);
284 double[] meanRatesM1 = meanElementsRates(stateM1, j2Squared);
285
286 SpacecraftState stateP1 = shiftState(state, orbitType, 1 * steps[i], i);
287 double[] meanRatesP1 = meanElementsRates(stateP1, j2Squared);
288
289 SpacecraftState stateP2 = shiftState(state, orbitType, 2 * steps[i], i);
290 double[] meanRatesP2 = meanElementsRates(stateP2, j2Squared);
291
292 SpacecraftState stateP3 = shiftState(state, orbitType, 3 * steps[i], i);
293 double[] meanRatesP3 = meanElementsRates(stateP3, j2Squared);
294
295 SpacecraftState stateP4 = shiftState(state, orbitType, 4 * steps[i], i);
296 double[] meanRatesP4 = meanElementsRates(stateP4, j2Squared);
297
298 fillJacobianColumn(meanElementRatesJacobianRef, i, orbitType, steps[i],
299 meanRatesM4, meanRatesM3, meanRatesM2, meanRatesM1,
300 meanRatesP1, meanRatesP2, meanRatesP3, meanRatesP4);
301
302 }
303
304 for (int m = 0; m < 6; ++m) {
305 for (int n = 0; n < 6; ++n) {
306 if (meanElementRatesJacobian[m][n] != 0.0) {
307 double error = FastMath.abs((meanElementRatesJacobian[m][n] - meanElementRatesJacobianRef[m][n]) / meanElementRatesJacobianRef[m][n]);
308 Assertions.assertEquals(0, error, 3.19E-8);
309 }
310 }
311 }
312
313 }
314
315 private Orbit createOrbit(final double perigeeAltitude, final double apogeeAltitude) {
316
317
318 final Frame frame = FramesFactory.getEME2000();
319 final AbsoluteDate epoch = new AbsoluteDate(2007, 04, 16, 0, 46, 42.400, TimeScalesFactory.getUTC());
320
321
322 final double apogee = Constants.WGS84_EARTH_EQUATORIAL_RADIUS + apogeeAltitude;
323 final double perigee = Constants.WGS84_EARTH_EQUATORIAL_RADIUS + perigeeAltitude;
324 final double sma = 0.5 * (apogee + perigee);
325 final double ecc = 1.0 - perigee / sma;
326 final double inc = FastMath.toRadians(10.0);
327 final double raan = FastMath.toRadians(40.0);
328 final double aop = FastMath.toRadians(120.0);
329 final double anom = 0.0;
330 final PositionAngleType angleType = PositionAngleType.MEAN;
331
332
333 final KeplerianOrbit kep = new KeplerianOrbit(sma, ecc, inc, aop, raan, anom, angleType, frame, epoch, provider.getMu());
334
335
336 return OrbitType.EQUINOCTIAL.convertType(kep);
337
338 }
339
340 private FieldOrbit<Binary64> createOrbit(final Field<Binary64> field, final double perigeeAltitude, final double apogeeAltitude) {
341
342
343 final Binary64 zero = field.getZero();
344
345
346 final Frame frame = FramesFactory.getEME2000();
347 final AbsoluteDate epoch = new AbsoluteDate(2007, 04, 16, 0, 46, 42.400, TimeScalesFactory.getUTC());
348 final FieldAbsoluteDate<Binary64> fieldEpoch = new FieldAbsoluteDate<Binary64>(field, epoch);
349
350
351 final double apogee = Constants.WGS84_EARTH_EQUATORIAL_RADIUS + apogeeAltitude;
352 final double perigee = Constants.WGS84_EARTH_EQUATORIAL_RADIUS + perigeeAltitude;
353 final double sma = 0.5 * (apogee + perigee);
354 final double ecc = 1.0 - perigee / sma;
355 final double inc = FastMath.toRadians(10.0);
356 final double raan = FastMath.toRadians(40.0);
357 final double aop = FastMath.toRadians(120.0);
358 final double anom = 0.0;
359 final PositionAngleType angleType = PositionAngleType.MEAN;
360
361
362 final FieldKeplerianOrbit<Binary64> fieldKep = new FieldKeplerianOrbit<Binary64>(zero.add(sma), zero.add(ecc), zero.add(inc),
363 zero.add(aop), zero.add(raan), zero.add(anom),
364 angleType, frame, fieldEpoch, zero.add(provider.getMu()));
365
366
367 return OrbitType.EQUINOCTIAL.convertType(fieldKep);
368
369 }
370
371 private double[] meanElementsRates(SpacecraftState state, DSSTForceModel force) {
372 AuxiliaryElements auxiliaryElements = new AuxiliaryElements(state.getOrbit(), 1);
373 return force.getMeanElementRate(state, auxiliaryElements, force.getParameters());
374 }
375
376 private void fillJacobianColumn(double[][] jacobian, int column,
377 OrbitType orbitType, double h,
378 double[] M4h, double[] M3h,
379 double[] M2h, double[] M1h,
380 double[] P1h, double[] P2h,
381 double[] P3h, double[] P4h) {
382 for (int i = 0; i < jacobian.length; ++i) {
383 jacobian[i][column] = ( -3 * (P4h[i] - M4h[i]) +
384 32 * (P3h[i] - M3h[i]) -
385 168 * (P2h[i] - M2h[i]) +
386 672 * (P1h[i] - M1h[i])) / (840 * h);
387 }
388 }
389
390 private SpacecraftState shiftState(SpacecraftState state, OrbitType orbitType,
391 double delta, int column) {
392 double[][] array = stateToArray(state, orbitType);
393 array[0][column] += delta;
394 return arrayToState(array, orbitType, state.getFrame(), state.getDate(),
395 state.getOrbit().getMu(), state.getAttitude());
396
397 }
398
399 private double[][] stateToArray(SpacecraftState state, OrbitType orbitType) {
400 double[][] array = new double[2][6];
401 orbitType.mapOrbitToArray(state.getOrbit(), PositionAngleType.MEAN, array[0], array[1]);
402 return array;
403 }
404
405 private SpacecraftState arrayToState(double[][] array, OrbitType orbitType,
406 Frame frame, AbsoluteDate date, double mu,
407 Attitude attitude) {
408 EquinoctialOrbit orbit = (EquinoctialOrbit) orbitType.mapArrayToOrbit(array[0], array[1], PositionAngleType.MEAN, date, mu, frame);
409 return new SpacecraftState(orbit, attitude);
410 }
411
412 private void addToRow(final double[] derivatives, final int index,
413 final double[][] jacobian) {
414 for (int i = 0; i < 6; i++) {
415 jacobian[index][i] += derivatives[i];
416 }
417 }
418 }