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