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

  18. import org.hipparchus.analysis.interpolation.HermiteInterpolator;
  19. import org.hipparchus.linear.BlockRealMatrix;
  20. import org.hipparchus.linear.MatrixUtils;
  21. import org.hipparchus.linear.RealMatrix;
  22. import org.orekit.errors.OrekitInternalError;
  23. import org.orekit.frames.Frame;
  24. import org.orekit.frames.LOFType;
  25. import org.orekit.orbits.Orbit;
  26. import org.orekit.orbits.OrbitType;
  27. import org.orekit.orbits.PositionAngleType;
  28. import org.orekit.time.AbsoluteDate;
  29. import org.orekit.time.TimeInterpolator;
  30. import org.orekit.time.TimeStampedPair;
  31. import org.orekit.utils.CartesianDerivativesFilter;

  32. import java.util.ArrayList;
  33. import java.util.List;

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

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

  60.     /**
  61.      * Constructor using an output local orbital frame and :
  62.      * <ul>
  63.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  64.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  65.      *     <li>Use of position and two time derivatives during interpolation</li>
  66.      * </ul>
  67.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  68.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  69.      * phenomenon</a> and numerical problems (including NaN appearing).
  70.      * <p>
  71.      * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
  72.      * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
  73.      * case.</b>
  74.      *
  75.      * @param orbitInterpolator orbit interpolator
  76.      * @param outLOF output local orbital frame
  77.      */
  78.     public StateCovarianceKeplerianHermiteInterpolator(final TimeInterpolator<Orbit> orbitInterpolator,
  79.                                                        final LOFType outLOF) {
  80.         this(DEFAULT_INTERPOLATION_POINTS, orbitInterpolator, outLOF);
  81.     }

  82.     /**
  83.      * Constructor using an output local orbital frame and :
  84.      * <ul>
  85.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  86.      *     <li>Use of position and two time derivatives during interpolation</li>
  87.      * </ul>
  88.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  89.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  90.      * phenomenon</a> and numerical problems (including NaN appearing).
  91.      * <p>
  92.      * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
  93.      * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
  94.      * case.</b>
  95.      *
  96.      * @param interpolationPoints number of interpolation points
  97.      * @param orbitInterpolator orbit interpolator
  98.      * @param outLOF output local orbital frame
  99.      */
  100.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
  101.                                                        final TimeInterpolator<Orbit> orbitInterpolator,
  102.                                                        final LOFType outLOF) {
  103.         this(interpolationPoints, orbitInterpolator, CartesianDerivativesFilter.USE_PVA,
  104.              outLOF);
  105.     }

  106.     /**
  107.      * Constructor using an output local orbital frame and :
  108.      * <ul>
  109.      *     <li>Use of position and two time derivatives during interpolation</li>
  110.      * </ul>
  111.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  112.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  113.      * phenomenon</a> and numerical problems (including NaN appearing).
  114.      * <p>
  115.      * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
  116.      * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
  117.      * case.</b>
  118.      *
  119.      * @param interpolationPoints number of interpolation points
  120.      * @param orbitInterpolator orbit interpolator
  121.      * @param outLOF output local orbital frame
  122.      * @param filter filter for derivatives from the sample to use in position-velocity-acceleration interpolation
  123.      */
  124.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
  125.                                                        final TimeInterpolator<Orbit> orbitInterpolator,
  126.                                                        final CartesianDerivativesFilter filter,
  127.                                                        final LOFType outLOF) {
  128.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, filter, outLOF);
  129.     }

  130.     /**
  131.      * Constructor using an output local orbital frame.
  132.      * <p>
  133.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  134.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  135.      * phenomenon</a> and numerical problems (including NaN appearing).
  136.      * <p>
  137.      * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
  138.      * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
  139.      * case.</b>
  140.      *
  141.      * @param interpolationPoints number of interpolation points
  142.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  143.      * @param orbitInterpolator orbit interpolator
  144.      * @param outLOF output local orbital frame
  145.      * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
  146.      * derivatives should be used during the interpolation.
  147.      */
  148.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  149.                                                        final TimeInterpolator<Orbit> orbitInterpolator,
  150.                                                        final CartesianDerivativesFilter filter, final LOFType outLOF) {
  151.         super(interpolationPoints, extrapolationThreshold, orbitInterpolator, outLOF);
  152.         this.filter = filter;
  153.     }

  154.     /**
  155.      * Constructor using an output frame and :
  156.      * <ul>
  157.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  158.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  159.      *     <li>Use of position and two time derivatives during interpolation</li>
  160.      * </ul>
  161.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  162.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  163.      * phenomenon</a> and numerical problems (including NaN appearing).
  164.      *
  165.      * @param orbitInterpolator orbit interpolator
  166.      * @param outFrame output frame
  167.      * @param outOrbitType output orbit type
  168.      * @param outPositionAngleType output position angle
  169.      */
  170.     public StateCovarianceKeplerianHermiteInterpolator(final TimeInterpolator<Orbit> orbitInterpolator, final Frame outFrame,
  171.                                                        final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
  172.         this(DEFAULT_INTERPOLATION_POINTS, orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
  173.     }

  174.     /**
  175.      * Constructor using an output frame and :
  176.      * <ul>
  177.      *     <li>Default number of interpolation points of {@code DEFAULT_INTERPOLATION_POINTS}</li>
  178.      *     <li>Use of position and two time derivatives during interpolation</li>
  179.      * </ul>
  180.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  181.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  182.      * phenomenon</a> and numerical problems (including NaN appearing).
  183.      *
  184.      * @param interpolationPoints number of interpolation points
  185.      * @param orbitInterpolator orbit interpolator
  186.      * @param outFrame output frame
  187.      * @param outOrbitType output orbit type
  188.      * @param outPositionAngleType output position angle
  189.      */
  190.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
  191.                                                        final TimeInterpolator<Orbit> orbitInterpolator, final Frame outFrame,
  192.                                                        final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
  193.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, CartesianDerivativesFilter.USE_PVA,
  194.              outFrame, outOrbitType, outPositionAngleType);
  195.     }

  196.     /**
  197.      * Constructor using an output frame and :
  198.      * <ul>
  199.      *     <li>Default extrapolation threshold value ({@code DEFAULT_EXTRAPOLATION_THRESHOLD_SEC} s)</li>
  200.      * </ul>
  201.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  202.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  203.      * phenomenon</a> and numerical problems (including NaN appearing).
  204.      *
  205.      * @param interpolationPoints number of interpolation points
  206.      * @param orbitInterpolator orbit interpolator
  207.      * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
  208.      * derivatives should be used during the interpolation.
  209.      * @param outFrame output frame
  210.      * @param outOrbitType output orbit type
  211.      * @param outPositionAngleType output position angle
  212.      */
  213.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints,
  214.                                                        final TimeInterpolator<Orbit> orbitInterpolator,
  215.                                                        final CartesianDerivativesFilter filter, final Frame outFrame,
  216.                                                        final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
  217.         this(interpolationPoints, DEFAULT_EXTRAPOLATION_THRESHOLD_SEC, orbitInterpolator, filter, outFrame, outOrbitType,
  218.                 outPositionAngleType);
  219.     }

  220.     /**
  221.      * Constructor using an output frame.
  222.      * <p>
  223.      * As this implementation of interpolation is polynomial, it should be used only with small number of interpolation
  224.      * points (about 10-20 points) in order to avoid <a href="http://en.wikipedia.org/wiki/Runge%27s_phenomenon">Runge's
  225.      * phenomenon</a> and numerical problems (including NaN appearing).
  226.      *
  227.      * @param interpolationPoints number of interpolation points
  228.      * @param extrapolationThreshold extrapolation threshold beyond which the propagation will fail
  229.      * @param orbitInterpolator orbit interpolator
  230.      * @param filter filter defining if only the state covariance value are used or if first or/and second Keplerian
  231.      * derivatives should be used during the interpolation.
  232.      * @param outFrame output frame
  233.      * @param outOrbitType output orbit type
  234.      * @param outPositionAngleType output position angle
  235.      */
  236.     public StateCovarianceKeplerianHermiteInterpolator(final int interpolationPoints, final double extrapolationThreshold,
  237.                                                        final TimeInterpolator<Orbit> orbitInterpolator,
  238.                                                        final CartesianDerivativesFilter filter, final Frame outFrame,
  239.                                                        final OrbitType outOrbitType, final PositionAngleType outPositionAngleType) {
  240.         super(interpolationPoints, extrapolationThreshold, orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
  241.         this.filter = filter;
  242.     }

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

  251.     /** {@inheritDoc} */
  252.     @Override
  253.     protected StateCovariance computeInterpolatedCovarianceInOrbitFrame(
  254.             final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
  255.             final Orbit interpolatedOrbit) {

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

  261.             covarianceValueAndDerivativesList.add(currentCovarianceValueAndDerivatives);
  262.         }

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

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

  272.     /**
  273.      * Compute and combine state covariance matrix and its two Keplerian time derivatives.
  274.      *
  275.      * @param uncertainState orbit and its associated covariance
  276.      * @param interpolatedOrbit interpolated orbit
  277.      *
  278.      * @return state covariance matrix and its two time derivatives
  279.      */
  280.     private double[][][] computeAndCombineCovarianceValueAndDerivatives(
  281.             final TimeStampedPair<Orbit, StateCovariance> uncertainState, final Orbit interpolatedOrbit) {

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

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

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

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

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

  294.         final RealMatrix covarianceMatrixFirstDerInKep;
  295.         final RealMatrix covarianceMatrixSecondDerInKep;

  296.         switch (filter) {
  297.             case USE_P:
  298.                 covarianceMatrixFirstDerInKep = MatrixUtils.createRealMatrix(dim, dim);
  299.                 covarianceMatrixSecondDerInKep = MatrixUtils.createRealMatrix(dim, dim);
  300.                 break;
  301.             case USE_PV:
  302.                 covarianceMatrixFirstDerInKep = computeCovarianceFirstDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
  303.                 covarianceMatrixSecondDerInKep = MatrixUtils.createRealMatrix(dim, dim);
  304.                 break;
  305.             case USE_PVA:
  306.                 covarianceMatrixFirstDerInKep = computeCovarianceFirstDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
  307.                 covarianceMatrixSecondDerInKep =
  308.                         computeCovarianceSecondDerivative(orbit, covarianceInOrbitFrameInEquiMatrix);
  309.                 break;
  310.             default:
  311.                 // Should never happen
  312.                 throw new OrekitInternalError(null);
  313.         }

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

  319.     /**
  320.      * Compute interpolated state covariance in equinoctial elements using a Hermite interpolator.
  321.      *
  322.      * @param interpolationDate interpolation date
  323.      * @param uncertainStates list of orbits and their associated covariances
  324.      * @param covarianceValueAndDerivativesList list of covariances and their associated first and second time derivatives
  325.      *
  326.      * @return interpolated state covariance in equinoctial elements
  327.      *
  328.      * @see HermiteInterpolator
  329.      */
  330.     private RealMatrix computeInterpolatedStateCovariance(final AbsoluteDate interpolationDate,
  331.                                                           final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
  332.                                                           final List<double[][][]> covarianceValueAndDerivativesList) {

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

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

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

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

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

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

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

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

  350.         return interpolatedCovarianceMatrix;
  351.     }

  352.     /**
  353.      * Add sample to given interpolator.
  354.      *
  355.      * @param interpolator interpolator to add sample to
  356.      * @param deltaT abscissa for interpolation
  357.      * @param valueAndDerivatives value and associated derivatives to add
  358.      */
  359.     private void addSampleToInterpolator(final HermiteInterpolator interpolator, final double deltaT,
  360.                                          final double[] valueAndDerivatives) {
  361.         switch (filter) {
  362.             case USE_P:
  363.                 interpolator.addSamplePoint(deltaT, new double[] { valueAndDerivatives[0] });
  364.                 break;
  365.             case USE_PV:
  366.                 interpolator.addSamplePoint(deltaT,
  367.                                             new double[] { valueAndDerivatives[0] },
  368.                                             new double[] { valueAndDerivatives[1] });
  369.                 break;
  370.             case USE_PVA:
  371.                 interpolator.addSamplePoint(deltaT,
  372.                                             new double[] { valueAndDerivatives[0] },
  373.                                             new double[] { valueAndDerivatives[1] },
  374.                                             new double[] { valueAndDerivatives[2] });
  375.                 break;
  376.             default:
  377.                 // Should never happen
  378.                 throw new OrekitInternalError(null);
  379.         }
  380.     }

  381.     /**
  382.      * Compute state covariance first Keplerian time derivative.
  383.      *
  384.      * @param orbit orbit
  385.      * @param covarianceMatrixInEqui state covariance matrix in equinoctial elements
  386.      *
  387.      * @return state covariance first time derivative
  388.      */
  389.     private RealMatrix computeCovarianceFirstDerivative(final Orbit orbit,
  390.                                                         final RealMatrix covarianceMatrixInEqui) {

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

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

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

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

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

  407.                     covarianceFirstDerivative.setEntry(i, j, value);
  408.                 }
  409.             }
  410.         }

  411.         return covarianceFirstDerivative;

  412.     }

  413.     /**
  414.      * Compute state covariance second Keplerian time derivative.
  415.      *
  416.      * @param orbit orbit
  417.      * @param covarianceMatrixInEqui state covariance matrix in equinoctial elements
  418.      *
  419.      * @return state covariance second time derivative
  420.      */
  421.     private RealMatrix computeCovarianceSecondDerivative(final Orbit orbit,
  422.                                                          final RealMatrix covarianceMatrixInEqui) {

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

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

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

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

  431.                     covarianceSecondDerivative.setEntry(i, j, value);
  432.                 }
  433.                 else {
  434.                     covarianceSecondDerivative.setEntry(i, j, 0);
  435.                 }
  436.             }
  437.         }

  438.         return covarianceSecondDerivative;

  439.     }

  440.     /**
  441.      * Combine state covariance matrix and its two Keplerian time derivatives.
  442.      *
  443.      * @param covarianceMatrixInEqui covariance matrix in equinoctial elements
  444.      * @param covarianceMatrixFirstDerInEqui covariance matrix first time derivative in equinoctial elements
  445.      * @param covarianceMatrixSecondDerInEqui covariance matrix second time derivative in equinoctial elements
  446.      *
  447.      * @return state covariance matrix and its two time derivatives
  448.      */
  449.     private double[][][] combineCovarianceValueAndDerivatives(final RealMatrix covarianceMatrixInEqui,
  450.                                                               final RealMatrix covarianceMatrixFirstDerInEqui,
  451.                                                               final RealMatrix covarianceMatrixSecondDerInEqui) {

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

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

  461.         return covarianceValueAndDerivativesArray;
  462.     }
  463. }