1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.utils;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.analysis.UnivariateFunction;
23 import org.hipparchus.analysis.differentiation.DSFactory;
24 import org.hipparchus.analysis.differentiation.Derivative;
25 import org.hipparchus.analysis.differentiation.DerivativeStructure;
26 import org.hipparchus.analysis.differentiation.UnivariateDifferentiableFunction;
27 import org.hipparchus.analysis.polynomials.PolynomialFunction;
28 import org.hipparchus.analysis.solvers.BaseUnivariateSolver;
29 import org.hipparchus.analysis.solvers.BracketingNthOrderBrentSolver;
30 import org.hipparchus.analysis.solvers.UnivariateSolverUtils;
31 import org.hipparchus.exception.LocalizedCoreFormats;
32 import org.hipparchus.exception.MathIllegalArgumentException;
33 import org.hipparchus.exception.MathIllegalStateException;
34 import org.hipparchus.exception.MathRuntimeException;
35 import org.hipparchus.geometry.euclidean.threed.Vector3D;
36 import org.hipparchus.ode.nonstiff.DormandPrince853Integrator;
37 import org.hipparchus.random.RandomGenerator;
38 import org.hipparchus.random.Well19937a;
39 import org.hipparchus.util.FastMath;
40 import org.hipparchus.util.FieldSinCos;
41 import org.hipparchus.util.MathUtils;
42 import org.hipparchus.util.SinCos;
43 import org.junit.Assert;
44 import org.junit.Before;
45 import org.junit.Test;
46 import org.orekit.Utils;
47 import org.orekit.bodies.BodyShape;
48 import org.orekit.bodies.CelestialBodyFactory;
49 import org.orekit.bodies.GeodeticPoint;
50 import org.orekit.bodies.OneAxisEllipsoid;
51 import org.orekit.errors.OrekitException;
52 import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
53 import org.orekit.forces.gravity.ThirdBodyAttraction;
54 import org.orekit.forces.gravity.potential.GravityFieldFactory;
55 import org.orekit.forces.gravity.potential.ICGEMFormatReader;
56 import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
57 import org.orekit.frames.FramesFactory;
58 import org.orekit.orbits.CircularOrbit;
59 import org.orekit.orbits.Orbit;
60 import org.orekit.orbits.OrbitType;
61 import org.orekit.orbits.PositionAngle;
62 import org.orekit.propagation.Propagator;
63 import org.orekit.propagation.SpacecraftState;
64 import org.orekit.propagation.numerical.NumericalPropagator;
65 import org.orekit.time.AbsoluteDate;
66 import org.orekit.time.TimeScalarFunction;
67 import org.orekit.time.TimeScale;
68 import org.orekit.time.TimeScalesFactory;
69
70 public class SecularAndHarmonicTest {
71
72 private TimeScale utc;
73 private NormalizedSphericalHarmonicsProvider gravityField;
74 private BodyShape earth;
75 private TimeScalarFunction gmst;
76
77 @Before
78 public void setUp() {
79
80 Utils.setDataRoot("regular-data:potential");
81 GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", false));
82 utc = TimeScalesFactory.getUTC();
83 gravityField = GravityFieldFactory.getNormalizedProvider(8, 8);
84 earth = new OneAxisEllipsoid(Constants.WGS84_EARTH_EQUATORIAL_RADIUS,
85 Constants.WGS84_EARTH_FLATTENING,
86 FramesFactory.getGTOD(IERSConventions.IERS_2010, true));
87 TimeScale ut1 = TimeScalesFactory.getUT1(IERSConventions.IERS_2010, true);
88 gmst = IERSConventions.IERS_2010.getGMSTFunction(ut1);
89 }
90
91 @Test
92 public void testSunSynchronization() {
93
94 int nbOrbits = 143;
95 double mst = 10.5;
96
97
98
99 CircularOrbit initialGuessedOrbit =
100 new CircularOrbit(7169867.824275421,
101 0.0, 0.0010289683741791197,
102 FastMath.toRadians(98.5680307986701),
103 FastMath.toRadians(-189.6132856166402),
104 FastMath.PI, PositionAngle.TRUE,
105 FramesFactory.getEME2000(),
106 new AbsoluteDate(2003, 4, 5, utc),
107 gravityField.getMu());
108 final double[] initialMSTModel = fitGMST(initialGuessedOrbit, nbOrbits, mst);
109
110
111 Assert.assertTrue(FastMath.abs(mst - initialMSTModel[0]) * 3600.0 > 0.4);
112 Assert.assertTrue(FastMath.abs(initialMSTModel[1]) * 3600 > 0.5 / Constants.JULIAN_DAY);
113
114 CircularOrbit finalOrbit =
115 new CircularOrbit(7173353.364197798,
116 -3.908629707615073E-4, 0.0013502004064500472,
117 FastMath.toRadians(98.56430772945006),
118 FastMath.toRadians(-189.61151932993425),
119 FastMath.PI, PositionAngle.TRUE,
120 FramesFactory.getEME2000(),
121 new AbsoluteDate(2003, 4, 5, utc),
122 gravityField.getMu());
123 final double[] finalMSTModel = fitGMST(finalOrbit, nbOrbits, mst);
124
125
126 Assert.assertTrue(FastMath.abs(mst - finalMSTModel[0]) * 3600.0 < 0.0012);
127 Assert.assertTrue(FastMath.abs(finalMSTModel[1]) * 3600 < 0.0004 / Constants.JULIAN_DAY);
128
129 }
130
131 @Test
132 public void testReset() {
133 final double SUN_PULSATION = 4.0 * FastMath.PI / Constants.JULIAN_YEAR;
134
135
136 final RandomGenerator random = new Well19937a(0x8de2c5d0e210588dl);
137 final AbsoluteDate t0 = AbsoluteDate.J2000_EPOCH;
138 final double[][] data = new double[2][200];
139 for (int iD = 0; iD < data[0].length; ++iD) {
140 data[0][iD] = random.nextDouble();
141 data[1][iD] = random.nextDouble();
142 }
143
144
145
146
147 final double[] initialGuess = { 0.0, 0.0, 0.0, 0.0 };
148 final SecularAndHarmonic[] indepModel = new SecularAndHarmonic[2];
149 final SecularAndHarmonic resettingModel = new SecularAndHarmonic(1, SUN_PULSATION);
150 for (int iM = 0; iM < indepModel.length; ++iM) {
151 indepModel[iM] = new SecularAndHarmonic(1, SUN_PULSATION);
152 indepModel[iM].resetFitting(t0, initialGuess);
153 resettingModel.resetFitting(t0, initialGuess);
154
155 for (int iD = 0; iD < data[0].length; ++iD) {
156 final AbsoluteDate t = t0.shiftedBy(iD);
157 indepModel[iM].addPoint(t, data[iM][iD]);
158 resettingModel.addPoint(t, data[iM][iD]);
159 }
160 indepModel[iM].fit();
161 resettingModel.fit();
162
163 Assert.assertArrayEquals(indepModel[iM].getFittedParameters(),
164 resettingModel.getFittedParameters(),
165 1e-14);
166 }
167
168 }
169
170 @Test
171 public void testLinear() {
172 SecularAndHarmonic sh = new SecularAndHarmonic(1);
173 sh.setConvergenceRMS(1.0e-6);
174 sh.setMaxIter(5);
175 sh.resetFitting(AbsoluteDate.J2000_EPOCH, 0.0, 0.0);
176 sh.addPoint(sh.getReferenceDate().shiftedBy(1.0), 1.0);
177 sh.addPoint(sh.getReferenceDate().shiftedBy(2.0), 2.0);
178 sh.addPoint(sh.getReferenceDate().shiftedBy(3.0), 3.0);
179 sh.fit();
180 Assert.assertEquals(0.0, sh.getFittedParameters()[0], 1.0e-15);
181 Assert.assertEquals(1.0, sh.getFittedParameters()[1], 1.0e-15);
182 }
183
184 @Test
185 public void testImpossibleConvergence() {
186 SecularAndHarmonic sh = new SecularAndHarmonic(1);
187 sh.setConvergenceRMS(1.0e-6);
188 sh.setMaxIter(5);
189 sh.resetFitting(AbsoluteDate.J2000_EPOCH, 0.0, 0.0);
190 sh.addPoint(sh.getReferenceDate().shiftedBy(1.0), 1.0);
191 sh.addPoint(sh.getReferenceDate().shiftedBy(2.0), 2.0);
192 sh.addPoint(sh.getReferenceDate().shiftedBy(3.0), Double.NaN);
193 try {
194 sh.fit();
195 Assert.fail("an exception should have been thrown");
196 } catch (MathIllegalStateException mise) {
197 Assert.assertEquals(LocalizedCoreFormats.MAX_COUNT_EXCEEDED, mise.getSpecifier());
198 }
199 }
200
201 @Test
202 public void testConvergence() {
203 try {
204 doTestConvergence(0.2, 3);
205 Assert.fail("an exception should have been thrown");
206 } catch (MathIllegalStateException mise) {
207 Assert.assertEquals(LocalizedCoreFormats.MAX_COUNT_EXCEEDED, mise.getSpecifier());
208 }
209 doTestConvergence(0.3, 3);
210 }
211
212 private void doTestConvergence(final double rms, final int maxIter) {
213 SecularAndHarmonic sh = new SecularAndHarmonic(0, 1.0, 2.0, 3.0);
214 sh.setConvergenceRMS(rms);
215 sh.setMaxIter(maxIter);
216 sh.resetFitting(AbsoluteDate.J2000_EPOCH, new double[7]);
217 for (double t = -1.0; t <= 1.0; t += 0.125) {
218
219 sh.addPoint(sh.getReferenceDate().shiftedBy(t), FastMath.copySign(1.0, t));
220 }
221 sh.fit();
222 }
223
224 @Test
225 public void testPerfectOsculatingModel() {
226
227 final AbsoluteDate tRef = AbsoluteDate.J2000_EPOCH;
228 final PerfectModel osc = new PerfectModel(tRef);
229 osc.setSecular(1.0, 0.5, 0.125);
230 osc.addHarmonics(2.0, 5.0, -5.0);
231 osc.addHarmonics(3.0, 4.0, -6.0);
232 osc.addHarmonics(5.0, 3.0, -7.0);
233 osc.addHarmonics(7.0, 2.0, -8.0);
234 SecularAndHarmonic sh = osc.fittedSH(200, 1.0);
235
236 Assert.assertEquals(0.0, sh.getReferenceDate().durationFrom(tRef), 1.0e-10);
237
238 Assert.assertEquals(FastMath.hypot(5, -5) +
239 FastMath.hypot(4, -6) +
240 FastMath.hypot(3, -7) +
241 FastMath.hypot(2, -8),
242 sh.getHarmonicAmplitude(), 1.0e-10);
243
244 for (double dt = 0.0; dt < 120.0; dt += 1.0) {
245 final AbsoluteDate date = tRef.shiftedBy(dt);
246 Assert.assertEquals(osc.value(date), sh.osculatingValue(date), 1.0e-10);
247 Assert.assertEquals(osc.firstDerivative(date), sh.osculatingDerivative(date), 1.0e-10);
248 Assert.assertEquals(osc.secondDerivative(date), sh.osculatingSecondDerivative(date), 1.0e-10);
249 }
250
251 }
252
253 @Test
254 public void testPerfectMeanModel() {
255
256 final AbsoluteDate tRef = AbsoluteDate.J2000_EPOCH;
257 final PerfectModel osc = new PerfectModel(tRef);
258 osc.setSecular(1.0, 0.5, 0.125);
259 osc.addHarmonics(2.0, 5.0, -5.0);
260 osc.addHarmonics(3.0, 4.0, -6.0);
261 osc.addHarmonics(5.0, 3.0, -7.0);
262 osc.addHarmonics(7.0, 2.0, -8.0);
263 SecularAndHarmonic sh = osc.fittedSH(200, 1.0);
264 final PerfectModel mean = new PerfectModel(tRef);
265 mean.setSecular(1.0, 0.5);
266 mean.addHarmonics(2.0, 5.0, -5.0);
267 mean.addHarmonics(3.0, 4.0, -6.0);
268
269 for (double dt = 0.0; dt < 12.0; dt += 0.01) {
270 final AbsoluteDate date = tRef.shiftedBy(dt);
271 Assert.assertEquals(mean.value(date), sh.meanValue(date, 1, 2), 1.0e-10);
272 Assert.assertEquals(mean.firstDerivative(date), sh.meanDerivative(date, 1, 2), 1.0e-10);
273 Assert.assertEquals(mean.secondDerivative(date), sh.meanSecondDerivative(date, 1, 2), 1.0e-10);
274 }
275
276 }
277
278 private double[] fitGMST(CircularOrbit orbit, int nbOrbits, double mst) {
279
280 double period = orbit.getKeplerianPeriod();
281 double duration = nbOrbits * period;
282 NumericalPropagator propagator = createPropagator(orbit);
283 SecularAndHarmonic mstModel = new SecularAndHarmonic(2,
284 2.0 * FastMath.PI / Constants.JULIAN_YEAR,
285 4.0 * FastMath.PI / Constants.JULIAN_YEAR,
286 2.0 * FastMath.PI / Constants.JULIAN_DAY,
287 4.0 * FastMath.PI / Constants.JULIAN_DAY);
288 mstModel.resetFitting(orbit.getDate(), new double[] {
289 mst, -1.0e-10, -1.0e-17,
290 1.0e-3, 1.0e-3, 1.0e-5, 1.0e-5, 1.0e-5, 1.0e-5, 1.0e-5, 1.0e-5
291 });
292
293
294 SpacecraftState crossing =
295 findFirstCrossing(0.0, false, orbit.getDate(),
296 orbit.getDate().shiftedBy(2 * period),
297 0.01 * period, propagator);
298
299 while (crossing != null &&
300 crossing.getDate().durationFrom(orbit.getDate()) < (nbOrbits * period)) {
301 final AbsoluteDate previousDate = crossing.getDate();
302 crossing = findLatitudeCrossing(0.0, previousDate.shiftedBy(period),
303 previousDate.shiftedBy(2 * period),
304 0.01 * period, period / 8, propagator);
305 if (crossing != null) {
306
307
308 mstModel.addPoint(crossing.getDate(), meanSolarTime(crossing.getOrbit()));
309
310
311 period = crossing.getDate().durationFrom(previousDate);
312
313 }
314
315 }
316
317
318
319 mstModel.fit();
320 return mstModel.approximateAsPolynomialOnly(1, orbit.getDate(), 2, 2,
321 orbit.getDate(), orbit.getDate().shiftedBy(duration),
322 0.01 * period);
323
324 }
325
326 private NumericalPropagator createPropagator(CircularOrbit orbit) {
327 OrbitType type = OrbitType.CIRCULAR;
328 double[][] tolerances = NumericalPropagator.tolerances(0.1, orbit, type);
329 DormandPrince853Integrator integrator =
330 new DormandPrince853Integrator(1.0, 600, tolerances[0], tolerances[1]);
331 integrator.setInitialStepSize(60.0);
332 NumericalPropagator propagator = new NumericalPropagator(integrator);
333 propagator.addForceModel(new HolmesFeatherstoneAttractionModel(earth.getBodyFrame(), gravityField));
334 propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getSun()));
335 propagator.addForceModel(new ThirdBodyAttraction(CelestialBodyFactory.getMoon()));
336 propagator.setOrbitType(type);
337 propagator.resetInitialState(new SpacecraftState(orbit));
338 return propagator;
339 }
340
341 private SpacecraftState findFirstCrossing(final double latitude, final boolean ascending,
342 final AbsoluteDate searchStart, final AbsoluteDate end,
343 final double stepSize, final Propagator propagator) {
344
345 double previousLatitude = Double.NaN;
346 for (AbsoluteDate date = searchStart; date.compareTo(end) < 0; date = date.shiftedBy(stepSize)) {
347 final PVCoordinates pv = propagator.propagate(date).getPVCoordinates(earth.getBodyFrame());
348 final double currentLatitude = earth.transform(pv.getPosition(), earth.getBodyFrame(), date).getLatitude();
349 if (((previousLatitude <= latitude) && (currentLatitude >= latitude) && ascending) ||
350 ((previousLatitude >= latitude) && (currentLatitude <= latitude) && !ascending)) {
351 return findLatitudeCrossing(latitude, date.shiftedBy(-0.5 * stepSize), end,
352 0.5 * stepSize, 2 * stepSize, propagator);
353 }
354 previousLatitude = currentLatitude;
355 }
356
357 throw new OrekitException(LocalizedCoreFormats.SIMPLE_MESSAGE,
358 "latitude " + FastMath.toDegrees(latitude) + " never crossed");
359
360 }
361
362 private SpacecraftState findLatitudeCrossing(final double latitude,
363 final AbsoluteDate guessDate, final AbsoluteDate endDate,
364 final double shift, final double maxShift,
365 final Propagator propagator)
366 throws MathRuntimeException {
367
368
369 final UnivariateFunction latitudeFunction = new UnivariateFunction() {
370
371 public double value(double x) {
372 try {
373 final SpacecraftState state = propagator.propagate(guessDate.shiftedBy(x));
374 final Vector3D position = state.getPVCoordinates(earth.getBodyFrame()).getPosition();
375 final GeodeticPoint point = earth.transform(position, earth.getBodyFrame(), state.getDate());
376 return point.getLatitude() - latitude;
377 } catch (OrekitException oe) {
378 throw new RuntimeException(oe);
379 }
380 }
381 };
382
383
384 double span;
385 if (guessDate.shiftedBy(shift).compareTo(endDate) > 0) {
386
387 span = endDate.durationFrom(guessDate) - 1e-3;
388 } else {
389 span = shift;
390 }
391
392 while (!UnivariateSolverUtils.isBracketing(latitudeFunction, -span, span)) {
393
394 if (2 * span > maxShift) {
395
396 UnivariateSolverUtils.verifyBracketing(latitudeFunction, -span, span);
397 } else if (guessDate.shiftedBy(2 * span).compareTo(endDate) > 0) {
398
399 return null;
400 }
401
402
403 span *= 2;
404
405 }
406
407
408 final BaseUnivariateSolver<UnivariateFunction> solver =
409 new BracketingNthOrderBrentSolver(0.1, 5);
410 final double dt = solver.solve(1000, latitudeFunction, -span, span);
411 return propagator.propagate(guessDate.shiftedBy(dt));
412
413 }
414
415 private double meanSolarTime(final Orbit orbit) {
416
417
418 final Vector3D position = orbit.getPVCoordinates().getPosition();
419 final double time = orbit.getDate().getComponents(TimeScalesFactory.getUTC()).getTime().getSecondsInUTCDay();
420 final double theta = gmst.value(orbit.getDate());
421 final double sunAlpha = theta + FastMath.PI * (1 - time / (Constants.JULIAN_DAY * 0.5));
422 final double dAlpha = MathUtils.normalizeAngle(position.getAlpha() - sunAlpha, 0);
423
424
425 return 12.0 * (1.0 + dAlpha / FastMath.PI);
426
427 }
428
429
430 private static class PerfectModel {
431
432 private static final DSFactory FACTORY = new DSFactory(1, 2);
433
434 private final AbsoluteDate tRef;
435 private PolynomialFunction secularTerms;
436 private List<Harmonics> periodicTerms;
437
438 PerfectModel(final AbsoluteDate tRef) {
439 this.tRef = tRef;
440 setSecular(0.0);
441 periodicTerms = new ArrayList<>();
442 }
443
444 public void setSecular(final double...c) {
445 secularTerms = new PolynomialFunction(c);
446 }
447
448 public void addHarmonics(final double omega, final double cosCoeff, final double sinCoeff) {
449 periodicTerms.add(new Harmonics(omega, cosCoeff, sinCoeff));
450 }
451
452 public SecularAndHarmonic fittedSH(final int n, final double step) {
453 final SecularAndHarmonic sh = new SecularAndHarmonic(secularTerms.degree(),
454 periodicTerms.stream().
455 mapToDouble(h -> h.omega).
456 toArray());
457 sh.resetFitting(tRef, new double[secularTerms.degree() + 1 + 2 * periodicTerms.size()]);
458 for (int i = 0; i < n; ++i) {
459 final AbsoluteDate date = tRef.shiftedBy(i * step);
460 sh.addPoint(date, value(date));
461 }
462 sh.fit();
463 return sh;
464 }
465
466 private DerivativeStructure valueDS(final AbsoluteDate t) {
467 final DerivativeStructure x = FACTORY.variable(0, t.durationFrom(tRef));
468 DerivativeStructure y = secularTerms.value(x);
469 for (final UnivariateDifferentiableFunction f : periodicTerms) {
470 y = y.add(f.value(x));
471 }
472 return y;
473 }
474
475 public double value(final AbsoluteDate t) {
476 return valueDS(t).getValue();
477 }
478
479 public double firstDerivative(final AbsoluteDate t) {
480 return valueDS(t).getPartialDerivative(1);
481 }
482
483 public double secondDerivative(final AbsoluteDate t) {
484 return valueDS(t).getPartialDerivative(2);
485 }
486
487 private static class Harmonics implements UnivariateDifferentiableFunction {
488
489 private final double omega;
490 private final double cosCoeff;
491 private final double sinCoeff;
492
493 Harmonics(final double omega, final double cosCoeff, final double sinCoeff) {
494 this.omega = omega;
495 this.cosCoeff = cosCoeff;
496 this.sinCoeff = sinCoeff;
497 }
498
499 public double value(final double x) {
500 final SinCos sc = FastMath.sinCos(x * omega);
501 return sc.cos() * cosCoeff + sc.sin() * sinCoeff;
502 }
503
504 @Override
505 public <T extends Derivative<T>> T value(T x) throws MathIllegalArgumentException {
506 final FieldSinCos<T> sc = FastMath.sinCos(x.multiply(omega));
507 return sc.cos().multiply(cosCoeff).add(sc.sin().multiply(sinCoeff));
508 }
509
510 }
511
512 }
513
514 }