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