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);
- }
- }