StateCovarianceKeplerianHermiteInterpolator.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.propagation;

import org.hipparchus.analysis.interpolation.HermiteInterpolator;
import org.hipparchus.linear.BlockRealMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.orekit.errors.OrekitInternalError;
import org.orekit.frames.Frame;
import org.orekit.frames.LOFType;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeInterpolator;
import org.orekit.time.TimeStampedPair;
import org.orekit.utils.CartesianDerivativesFilter;

import java.util.ArrayList;
import java.util.List;

/**
 * State covariance Keplerian quintic interpolator.
 * <p>
 * Its purpose is to interpolate state covariance between tabulated state covariances using polynomial interpolation. To do
 * so, it uses a {@link HermiteInterpolator} and compute the first and second order derivatives at tabulated states assuming
 * a standard Keplerian motion depending on given derivatives filter.
 * <p>
 * It gives very accurate results as explained <a
 * href="https://orekit.org/doc/technical-notes/Implementation_of_covariance_interpolation_in_Orekit.pdf">here</a>. In the
 * very poorly tracked test case evolving in a highly dynamical environment mentioned in the linked thread, the user can
 * expect at worst errors of less than 0.2% in position sigmas and less than 0.35% in velocity sigmas with steps of 40mn
 * between tabulated values.
 * <p>
 * However, note that this method does not guarantee the positive definiteness of the computed state covariance as opposed to
 * {@link StateCovarianceBlender}.
 *
 * @author Vincent Cucchietti
 * @see HermiteInterpolator
 * @see StateCovarianceBlender
 */
public class StateCovarianceKeplerianHermiteInterpolator extends AbstractStateCovarianceInterpolator {

    /**
     * Filter defining if only the state covariance value are used or if first or/and second Keplerian derivatives should be
     * used.
     */
    private final CartesianDerivativesFilter filter;

    /**
     * Constructor using an output local orbital frame and :
     * <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).
     * <p>
     * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
     * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
     * case.</b>
     *
     * @param orbitInterpolator orbit interpolator
     * @param outLOF output local orbital frame
     */
    public StateCovarianceKeplerianHermiteInterpolator(final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final LOFType outLOF) {
        this(DEFAULT_INTERPOLATION_POINTS, orbitInterpolator, outLOF);
    }

    /**
     * Constructor using an output local orbital frame and :
     * <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).
     * <p>
     * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
     * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
     * case.</b>
     *
     * @param interpolationPoints number of interpolation points
     * @param orbitInterpolator orbit interpolator
     * @param outLOF output local orbital frame
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
                                                       final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final LOFType outLOF) {
        this(interpolationPoints, orbitInterpolator, CartesianDerivativesFilter.USE_PVA,
             outLOF);
    }

    /**
     * Constructor using an output local orbital frame and :
     * <ul>
     *     <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).
     * <p>
     * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
     * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
     * case.</b>
     *
     * @param interpolationPoints number of interpolation points
     * @param orbitInterpolator orbit interpolator
     * @param outLOF output local orbital frame
     * @param filter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
                                                       final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final CartesianDerivativesFilter filter,
                                                       final LOFType outLOF) {
        this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, filter, outLOF);
    }

    /**
     * Constructor using an output local orbital frame.
     * <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).
     * <p>
     * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
     * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
     * case.</b>
     *
     * @param interpolationPoints number of interpolation points
     * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
     * @param orbitInterpolator orbit interpolator
     * @param outLOF output local orbital frame
     * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
     * derivatives should be used during the interpolation.
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
                                                       final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final CartesianDerivativesFilter filter, final LOFType outLOF) {
        super(interpolationPoints, extrapolationThreshold, orbitInterpolator, outLOF);
        this.filter = filter;
    }

    /**
     * Constructor using an output frame and :
     * <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 orbitInterpolator orbit interpolator
     * @param outFrame output frame
     * @param outOrbitType output orbit type
     * @param outPositionAngleType output position angle
     */
    public StateCovarianceKeplerianHermiteInterpolator(final TimeInterpolator<Orbit> orbitInterpolator, final Frame outFrame,
                                                       final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
        this(DEFAULT_INTERPOLATION_POINTS, orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
    }

    /**
     * Constructor using an output frame and :
     * <ul>
     *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</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 orbitInterpolator orbit interpolator
     * @param outFrame output frame
     * @param outOrbitType output orbit type
     * @param outPositionAngleType output position angle
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
                                                       final TimeInterpolator<Orbit> orbitInterpolator, final Frame outFrame,
                                                       final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
        this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, CartesianDerivativesFilter.USE_PVA,
             outFrame, outOrbitType, outPositionAngleType);
    }

    /**
     * Constructor using an output frame and :
     * <ul>
     *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</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 orbitInterpolator orbit interpolator
     * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
     * derivatives should be used during the interpolation.
     * @param outFrame output frame
     * @param outOrbitType output orbit type
     * @param outPositionAngleType output position angle
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
                                                       final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final CartesianDerivativesFilter filter, final Frame outFrame,
                                                       final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
        this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, filter, outFrame, outOrbitType,
                outPositionAngleType);
    }

    /**
     * Constructor using an output frame.
     * <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 orbitInterpolator orbit interpolator
     * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
     * derivatives should be used during the interpolation.
     * @param outFrame output frame
     * @param outOrbitType output orbit type
     * @param outPositionAngleType output position angle
     */
    public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
                                                       final TimeInterpolator<Orbit> orbitInterpolator,
                                                       final CartesianDerivativesFilter filter, final Frame outFrame,
                                                       final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
        super(interpolationPoints, extrapolationThreshold, orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
        this.filter = filter;
    }

    /** Get Filter defining if only the state covariance value are used or if first or/and second Keplerian derivatives
     * should be used.
     * @return Filter defining if only the state covariance value are used or if first or/and second Keplerian derivatives
     * should be used.
     */
    public CartesianDerivativesFilter getFilter() {
        return filter;
    }

    /** {@inheritDoc} */
    @Override
    protected StateCovariance computeInterpolatedCovarianceInOrbitFrame(
            final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
            final Orbit interpolatedOrbit) {

        // Compute and combine covariances value and time derivatives
        final List<double[][][]> covarianceValueAndDerivativesList = new ArrayList<>();
        for (TimeStampedPair<Orbit, StateCovariance> uncertainState : uncertainStates) {
            final double[][][] currentCovarianceValueAndDerivatives =
                    computeAndCombineCovarianceValueAndDerivatives(uncertainState, interpolatedOrbit);

            covarianceValueAndDerivativesList.add(currentCovarianceValueAndDerivatives);
        }

        // Interpolate covariance matrix in equinoctial elements
        final RealMatrix interpolatedCovarianceMatrixInEqui =
                computeInterpolatedStateCovariance(interpolatedOrbit.getDate(),
                                                   uncertainStates,
                                                   covarianceValueAndDerivativesList);

        return new StateCovariance(interpolatedCovarianceMatrixInEqui,
                                   interpolatedOrbit.getDate(), interpolatedOrbit.getFrame(),
                                   OrbitType.EQUINOCTIAL, DEFAULT_POSITION_ANGLE);
    }

    /**
     * Compute and combine state covariance matrix and its two Keplerian time derivatives.
     *
     * @param uncertainState orbit and its associated covariance
     * @param interpolatedOrbit interpolated orbit
     *
     * @return state covariance matrix and its two time derivatives
     */
    private double[][][] computeAndCombineCovarianceValueAndDerivatives(
            final TimeStampedPair<Orbit, StateCovariance> uncertainState, final Orbit interpolatedOrbit) {

        // Get orbit and associated covariance
        final Orbit           orbit      = uncertainState.getFirst();
        final StateCovariance covariance = uncertainState.getSecond();

        // Express covariance in interpolated orbit frame for consistency among the sample
        final StateCovariance covarianceInOrbitFrame = covariance.changeCovarianceFrame(orbit, interpolatedOrbit.getFrame());

        // Convert to equinoctial elements to avoid singularities
        final StateCovariance covarianceInOrbitFrameInEqui =
                covarianceInOrbitFrame.changeCovarianceType(orbit, OrbitType.EQUINOCTIAL, DEFAULT_POSITION_ANGLE);

        // Get matrix
        final RealMatrix covarianceInOrbitFrameInEquiMatrix = covarianceInOrbitFrameInEqui.getMatrix();

        // Compute covariance first and second time derivative according to instance filter
        final int dim = StateCovariance.STATE_DIMENSION;

        final RealMatrix covarianceMatrixFirstDerInKep;
        final RealMatrix covarianceMatrixSecondDerInKep;

        switch (filter) {
            case USE_P:
                covarianceMatrixFirstDerInKep = MatrixUtils.createRealMatrix(dim, dim);
                covarianceMatrixSecondDerInKep = MatrixUtils.createRealMatrix(dim, dim);
                break;
            case USE_PV:
                covarianceMatrixFirstDerInKep = computeCovarianceFirstDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
                covarianceMatrixSecondDerInKep = MatrixUtils.createRealMatrix(dim, dim);
                break;
            case USE_PVA:
                covarianceMatrixFirstDerInKep = computeCovarianceFirstDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
                covarianceMatrixSecondDerInKep =
                        computeCovarianceSecondDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
                break;
            default:
                // Should never happen
                throw new OrekitInternalError(null);
        }

        // Combine and output the state covariance and its first two time derivative in a single array
        return combineCovarianceValueAndDerivatives(covarianceInOrbitFrameInEquiMatrix,
                                                    covarianceMatrixFirstDerInKep,
                                                    covarianceMatrixSecondDerInKep);
    }

    /**
     * Compute interpolated state covariance in equinoctial elements using a Hermite interpolator.
     *
     * @param interpolationDate interpolation date
     * @param uncertainStates list of orbits and their associated covariances
     * @param covarianceValueAndDerivativesList list of covariances and their associated first and second time derivatives
     *
     * @return interpolated state covariance in equinoctial elements
     *
     * @see HermiteInterpolator
     */
    private RealMatrix computeInterpolatedStateCovariance(final AbsoluteDate interpolationDate,
                                                          final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
                                                          final List<double[][][]> covarianceValueAndDerivativesList) {

        final RealMatrix interpolatedCovarianceMatrix = new BlockRealMatrix(new double[ROW_DIM][COLUMN_DIM]);

        // Interpolate each element in the covariance matrix
        for (int i = 0; i < ROW_DIM; i++) {
            for (int j = 0; j < COLUMN_DIM; j++) {

                // Create an interpolator for each element
                final HermiteInterpolator tempInterpolator = new HermiteInterpolator();

                // Fill interpolator with all samples value and associated derivatives
                for (int k = 0; k < uncertainStates.size(); k++) {
                    final TimeStampedPair<Orbit, StateCovariance> currentUncertainStates = uncertainStates.get(k);

                    final double[][][] currentCovarianceValueAndDerivatives = covarianceValueAndDerivativesList.get(k);

                    final double deltaT = currentUncertainStates.getDate().durationFrom(interpolationDate);

                    addSampleToInterpolator(tempInterpolator, deltaT, currentCovarianceValueAndDerivatives[i][j]);
                }

                // Interpolate
                interpolatedCovarianceMatrix.setEntry(i, j, tempInterpolator.value(0)[0]);
            }
        }

        return interpolatedCovarianceMatrix;
    }

    /**
     * Add sample to given interpolator.
     *
     * @param interpolator interpolator to add sample to
     * @param deltaT abscissa for interpolation
     * @param valueAndDerivatives value and associated derivatives to add
     */
    private void addSampleToInterpolator(final HermiteInterpolator interpolator, final double deltaT,
                                         final double[] valueAndDerivatives) {
        switch (filter) {
            case USE_P:
                interpolator.addSamplePoint(deltaT, new double[] { valueAndDerivatives[0] });
                break;
            case USE_PV:
                interpolator.addSamplePoint(deltaT,
                                            new double[] { valueAndDerivatives[0] },
                                            new double[] { valueAndDerivatives[1] });
                break;
            case USE_PVA:
                interpolator.addSamplePoint(deltaT,
                                            new double[] { valueAndDerivatives[0] },
                                            new double[] { valueAndDerivatives[1] },
                                            new double[] { valueAndDerivatives[2] });
                break;
            default:
                // Should never happen
                throw new OrekitInternalError(null);
        }
    }

    /**
     * Compute state covariance first Keplerian time derivative.
     *
     * @param orbit orbit
     * @param covarianceMatrixInEqui state covariance matrix in equinoctial elements
     *
     * @return state covariance first time derivative
     */
    private RealMatrix computeCovarianceFirstDerivative(final Orbit orbit,
                                                        final RealMatrix covarianceMatrixInEqui) {

        final RealMatrix covarianceFirstDerivative = new BlockRealMatrix(ROW_DIM, COLUMN_DIM);

        // Compute common term used in papers
        final double m = orbit.getMeanAnomalyDotWrtA();

        // Compute first time derivative of each element in the covariance matrix
        for (int i = 0; i < ROW_DIM; i++) {
            for (int j = 0; j < COLUMN_DIM; j++) {
                if (i != 5 && j != 5) {
                    covarianceFirstDerivative.setEntry(i, j, 0);
                }
                else if (i == 5 && j != 5) {

                    final double value = covarianceMatrixInEqui.getEntry(0, j) * m;

                    covarianceFirstDerivative.setEntry(i, j, value);
                    covarianceFirstDerivative.setEntry(j, i, value);
                }
                else {
                    final double value = 2 * covarianceMatrixInEqui.getEntry(0, 5) * m;

                    covarianceFirstDerivative.setEntry(i, j, value);
                }
            }
        }

        return covarianceFirstDerivative;

    }

    /**
     * Compute state covariance second Keplerian time derivative.
     *
     * @param orbit orbit
     * @param covarianceMatrixInEqui state covariance matrix in equinoctial elements
     *
     * @return state covariance second time derivative
     */
    private RealMatrix computeCovarianceSecondDerivative(final Orbit orbit,
                                                         final RealMatrix covarianceMatrixInEqui) {

        final RealMatrix covarianceSecondDerivative = new BlockRealMatrix(ROW_DIM, COLUMN_DIM);

        // Compute common term used in papers
        final double m = orbit.getMeanAnomalyDotWrtA();

        // Compute second time derivative of each element in the covariance matrix
        for (int i = 0; i < ROW_DIM; i++) {
            for (int j = 0; j < COLUMN_DIM; j++) {
                if (i == 5 && j == 5) {

                    final double value = 2 * covarianceMatrixInEqui.getEntry(0, 0) * m * m;

                    covarianceSecondDerivative.setEntry(i, j, value);
                }
                else {
                    covarianceSecondDerivative.setEntry(i, j, 0);
                }
            }
        }

        return covarianceSecondDerivative;

    }

    /**
     * Combine state covariance matrix and its two Keplerian time derivatives.
     *
     * @param covarianceMatrixInEqui covariance matrix in equinoctial elements
     * @param covarianceMatrixFirstDerInEqui covariance matrix first time derivative in equinoctial elements
     * @param covarianceMatrixSecondDerInEqui covariance matrix second time derivative in equinoctial elements
     *
     * @return state covariance matrix and its two time derivatives
     */
    private double[][][] combineCovarianceValueAndDerivatives(final RealMatrix covarianceMatrixInEqui,
                                                              final RealMatrix covarianceMatrixFirstDerInEqui,
                                                              final RealMatrix covarianceMatrixSecondDerInEqui) {

        final double[][][] covarianceValueAndDerivativesArray = new double[ROW_DIM][COLUMN_DIM][3];

        // Combine covariance and its first two time derivatives in a single 3D array
        for (int i = 0; i < ROW_DIM; i++) {
            for (int j = 0; j < COLUMN_DIM; j++) {
                covarianceValueAndDerivativesArray[i][j][0] = covarianceMatrixInEqui.getEntry(i, j);
                covarianceValueAndDerivativesArray[i][j][1] = covarianceMatrixFirstDerInEqui.getEntry(i, j);
                covarianceValueAndDerivativesArray[i][j][2] = covarianceMatrixSecondDerInEqui.getEntry(i, j);
            }
        }

        return covarianceValueAndDerivativesArray;
    }
}