1 /* Copyright 2002-2023 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.orbits;
18
19 import org.hipparchus.analysis.interpolation.HermiteInterpolator;
20 import org.hipparchus.util.MathUtils;
21 import org.orekit.errors.OrekitInternalError;
22 import org.orekit.frames.Frame;
23 import org.orekit.time.AbsoluteDate;
24 import org.orekit.time.TimeInterpolator;
25 import org.orekit.utils.CartesianDerivativesFilter;
26 import org.orekit.utils.TimeStampedPVCoordinates;
27 import org.orekit.utils.TimeStampedPVCoordinatesHermiteInterpolator;
28
29 import java.util.List;
30 import java.util.stream.Stream;
31
32 /**
33 * Class using a Hermite interpolator to interpolate orbits.
34 * <p>
35 * Depending on given sample orbit type, the interpolation may differ :
36 * <ul>
37 * <li>For Keplerian, Circular and Equinoctial orbits, the interpolated instance is created by polynomial Hermite
38 * interpolation, using derivatives when available. </li>
39 * <li>For Cartesian orbits, the interpolated instance is created using the cartesian derivatives filter given at
40 * instance construction. Hence, it will fall back to Lagrange interpolation if this instance has been designed to not
41 * use derivatives.
42 * </ul>
43 * <p>
44 * In any case, it should be used only with small number of interpolation points (about 10-20 points) in order to avoid
45 * <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a> and numerical problems
46 * (including NaN appearing).
47 *
48 * @author Luc Maisonobe
49 * @author Vincent Cucchietti
50 * @see Orbit
51 * @see HermiteInterpolator
52 */
53 public class OrbitHermiteInterpolator extends AbstractOrbitInterpolator {
54
55 /** Filter for derivatives from the sample to use in position-velocity-acceleration interpolation. */
56 private final CartesianDerivativesFilter pvaFilter;
57
58 /**
59 * Constructor with :
60 * <ul>
61 * <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
62 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
63 * <li>Use of position and two time derivatives during interpolation</li>
64 * </ul>
65 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
66 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
67 * phenomenon</a> and numerical problems (including NaN appearing).
68 *
69 * @param outputInertialFrame output inertial frame
70 */
71 public OrbitHermiteInterpolator(final Frame outputInertialFrame) {
72 this(DEFAULT_INTERPOLATION_POINTS, outputInertialFrame);
73 }
74
75 /**
76 * Constructor with :
77 * <ul>
78 * <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
79 * <li>Use of position and two time derivatives during interpolation</li>
80 * </ul>
81 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
82 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
83 * phenomenon</a> and numerical problems (including NaN appearing).
84 *
85 * @param interpolationPoints number of interpolation points
86 * @param outputInertialFrame output inertial frame
87 */
88 public OrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame) {
89 this(interpolationPoints, outputInertialFrame, CartesianDerivativesFilter.USE_PVA);
90 }
91
92 /**
93 * Constructor with default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s).
94 * <p>
95 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
96 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
97 * phenomenon</a> and numerical problems (including NaN appearing).
98 *
99 * @param interpolationPoints number of interpolation points
100 * @param outputInertialFrame output inertial frame
101 * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation. Used
102 * only when interpolating Cartesian orbits.
103 */
104 public OrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame,
105 final CartesianDerivativesFilter pvaFilter) {
106 this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, outputInertialFrame, pvaFilter);
107 }
108
109 /**
110 * Constructor.
111 * <p>
112 * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
113 * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
114 * phenomenon</a> and numerical problems (including NaN appearing).
115 *
116 * @param interpolationPoints number of interpolation points
117 * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
118 * @param outputInertialFrame output inertial frame
119 * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation. Used
120 * only when interpolating Cartesian orbits.
121 */
122 public OrbitHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
123 final Frame outputInertialFrame, final CartesianDerivativesFilter pvaFilter) {
124 super(interpolationPoints, extrapolationThreshold, outputInertialFrame);
125 this.pvaFilter = pvaFilter;
126 }
127
128 /** Get filter for derivatives from the sample to use in position-velocity-acceleration interpolation.
129 * @return filter for derivatives from the sample to use in position-velocity-acceleration interpolation
130 */
131 public CartesianDerivativesFilter getPVAFilter() {
132 return pvaFilter;
133 }
134
135 /**
136 * {@inheritDoc}
137 * <p>
138 * Depending on given sample orbit type, the interpolation may differ :
139 * <ul>
140 * <li>For Keplerian, Circular and Equinoctial orbits, the interpolated instance is created by polynomial Hermite
141 * interpolation, using derivatives when available. </li>
142 * <li>For Cartesian orbits, the interpolated instance is created using the cartesian derivatives filter given at
143 * instance construction. Hence, it will fall back to Lagrange interpolation if this instance has been designed to not
144 * use derivatives.
145 * </ul>
146 * If orbit interpolation on large samples is needed, using the {@link
147 * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
148 * low-level interpolation. The Ephemeris class automatically handles selection of
149 * a neighboring sub-sample with a predefined number of point from a large global sample
150 * in a thread-safe way.
151 *
152 * @param interpolationData interpolation data
153 *
154 * @return interpolated instance at given date
155 */
156 @Override
157 protected Orbit interpolate(final InterpolationData interpolationData) {
158 // Get orbit sample
159 final List<Orbit> sample = interpolationData.getNeighborList();
160
161 // Get orbit type for interpolation
162 final OrbitType orbitType = sample.get(0).getType();
163
164 if (orbitType == OrbitType.CARTESIAN) {
165 return interpolateCartesian(interpolationData.getInterpolationDate(), sample);
166 }
167 else {
168 return interpolateCommon(interpolationData.getInterpolationDate(), sample, orbitType);
169 }
170
171 }
172
173 /**
174 * Interpolate Cartesian orbit using specific method for Cartesian orbit.
175 *
176 * @param interpolationDate interpolation date
177 * @param sample orbits sample
178 *
179 * @return interpolated Cartesian orbit
180 */
181 private CartesianOrbit interpolateCartesian(final AbsoluteDate interpolationDate, final List<Orbit> sample) {
182
183 // Create time stamped position-velocity-acceleration Hermite interpolator
184 final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
185 new TimeStampedPVCoordinatesHermiteInterpolator(getNbInterpolationPoints(), getExtrapolationThreshold(),
186 pvaFilter);
187
188 // Convert sample to stream
189 final Stream<Orbit> sampleStream = sample.stream();
190
191 // Map time stamped position-velocity-acceleration coordinates
192 final Stream<TimeStampedPVCoordinates> sampleTimeStampedPV = sampleStream.map(Orbit::getPVCoordinates);
193
194 // Interpolate PVA
195 final TimeStampedPVCoordinates interpolated = interpolator.interpolate(interpolationDate, sampleTimeStampedPV);
196
197 // Use first entry gravitational parameter
198 final double mu = sample.get(0).getMu();
199
200 return new CartesianOrbit(interpolated, getOutputInertialFrame(), interpolationDate, mu);
201 }
202
203 /**
204 * Method gathering common parts of interpolation between circular, equinoctial and keplerian orbit.
205 *
206 * @param interpolationDate interpolation date
207 * @param orbits orbits sample
208 * @param orbitType interpolation method to use
209 *
210 * @return interpolated orbit
211 */
212 private Orbit interpolateCommon(final AbsoluteDate interpolationDate, final List<Orbit> orbits,
213 final OrbitType orbitType) {
214
215 // First pass to check if derivatives are available throughout the sample
216 boolean useDerivatives = true;
217 for (final Orbit orbit : orbits) {
218 useDerivatives = useDerivatives && orbit.hasDerivatives();
219 }
220
221 // Use first entry gravitational parameter
222 final double mu = orbits.get(0).getMu();
223
224 // Interpolate and build a new instance
225 final double[][] interpolated;
226 switch (orbitType) {
227 case CIRCULAR:
228 interpolated = interpolateCircular(interpolationDate, orbits, useDerivatives);
229 return new CircularOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
230 interpolated[0][3], interpolated[0][4], interpolated[0][5],
231 interpolated[1][0], interpolated[1][1], interpolated[1][2],
232 interpolated[1][3], interpolated[1][4], interpolated[1][5],
233 PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
234 case KEPLERIAN:
235 interpolated = interpolateKeplerian(interpolationDate, orbits, useDerivatives);
236 return new KeplerianOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
237 interpolated[0][3], interpolated[0][4], interpolated[0][5],
238 interpolated[1][0], interpolated[1][1], interpolated[1][2],
239 interpolated[1][3], interpolated[1][4], interpolated[1][5],
240 PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
241 case EQUINOCTIAL:
242 interpolated = interpolateEquinoctial(interpolationDate, orbits, useDerivatives);
243 return new EquinoctialOrbit(interpolated[0][0], interpolated[0][1], interpolated[0][2],
244 interpolated[0][3], interpolated[0][4], interpolated[0][5],
245 interpolated[1][0], interpolated[1][1], interpolated[1][2],
246 interpolated[1][3], interpolated[1][4], interpolated[1][5],
247 PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
248 default:
249 // Should never happen
250 throw new OrekitInternalError(null);
251 }
252
253 }
254
255 /**
256 * Build interpolating functions for circular orbit parameters.
257 *
258 * @param interpolationDate interpolation date
259 * @param orbits orbits sample
260 * @param useDerivatives flag defining if derivatives are available throughout the sample
261 *
262 * @return interpolating functions for circular orbit parameters
263 */
264 private double[][] interpolateCircular(final AbsoluteDate interpolationDate, final List<Orbit> orbits,
265 final boolean useDerivatives) {
266
267 // Set up an interpolator
268 final HermiteInterpolator interpolator = new HermiteInterpolator();
269
270 // Second pass to feed interpolator
271 AbsoluteDate previousDate = null;
272 double previousRAAN = Double.NaN;
273 double previousAlphaM = Double.NaN;
274 for (final Orbit orbit : orbits) {
275 final CircularOrbit circ = (CircularOrbit) OrbitType.CIRCULAR.convertType(orbit);
276 final double continuousRAAN;
277 final double continuousAlphaM;
278 if (previousDate == null) {
279 continuousRAAN = circ.getRightAscensionOfAscendingNode();
280 continuousAlphaM = circ.getAlphaM();
281 }
282 else {
283 final double dt = circ.getDate().durationFrom(previousDate);
284 final double keplerAM = previousAlphaM + circ.getKeplerianMeanMotion() * dt;
285 continuousRAAN = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
286 continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
287 }
288 previousDate = circ.getDate();
289 previousRAAN = continuousRAAN;
290 previousAlphaM = continuousAlphaM;
291 if (useDerivatives) {
292 interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
293 new double[] { circ.getA(),
294 circ.getCircularEx(),
295 circ.getCircularEy(),
296 circ.getI(),
297 continuousRAAN,
298 continuousAlphaM },
299 new double[] { circ.getADot(),
300 circ.getCircularExDot(),
301 circ.getCircularEyDot(),
302 circ.getIDot(),
303 circ.getRightAscensionOfAscendingNodeDot(),
304 circ.getAlphaMDot() });
305 }
306 else {
307 interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
308 new double[] { circ.getA(),
309 circ.getCircularEx(),
310 circ.getCircularEy(),
311 circ.getI(),
312 continuousRAAN,
313 continuousAlphaM });
314 }
315 }
316
317 return interpolator.derivatives(0.0, 1);
318 }
319
320 /**
321 * Build interpolating functions for keplerian orbit parameters.
322 *
323 * @param interpolationDate interpolation date
324 * @param orbits orbits sample
325 * @param useDerivatives flag defining if derivatives are available throughout the sample
326 *
327 * @return interpolating functions for keplerian orbit parameters
328 */
329 private double[][] interpolateKeplerian(final AbsoluteDate interpolationDate, final List<Orbit> orbits,
330 final boolean useDerivatives) {
331
332 // Set up an interpolator
333 final HermiteInterpolator interpolator = new HermiteInterpolator();
334
335 // Second pass to feed interpolator
336 AbsoluteDate previousDate = null;
337 double previousPA = Double.NaN;
338 double previousRAAN = Double.NaN;
339 double previousM = Double.NaN;
340 for (final Orbit orbit : orbits) {
341 final KeplerianOrbit kep = (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);
342 final double continuousPA;
343 final double continuousRAAN;
344 final double continuousM;
345 if (previousDate == null) {
346 continuousPA = kep.getPerigeeArgument();
347 continuousRAAN = kep.getRightAscensionOfAscendingNode();
348 continuousM = kep.getMeanAnomaly();
349 }
350 else {
351 final double dt = kep.getDate().durationFrom(previousDate);
352 final double keplerM = previousM + kep.getKeplerianMeanMotion() * dt;
353 continuousPA = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
354 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
355 continuousM = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
356 }
357 previousDate = kep.getDate();
358 previousPA = continuousPA;
359 previousRAAN = continuousRAAN;
360 previousM = continuousM;
361 if (useDerivatives) {
362 interpolator.addSamplePoint(kep.getDate().durationFrom(interpolationDate),
363 new double[] { kep.getA(),
364 kep.getE(),
365 kep.getI(),
366 continuousPA,
367 continuousRAAN,
368 continuousM },
369 new double[] { kep.getADot(),
370 kep.getEDot(),
371 kep.getIDot(),
372 kep.getPerigeeArgumentDot(),
373 kep.getRightAscensionOfAscendingNodeDot(),
374 kep.getMeanAnomalyDot() });
375 }
376 else {
377 interpolator.addSamplePoint(kep.getDate().durationFrom(interpolationDate),
378 new double[] { kep.getA(),
379 kep.getE(),
380 kep.getI(),
381 continuousPA,
382 continuousRAAN,
383 continuousM });
384 }
385 }
386
387 return interpolator.derivatives(0.0, 1);
388 }
389
390 /**
391 * Build interpolating functions for equinoctial orbit parameters.
392 *
393 * @param interpolationDate interpolation date
394 * @param orbits orbits sample
395 * @param useDerivatives flag defining if derivatives are available throughout the sample
396 *
397 * @return interpolating functions for equinoctial orbit parameters
398 */
399 private double[][] interpolateEquinoctial(final AbsoluteDate interpolationDate, final List<Orbit> orbits,
400 final boolean useDerivatives) {
401
402 // Set up an interpolator
403 final HermiteInterpolator interpolator = new HermiteInterpolator();
404
405 // Second pass to feed interpolator
406 AbsoluteDate previousDate = null;
407 double previousLm = Double.NaN;
408 for (final Orbit orbit : orbits) {
409 final EquinoctialOrbit equi = (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);
410 final double continuousLm;
411 if (previousDate == null) {
412 continuousLm = equi.getLM();
413 }
414 else {
415 final double dt = equi.getDate().durationFrom(previousDate);
416 final double keplerLm = previousLm + equi.getKeplerianMeanMotion() * dt;
417 continuousLm = MathUtils.normalizeAngle(equi.getLM(), keplerLm);
418 }
419 previousDate = equi.getDate();
420 previousLm = continuousLm;
421 if (useDerivatives) {
422 interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
423 new double[] { equi.getA(),
424 equi.getEquinoctialEx(),
425 equi.getEquinoctialEy(),
426 equi.getHx(),
427 equi.getHy(),
428 continuousLm },
429 new double[] {
430 equi.getADot(),
431 equi.getEquinoctialExDot(),
432 equi.getEquinoctialEyDot(),
433 equi.getHxDot(),
434 equi.getHyDot(),
435 equi.getLMDot() });
436 }
437 else {
438 interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
439 new double[] { equi.getA(),
440 equi.getEquinoctialEx(),
441 equi.getEquinoctialEy(),
442 equi.getHx(),
443 equi.getHy(),
444 continuousLm });
445 }
446 }
447
448 return interpolator.derivatives(0.0, 1);
449 }
450 }