FieldOrbitHermiteInterpolator.java
/* Copyright 2002-2024 CS GROUP
* Licensed to CS GROUP (CS) under one or more
* contributor license agreements. See the NOTICE file distributed with
* this work for additional information regarding copyright ownership.
* CS licenses this file to You under the Apache License, Version 2.0
* (the "License"); you may not use this file except in compliance with
* the License. You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
package org.orekit.orbits;
import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
import org.hipparchus.util.MathArrays;
import org.hipparchus.util.MathUtils;
import org.orekit.errors.OrekitInternalError;
import org.orekit.frames.Frame;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.FieldTimeInterpolator;
import org.orekit.utils.CartesianDerivativesFilter;
import org.orekit.utils.TimeStampedFieldPVCoordinates;
import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;
import java.util.List;
import java.util.stream.Stream;
/**
* Class using a Hermite interpolator to interpolate orbits.
* <p>
* Depending on given sample orbit type, the interpolation may differ :
* <ul>
* <li>For Keplerian, Circular and Equinoctial orbits, the interpolated instance is created by polynomial Hermite
* interpolation, using derivatives when available. </li>
* <li>For Cartesian orbits, the interpolated instance is created using the cartesian derivatives filter given at
* instance construction. Hence, it will fall back to Lagrange interpolation if this instance has been designed to not
* use derivatives.
* </ul>
* <p>
* In any case, it should be used only with small number of interpolation points (about 10-20 points) in order to avoid
* <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's phenomenon</a> and numerical problems
* (including NaN appearing).
*
* @param <KK> type of the field element
*
* @author Luc Maisonobe
* @author Vincent Cucchietti
* @see FieldOrbit
* @see FieldHermiteInterpolator
*/
public class FieldOrbitHermiteInterpolator<KK extends CalculusFieldElement<KK>> extends AbstractFieldOrbitInterpolator<KK> {
/** Filter for derivatives from the sample to use in position-velocity-acceleration interpolation. */
private final CartesianDerivativesFilter pvaFilter;
/** Field of the elements. */
private Field<KK> field;
/** Fielded zero. */
private KK zero;
/** Fielded one. */
private KK one;
/**
* Constructor with :
* <ul>
* <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
* <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
* <li>Use of position and two time derivatives during interpolation</li>
* </ul>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param outputInertialFrame output inertial frame
*/
public FieldOrbitHermiteInterpolator(final Frame outputInertialFrame) {
this(DEFAULT_INTERPOLATION_POINTS, outputInertialFrame);
}
/**
* Constructor with :
* <ul>
* <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
* <li>Use of position and two time derivatives during interpolation</li>
* </ul>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
* @param outputInertialFrame output inertial frame
*/
public FieldOrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame) {
this(interpolationPoints, outputInertialFrame, CartesianDerivativesFilter.USE_PVA);
}
/**
* Constructor with default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s).
* <p>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
* @param outputInertialFrame output inertial frame
* @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
*/
public FieldOrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame,
final CartesianDerivativesFilter pvaFilter) {
this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, outputInertialFrame, pvaFilter);
}
/**
* Constructor.
* <p>
* As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
* points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
* phenomenon</a> and numerical problems (including NaN appearing).
*
* @param interpolationPoints number of interpolation points
* @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
* @param outputInertialFrame output inertial frame
* @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
*/
public FieldOrbitHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
final Frame outputInertialFrame, final CartesianDerivativesFilter pvaFilter) {
super(interpolationPoints, extrapolationThreshold, outputInertialFrame);
this.pvaFilter = pvaFilter;
}
/** Get filter for derivatives from the sample to use in position-velocity-acceleration interpolation.
* @return filter for derivatives from the sample to use in position-velocity-acceleration interpolation
*/
public CartesianDerivativesFilter getPVAFilter() {
return pvaFilter;
}
/**
* {@inheritDoc}
* <p>
* Depending on given sample orbit type, the interpolation may differ :
* <ul>
* <li>For Keplerian, Circular and Equinoctial orbits, the interpolated instance is created by polynomial Hermite
* interpolation, using derivatives when available. </li>
* <li>For Cartesian orbits, the interpolated instance is created using the cartesian derivatives filter given at
* instance construction. Hence, it will fall back to Lagrange interpolation if this instance has been designed to not
* use derivatives.
* </ul>
* If orbit interpolation on large samples is needed, using the {@link
* org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
* low-level interpolation. The Ephemeris class automatically handles selection of
* a neighboring sub-sample with a predefined number of point from a large global sample
* in a thread-safe way.
*
* @param interpolationData interpolation data
*
* @return interpolated instance for given interpolation data
*/
@Override
protected FieldOrbit<KK> interpolate(final InterpolationData interpolationData) {
// Get interpolation date
final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();
// Get orbit sample
final List<FieldOrbit<KK>> sample = interpolationData.getNeighborList();
// Get first entry
final FieldOrbit<KK> firstEntry = sample.get(0);
// Get orbit type for interpolation
final OrbitType orbitType = firstEntry.getType();
// Extract field
this.field = firstEntry.getA().getField();
this.zero = field.getZero();
this.one = field.getOne();
if (orbitType == OrbitType.CARTESIAN) {
return interpolateCartesian(interpolationDate, sample);
}
else {
return interpolateCommon(interpolationDate, sample, orbitType);
}
}
/**
* Interpolate Cartesian orbit using specific method for Cartesian orbit.
*
* @param interpolationDate interpolation date
* @param sample orbits sample
*
* @return interpolated Cartesian orbit
*/
private FieldCartesianOrbit<KK> interpolateCartesian(final FieldAbsoluteDate<KK> interpolationDate,
final List<FieldOrbit<KK>> sample) {
// Create time stamped position-velocity-acceleration Hermite interpolator
final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<KK>, KK> interpolator =
new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(getNbInterpolationPoints(),
getExtrapolationThreshold(),
pvaFilter);
// Convert sample to stream
final Stream<FieldOrbit<KK>> sampleStream = sample.stream();
// Map time stamped position-velocity-acceleration coordinates
final Stream<TimeStampedFieldPVCoordinates<KK>> sampleTimeStampedPV = sampleStream.map(FieldOrbit::getPVCoordinates);
// Interpolate PVA
final TimeStampedFieldPVCoordinates<KK> interpolated =
interpolator.interpolate(interpolationDate, sampleTimeStampedPV);
// Use first entry gravitational parameter
final KK mu = sample.get(0).getMu();
return new FieldCartesianOrbit<>(interpolated, getOutputInertialFrame(), interpolationDate, mu);
}
/**
* Method gathering common parts of interpolation between circular, equinoctial and keplerian orbit.
*
* @param interpolationDate interpolation date
* @param orbits orbits sample
* @param orbitType interpolation method to use
*
* @return interpolated orbit
*/
private FieldOrbit<KK> interpolateCommon(final FieldAbsoluteDate<KK> interpolationDate,
final List<FieldOrbit<KK>> orbits,
final OrbitType orbitType) {
// First pass to check if derivatives are available throughout the sample
boolean useDerivatives = true;
for (final FieldOrbit<KK> orbit : orbits) {
useDerivatives = useDerivatives && orbit.hasDerivatives();
}
// Use first entry gravitational parameter
final KK mu = orbits.get(0).getMu();
// Interpolate and build a new instance
final KK[][] interpolated;
switch (orbitType) {
case CIRCULAR:
interpolated = interpolateCircular(interpolationDate, orbits, useDerivatives);
return new FieldCircularOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
interpolated[0][3], interpolated[0][4], interpolated[0][5],
interpolated[1][0], interpolated[1][1], interpolated[1][2],
interpolated[1][3], interpolated[1][4], interpolated[1][5],
PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
case KEPLERIAN:
interpolated = interpolateKeplerian(interpolationDate, orbits, useDerivatives);
return new FieldKeplerianOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
interpolated[0][3], interpolated[0][4], interpolated[0][5],
interpolated[1][0], interpolated[1][1], interpolated[1][2],
interpolated[1][3], interpolated[1][4], interpolated[1][5],
PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
case EQUINOCTIAL:
interpolated = interpolateEquinoctial(interpolationDate, orbits, useDerivatives);
return new FieldEquinoctialOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
interpolated[0][3], interpolated[0][4], interpolated[0][5],
interpolated[1][0], interpolated[1][1], interpolated[1][2],
interpolated[1][3], interpolated[1][4], interpolated[1][5],
PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
default:
// Should never happen
throw new OrekitInternalError(null);
}
}
/**
* Build interpolating functions for circular orbit parameters.
*
* @param interpolationDate interpolation date
* @param orbits orbits sample
* @param useDerivatives flag defining if derivatives are available throughout the sample
*
* @return interpolating functions for circular orbit parameters
*/
private KK[][] interpolateCircular(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
final boolean useDerivatives) {
// set up an interpolator
final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();
// second pass to feed interpolator
FieldAbsoluteDate<KK> previousDate = null;
KK previousRAAN = zero.add(Double.NaN);
KK previousAlphaM = zero.add(Double.NaN);
for (final FieldOrbit<KK> orbit : orbits) {
final FieldCircularOrbit<KK> circ = (FieldCircularOrbit<KK>) OrbitType.CIRCULAR.convertType(orbit);
final KK continuousRAAN;
final KK continuousAlphaM;
if (previousDate == null) {
continuousRAAN = circ.getRightAscensionOfAscendingNode();
continuousAlphaM = circ.getAlphaM();
}
else {
final KK dt = circ.getDate().durationFrom(previousDate);
final KK keplerAM = previousAlphaM.add(circ.getKeplerianMeanMotion().multiply(dt));
continuousRAAN = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
}
previousDate = circ.getDate();
previousRAAN = continuousRAAN;
previousAlphaM = continuousAlphaM;
final KK[] toAdd = MathArrays.buildArray(one.getField(), 6);
toAdd[0] = circ.getA();
toAdd[1] = circ.getCircularEx();
toAdd[2] = circ.getCircularEy();
toAdd[3] = circ.getI();
toAdd[4] = continuousRAAN;
toAdd[5] = continuousAlphaM;
if (useDerivatives) {
final KK[] toAddDot = MathArrays.buildArray(one.getField(), 6);
toAddDot[0] = circ.getADot();
toAddDot[1] = circ.getCircularExDot();
toAddDot[2] = circ.getCircularEyDot();
toAddDot[3] = circ.getIDot();
toAddDot[4] = circ.getRightAscensionOfAscendingNodeDot();
toAddDot[5] = circ.getAlphaMDot();
interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
toAdd, toAddDot);
}
else {
interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
toAdd);
}
}
return interpolator.derivatives(zero, 1);
}
/**
* Build interpolating functions for keplerian orbit parameters.
*
* @param interpolationDate interpolation date
* @param orbits orbits sample
* @param useDerivatives flag defining if derivatives are available throughout the sample
*
* @return interpolating functions for keplerian orbit parameters
*/
private KK[][] interpolateKeplerian(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
final boolean useDerivatives) {
// Set up an interpolator
final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();
// Second pass to feed interpolator
FieldAbsoluteDate<KK> previousDate = null;
KK previousPA = zero.add(Double.NaN);
KK previousRAAN = zero.add(Double.NaN);
KK previousM = zero.add(Double.NaN);
for (final FieldOrbit<KK> orbit : orbits) {
final FieldKeplerianOrbit<KK> kep = (FieldKeplerianOrbit<KK>) OrbitType.KEPLERIAN.convertType(orbit);
final KK continuousPA;
final KK continuousRAAN;
final KK continuousM;
if (previousDate == null) {
continuousPA = kep.getPerigeeArgument();
continuousRAAN = kep.getRightAscensionOfAscendingNode();
continuousM = kep.getMeanAnomaly();
}
else {
final KK dt = kep.getDate().durationFrom(previousDate);
final KK keplerM = previousM.add(kep.getKeplerianMeanMotion().multiply(dt));
continuousPA = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
continuousM = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
}
previousDate = kep.getDate();
previousPA = continuousPA;
previousRAAN = continuousRAAN;
previousM = continuousM;
final KK[] toAdd = MathArrays.buildArray(field, 6);
toAdd[0] = kep.getA();
toAdd[1] = kep.getE();
toAdd[2] = kep.getI();
toAdd[3] = continuousPA;
toAdd[4] = continuousRAAN;
toAdd[5] = continuousM;
if (useDerivatives) {
final KK[] toAddDot = MathArrays.buildArray(field, 6);
toAddDot[0] = kep.getADot();
toAddDot[1] = kep.getEDot();
toAddDot[2] = kep.getIDot();
toAddDot[3] = kep.getPerigeeArgumentDot();
toAddDot[4] = kep.getRightAscensionOfAscendingNodeDot();
toAddDot[5] = kep.getMeanAnomalyDot();
interpolator.addSamplePoint(kep.getDate().durationFrom(interpolationDate),
toAdd, toAddDot);
}
else {
interpolator.addSamplePoint(this.zero.add(kep.getDate().durationFrom(interpolationDate)),
toAdd);
}
}
return interpolator.derivatives(zero, 1);
}
/**
* Build interpolating functions for equinoctial orbit parameters.
*
* @param interpolationDate interpolation date
* @param orbits orbits sample
* @param useDerivatives flag defining if derivatives are available throughout the sample
*
* @return interpolating functions for equinoctial orbit parameters
*/
private KK[][] interpolateEquinoctial(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
final boolean useDerivatives) {
// set up an interpolator
final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();
// second pass to feed interpolator
FieldAbsoluteDate<KK> previousDate = null;
KK previousLm = zero.add(Double.NaN);
for (final FieldOrbit<KK> orbit : orbits) {
final FieldEquinoctialOrbit<KK> equi = (FieldEquinoctialOrbit<KK>) OrbitType.EQUINOCTIAL.convertType(orbit);
final KK continuousLm;
if (previousDate == null) {
continuousLm = equi.getLM();
}
else {
final KK dt = equi.getDate().durationFrom(previousDate);
final KK keplerLm = previousLm.add(equi.getKeplerianMeanMotion().multiply(dt));
continuousLm = MathUtils.normalizeAngle(equi.getLM(), keplerLm);
}
previousDate = equi.getDate();
previousLm = continuousLm;
final KK[] toAdd = MathArrays.buildArray(field, 6);
toAdd[0] = equi.getA();
toAdd[1] = equi.getEquinoctialEx();
toAdd[2] = equi.getEquinoctialEy();
toAdd[3] = equi.getHx();
toAdd[4] = equi.getHy();
toAdd[5] = continuousLm;
if (useDerivatives) {
final KK[] toAddDot = MathArrays.buildArray(one.getField(), 6);
toAddDot[0] = equi.getADot();
toAddDot[1] = equi.getEquinoctialExDot();
toAddDot[2] = equi.getEquinoctialEyDot();
toAddDot[3] = equi.getHxDot();
toAddDot[4] = equi.getHyDot();
toAddDot[5] = equi.getLMDot();
interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
toAdd, toAddDot);
}
else {
interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
toAdd);
}
}
return interpolator.derivatives(zero, 1);
}
}