FieldOrbitHermiteInterpolator.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;

  18. import org.hipparchus.CalculusFieldElement;
  19. import org.hipparchus.Field;
  20. import org.hipparchus.analysis.interpolation.FieldHermiteInterpolator;
  21. import org.hipparchus.util.MathArrays;
  22. import org.hipparchus.util.MathUtils;
  23. import org.orekit.errors.OrekitInternalError;
  24. import org.orekit.frames.Frame;
  25. import org.orekit.time.FieldAbsoluteDate;
  26. import org.orekit.time.FieldTimeInterpolator;
  27. import org.orekit.utils.CartesianDerivativesFilter;
  28. import org.orekit.utils.TimeStampedFieldPVCoordinates;
  29. import org.orekit.utils.TimeStampedFieldPVCoordinatesHermiteInterpolator;

  30. import java.util.List;
  31. 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.  * @param <KK> type of the field element
  49.  *
  50.  * @author Luc Maisonobe
  51.  * @author Vincent Cucchietti
  52.  * @see FieldOrbit
  53.  * @see FieldHermiteInterpolator
  54.  */
  55. public class FieldOrbitHermiteInterpolator<KK extends CalculusFieldElement<KK>> extends AbstractFieldOrbitInterpolator<KK> {

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

  58.     /** Field of the elements. */
  59.     private Field<KK> field;

  60.     /** Fielded zero. */
  61.     private KK zero;

  62.     /** Fielded one. */
  63.     private KK one;

  64.     /**
  65.      * Constructor with :
  66.      * <ul>
  67.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  68.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  69.      *     <li>Use of position and two time derivatives during interpolation</li>
  70.      * </ul>
  71.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  72.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  73.      * phenomenon</a> and numerical problems (including NaN appearing).
  74.      *
  75.      * @param outputInertialFrame output inertial frame
  76.      */
  77.     public FieldOrbitHermiteInterpolator(final Frame outputInertialFrame) {
  78.         this(DEFAULT_INTERPOLATION_POINTS, outputInertialFrame);
  79.     }

  80.     /**
  81.      * Constructor with :
  82.      * <ul>
  83.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  84.      *     <li>Use of position and two time derivatives during interpolation</li>
  85.      * </ul>
  86.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  87.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  88.      * phenomenon</a> and numerical problems (including NaN appearing).
  89.      *
  90.      * @param interpolationPoints number of interpolation points
  91.      * @param outputInertialFrame output inertial frame
  92.      */
  93.     public FieldOrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame) {
  94.         this(interpolationPoints, outputInertialFrame, CartesianDerivativesFilter.USE_PVA);
  95.     }

  96.     /**
  97.      * Constructor with default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s).
  98.      * <p>
  99.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  100.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  101.      * phenomenon</a> and numerical problems (including NaN appearing).
  102.      *
  103.      * @param interpolationPoints number of interpolation points
  104.      * @param outputInertialFrame output inertial frame
  105.      * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
  106.      */
  107.     public FieldOrbitHermiteInterpolator(final int interpolationPoints, final Frame outputInertialFrame,
  108.                                          final CartesianDerivativesFilter pvaFilter) {
  109.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, outputInertialFrame, pvaFilter);
  110.     }

  111.     /**
  112.      * Constructor.
  113.      * <p>
  114.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  115.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  116.      * phenomenon</a> and numerical problems (including NaN appearing).
  117.      *
  118.      * @param interpolationPoints number of interpolation points
  119.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  120.      * @param outputInertialFrame output inertial frame
  121.      * @param pvaFilter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
  122.      */
  123.     public FieldOrbitHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  124.                                          final Frame outputInertialFrame, final CartesianDerivativesFilter pvaFilter) {
  125.         super(interpolationPoints, extrapolationThreshold, outputInertialFrame);
  126.         this.pvaFilter = pvaFilter;
  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.      * {@inheritDoc}
  136.      * <p>
  137.      * Depending on given sample orbit type, the interpolation may differ :
  138.      * <ul>
  139.      *    <li>For Keplerian, Circular and Equinoctial orbits, the interpolated instance is created by polynomial Hermite
  140.      *    interpolation, using derivatives when available. </li>
  141.      *    <li>For Cartesian orbits, the interpolated instance is created using the cartesian derivatives filter given at
  142.      *    instance construction. Hence, it will fall back to Lagrange interpolation if this instance has been designed to not
  143.      *    use derivatives.
  144.      * </ul>
  145.      * If orbit interpolation on large samples is needed, using the {@link
  146.      * org.orekit.propagation.analytical.Ephemeris} class is a better way than using this
  147.      * low-level interpolation. The Ephemeris class automatically handles selection of
  148.      * a neighboring sub-sample with a predefined number of point from a large global sample
  149.      * in a thread-safe way.
  150.      *
  151.      * @param interpolationData interpolation data
  152.      *
  153.      * @return interpolated instance for given interpolation data
  154.      */
  155.     @Override
  156.     protected FieldOrbit<KK> interpolate(final InterpolationData interpolationData) {

  157.         // Get interpolation date
  158.         final FieldAbsoluteDate<KK> interpolationDate = interpolationData.getInterpolationDate();

  159.         // Get orbit sample
  160.         final List<FieldOrbit<KK>> sample = interpolationData.getNeighborList();

  161.         // Get first entry
  162.         final FieldOrbit<KK> firstEntry = sample.get(0);

  163.         // Get orbit type for interpolation
  164.         final OrbitType orbitType = firstEntry.getType();

  165.         // Extract field
  166.         this.field = firstEntry.getA().getField();
  167.         this.zero  = field.getZero();
  168.         this.one   = field.getOne();

  169.         if (orbitType == OrbitType.CARTESIAN) {
  170.             return interpolateCartesian(interpolationDate, sample);
  171.         }
  172.         else {
  173.             return interpolateCommon(interpolationDate, sample, orbitType);
  174.         }

  175.     }

  176.     /**
  177.      * Interpolate Cartesian orbit using specific method for Cartesian orbit.
  178.      *
  179.      * @param interpolationDate interpolation date
  180.      * @param sample orbits sample
  181.      *
  182.      * @return interpolated Cartesian orbit
  183.      */
  184.     private FieldCartesianOrbit<KK> interpolateCartesian(final FieldAbsoluteDate<KK> interpolationDate,
  185.                                                          final List<FieldOrbit<KK>> sample) {

  186.         // Create time stamped position-velocity-acceleration Hermite interpolator
  187.         final FieldTimeInterpolator<TimeStampedFieldPVCoordinates<KK>, KK> interpolator =
  188.                 new TimeStampedFieldPVCoordinatesHermiteInterpolator<>(getNbInterpolationPoints(),
  189.                                                                        getExtrapolationThreshold(),
  190.                                                                        pvaFilter);

  191.         // Convert sample to stream
  192.         final Stream<FieldOrbit<KK>> sampleStream = sample.stream();

  193.         // Map time stamped position-velocity-acceleration coordinates
  194.         final Stream<TimeStampedFieldPVCoordinates<KK>> sampleTimeStampedPV = sampleStream.map(FieldOrbit::getPVCoordinates);

  195.         // Interpolate PVA
  196.         final TimeStampedFieldPVCoordinates<KK> interpolated =
  197.                 interpolator.interpolate(interpolationDate, sampleTimeStampedPV);

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

  200.         return new FieldCartesianOrbit<>(interpolated, getOutputInertialFrame(), interpolationDate, mu);
  201.     }

  202.     /**
  203.      * Method gathering common parts of interpolation between circular, equinoctial and keplerian orbit.
  204.      *
  205.      * @param interpolationDate interpolation date
  206.      * @param orbits orbits sample
  207.      * @param orbitType interpolation method to use
  208.      *
  209.      * @return interpolated orbit
  210.      */
  211.     private FieldOrbit<KK> interpolateCommon(final FieldAbsoluteDate<KK> interpolationDate,
  212.                                              final List<FieldOrbit<KK>> orbits,
  213.                                              final OrbitType orbitType) {

  214.         // First pass to check if derivatives are available throughout the sample
  215.         boolean useDerivatives = true;
  216.         for (final FieldOrbit<KK> orbit : orbits) {
  217.             useDerivatives = useDerivatives && orbit.hasDerivatives();
  218.         }

  219.         // Use first entry gravitational parameter
  220.         final KK mu = orbits.get(0).getMu();

  221.         // Interpolate and build a new instance
  222.         final KK[][] interpolated;
  223.         switch (orbitType) {
  224.             case CIRCULAR:
  225.                 interpolated = interpolateCircular(interpolationDate, orbits, useDerivatives);
  226.                 return new FieldCircularOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  227.                                                 interpolated[0][3], interpolated[0][4], interpolated[0][5],
  228.                                                 interpolated[1][0], interpolated[1][1], interpolated[1][2],
  229.                                                 interpolated[1][3], interpolated[1][4], interpolated[1][5],
  230.                                                 PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
  231.             case KEPLERIAN:
  232.                 interpolated = interpolateKeplerian(interpolationDate, orbits, useDerivatives);
  233.                 return new FieldKeplerianOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  234.                                                  interpolated[0][3], interpolated[0][4], interpolated[0][5],
  235.                                                  interpolated[1][0], interpolated[1][1], interpolated[1][2],
  236.                                                  interpolated[1][3], interpolated[1][4], interpolated[1][5],
  237.                                                  PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
  238.             case EQUINOCTIAL:
  239.                 interpolated = interpolateEquinoctial(interpolationDate, orbits, useDerivatives);
  240.                 return new FieldEquinoctialOrbit<>(interpolated[0][0], interpolated[0][1], interpolated[0][2],
  241.                                                    interpolated[0][3], interpolated[0][4], interpolated[0][5],
  242.                                                    interpolated[1][0], interpolated[1][1], interpolated[1][2],
  243.                                                    interpolated[1][3], interpolated[1][4], interpolated[1][5],
  244.                                                    PositionAngleType.MEAN, getOutputInertialFrame(), interpolationDate, mu);
  245.             default:
  246.                 // Should never happen
  247.                 throw new OrekitInternalError(null);
  248.         }

  249.     }

  250.     /**
  251.      * Build interpolating functions for circular orbit parameters.
  252.      *
  253.      * @param interpolationDate interpolation date
  254.      * @param orbits orbits sample
  255.      * @param useDerivatives flag defining if derivatives are available throughout the sample
  256.      *
  257.      * @return interpolating functions for circular orbit parameters
  258.      */
  259.     private KK[][] interpolateCircular(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
  260.                                        final boolean useDerivatives) {

  261.         // set up an interpolator
  262.         final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();

  263.         // second pass to feed interpolator
  264.         FieldAbsoluteDate<KK> previousDate   = null;
  265.         KK                    previousRAAN   = zero.add(Double.NaN);
  266.         KK                    previousAlphaM = zero.add(Double.NaN);
  267.         for (final FieldOrbit<KK> orbit : orbits) {
  268.             final FieldCircularOrbit<KK> circ = (FieldCircularOrbit<KK>) OrbitType.CIRCULAR.convertType(orbit);
  269.             final KK                     continuousRAAN;
  270.             final KK                     continuousAlphaM;
  271.             if (previousDate == null) {
  272.                 continuousRAAN   = circ.getRightAscensionOfAscendingNode();
  273.                 continuousAlphaM = circ.getAlphaM();
  274.             }
  275.             else {
  276.                 final KK dt       = circ.getDate().durationFrom(previousDate);
  277.                 final KK keplerAM = previousAlphaM.add(circ.getKeplerianMeanMotion().multiply(dt));
  278.                 continuousRAAN   = MathUtils.normalizeAngle(circ.getRightAscensionOfAscendingNode(), previousRAAN);
  279.                 continuousAlphaM = MathUtils.normalizeAngle(circ.getAlphaM(), keplerAM);
  280.             }
  281.             previousDate   = circ.getDate();
  282.             previousRAAN   = continuousRAAN;
  283.             previousAlphaM = continuousAlphaM;
  284.             final KK[] toAdd = MathArrays.buildArray(one.getField(), 6);
  285.             toAdd[0] = circ.getA();
  286.             toAdd[1] = circ.getCircularEx();
  287.             toAdd[2] = circ.getCircularEy();
  288.             toAdd[3] = circ.getI();
  289.             toAdd[4] = continuousRAAN;
  290.             toAdd[5] = continuousAlphaM;
  291.             if (useDerivatives) {
  292.                 final KK[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  293.                 toAddDot[0] = circ.getADot();
  294.                 toAddDot[1] = circ.getCircularExDot();
  295.                 toAddDot[2] = circ.getCircularEyDot();
  296.                 toAddDot[3] = circ.getIDot();
  297.                 toAddDot[4] = circ.getRightAscensionOfAscendingNodeDot();
  298.                 toAddDot[5] = circ.getAlphaMDot();
  299.                 interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
  300.                                             toAdd, toAddDot);
  301.             }
  302.             else {
  303.                 interpolator.addSamplePoint(circ.getDate().durationFrom(interpolationDate),
  304.                                             toAdd);
  305.             }
  306.         }

  307.         return interpolator.derivatives(zero, 1);
  308.     }

  309.     /**
  310.      * Build interpolating functions for keplerian orbit parameters.
  311.      *
  312.      * @param interpolationDate interpolation date
  313.      * @param orbits orbits sample
  314.      * @param useDerivatives flag defining if derivatives are available throughout the sample
  315.      *
  316.      * @return interpolating functions for keplerian orbit parameters
  317.      */
  318.     private KK[][] interpolateKeplerian(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
  319.                                         final boolean useDerivatives) {

  320.         // Set up an interpolator
  321.         final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();

  322.         // Second pass to feed interpolator
  323.         FieldAbsoluteDate<KK> previousDate = null;
  324.         KK                    previousPA   = zero.add(Double.NaN);
  325.         KK                    previousRAAN = zero.add(Double.NaN);
  326.         KK                    previousM    = zero.add(Double.NaN);
  327.         for (final FieldOrbit<KK> orbit : orbits) {
  328.             final FieldKeplerianOrbit<KK> kep = (FieldKeplerianOrbit<KK>) OrbitType.KEPLERIAN.convertType(orbit);
  329.             final KK                      continuousPA;
  330.             final KK                      continuousRAAN;
  331.             final KK                      continuousM;
  332.             if (previousDate == null) {
  333.                 continuousPA   = kep.getPerigeeArgument();
  334.                 continuousRAAN = kep.getRightAscensionOfAscendingNode();
  335.                 continuousM    = kep.getMeanAnomaly();
  336.             }
  337.             else {
  338.                 final KK dt      = kep.getDate().durationFrom(previousDate);
  339.                 final KK keplerM = previousM.add(kep.getKeplerianMeanMotion().multiply(dt));
  340.                 continuousPA   = MathUtils.normalizeAngle(kep.getPerigeeArgument(), previousPA);
  341.                 continuousRAAN = MathUtils.normalizeAngle(kep.getRightAscensionOfAscendingNode(), previousRAAN);
  342.                 continuousM    = MathUtils.normalizeAngle(kep.getMeanAnomaly(), keplerM);
  343.             }
  344.             previousDate = kep.getDate();
  345.             previousPA   = continuousPA;
  346.             previousRAAN = continuousRAAN;
  347.             previousM    = continuousM;
  348.             final KK[] toAdd = MathArrays.buildArray(field, 6);
  349.             toAdd[0] = kep.getA();
  350.             toAdd[1] = kep.getE();
  351.             toAdd[2] = kep.getI();
  352.             toAdd[3] = continuousPA;
  353.             toAdd[4] = continuousRAAN;
  354.             toAdd[5] = continuousM;
  355.             if (useDerivatives) {
  356.                 final KK[] toAddDot = MathArrays.buildArray(field, 6);
  357.                 toAddDot[0] = kep.getADot();
  358.                 toAddDot[1] = kep.getEDot();
  359.                 toAddDot[2] = kep.getIDot();
  360.                 toAddDot[3] = kep.getPerigeeArgumentDot();
  361.                 toAddDot[4] = kep.getRightAscensionOfAscendingNodeDot();
  362.                 toAddDot[5] = kep.getMeanAnomalyDot();
  363.                 interpolator.addSamplePoint(kep.getDate().durationFrom(interpolationDate),
  364.                                             toAdd, toAddDot);
  365.             }
  366.             else {
  367.                 interpolator.addSamplePoint(this.zero.add(kep.getDate().durationFrom(interpolationDate)),
  368.                                             toAdd);
  369.             }
  370.         }

  371.         return interpolator.derivatives(zero, 1);
  372.     }

  373.     /**
  374.      * Build interpolating functions for equinoctial orbit parameters.
  375.      *
  376.      * @param interpolationDate interpolation date
  377.      * @param orbits orbits sample
  378.      * @param useDerivatives flag defining if derivatives are available throughout the sample
  379.      *
  380.      * @return interpolating functions for equinoctial orbit parameters
  381.      */
  382.     private KK[][] interpolateEquinoctial(final FieldAbsoluteDate<KK> interpolationDate, final List<FieldOrbit<KK>> orbits,
  383.                                           final boolean useDerivatives) {

  384.         // set up an interpolator
  385.         final FieldHermiteInterpolator<KK> interpolator = new FieldHermiteInterpolator<>();

  386.         // second pass to feed interpolator
  387.         FieldAbsoluteDate<KK> previousDate = null;
  388.         KK                    previousLm   = zero.add(Double.NaN);
  389.         for (final FieldOrbit<KK> orbit : orbits) {
  390.             final FieldEquinoctialOrbit<KK> equi = (FieldEquinoctialOrbit<KK>) OrbitType.EQUINOCTIAL.convertType(orbit);
  391.             final KK                        continuousLm;
  392.             if (previousDate == null) {
  393.                 continuousLm = equi.getLM();
  394.             }
  395.             else {
  396.                 final KK dt       = equi.getDate().durationFrom(previousDate);
  397.                 final KK keplerLm = previousLm.add(equi.getKeplerianMeanMotion().multiply(dt));
  398.                 continuousLm = MathUtils.normalizeAngle(equi.getLM(), keplerLm);
  399.             }
  400.             previousDate = equi.getDate();
  401.             previousLm   = continuousLm;
  402.             final KK[] toAdd = MathArrays.buildArray(field, 6);
  403.             toAdd[0] = equi.getA();
  404.             toAdd[1] = equi.getEquinoctialEx();
  405.             toAdd[2] = equi.getEquinoctialEy();
  406.             toAdd[3] = equi.getHx();
  407.             toAdd[4] = equi.getHy();
  408.             toAdd[5] = continuousLm;
  409.             if (useDerivatives) {
  410.                 final KK[] toAddDot = MathArrays.buildArray(one.getField(), 6);
  411.                 toAddDot[0] = equi.getADot();
  412.                 toAddDot[1] = equi.getEquinoctialExDot();
  413.                 toAddDot[2] = equi.getEquinoctialEyDot();
  414.                 toAddDot[3] = equi.getHxDot();
  415.                 toAddDot[4] = equi.getHyDot();
  416.                 toAddDot[5] = equi.getLMDot();
  417.                 interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
  418.                                             toAdd, toAddDot);
  419.             }
  420.             else {
  421.                 interpolator.addSamplePoint(equi.getDate().durationFrom(interpolationDate),
  422.                                             toAdd);
  423.             }
  424.         }

  425.         return interpolator.derivatives(zero, 1);
  426.     }
  427. }