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