OrbitHermiteInterpolator.java

  1. /* Copyright 2002-2024 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;

  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;

  29. import java.util.List;
  30. import java.util.stream.Stream;

  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 {

  55.     /** Filter for derivatives from the sample to use in position-velocity-acceleration interpolation. */
  56.     private final CartesianDerivativesFilter pvaFilter;

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

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

  161.         // Get orbit type for interpolation
  162.         final OrbitType orbitType = sample.get(0).getType();

  164.         if (orbitType == OrbitType.CARTESIAN) {
  165.             return interpolateCartesian(interpolationData.getInterpolationDate(), sample);
  166.         }
  167.         else {
  168.             return interpolateCommon(interpolationData.getInterpolationDate(), sample, orbitType);
  169.         }

  171.     }

  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) {

  183.         // Create time stamped position-velocity-acceleration Hermite interpolator
  184.         final TimeInterpolator<TimeStampedPVCoordinates> interpolator =
  185.                 new TimeStampedPVCoordinatesHermiteInterpolator(getNbInterpolationPoints(), getExtrapolationThreshold(),
  186.                                                                 pvaFilter);

  188.         // Convert sample to stream
  189.         final Stream<Orbit> sampleStream = sample.stream();

  191.         // Map time stamped position-velocity-acceleration coordinates
  192.         final Stream<TimeStampedPVCoordinates> sampleTimeStampedPV = sampleStream.map(Orbit::getPVCoordinates);

  194.         // Interpolate PVA
  195.         final TimeStampedPVCoordinates interpolated = interpolator.interpolate(interpolationDate, sampleTimeStampedPV);

  197.         // Use first entry gravitational parameter
  198.         final double mu = sample.get(0).getMu();

  200.         return new CartesianOrbit(interpolated, getOutputInertialFrame(), interpolationDate, mu);
  201.     }

  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) {

  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.         }

  221.         // Use first entry gravitational parameter
  222.         final double mu = orbits.get(0).getMu();

  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.         }

  253.     }

  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) {

  267.         // Set up an interpolator
  268.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  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.         }

  317.         return interpolator.derivatives(0.0, 1);
  318.     }

  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) {

  332.         // Set up an interpolator
  333.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  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.         }

  387.         return interpolator.derivatives(0.0, 1);
  388.     }

  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) {

  402.         // Set up an interpolator
  403.         final HermiteInterpolator interpolator = new HermiteInterpolator();

  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.         }

  448.         return interpolator.derivatives(0.0, 1);
  449.     }
  450. }
Created with JaCoCo 0.8.12.202403310830