StateCovariance.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.linear.Array2DRowRealMatrix;
  19. import org.hipparchus.linear.MatrixUtils;
  20. import org.hipparchus.linear.RealMatrix;
  21. import org.orekit.errors.OrekitException;
  22. import org.orekit.errors.OrekitMessages;
  23. import org.orekit.frames.Frame;
  24. import org.orekit.frames.LOF;
  25. import org.orekit.frames.Transform;
  26. import org.orekit.orbits.Orbit;
  27. import org.orekit.orbits.OrbitType;
  28. import org.orekit.orbits.PositionAngleType;
  29. import org.orekit.time.AbsoluteDate;
  30. import org.orekit.time.TimeStamped;
  31. import org.orekit.utils.CartesianDerivativesFilter;

  32. /** This class is the representation of a covariance matrix at a given date.
  33.  * <p>
  34.  * Currently, the covariance only represents the orbital elements.
  35.  * <p>
  36.  * It is possible to change the covariance frame by using the
  37.  * {@link #changeCovarianceFrame(Orbit, Frame)} or {@link #changeCovarianceFrame(Orbit, LOF)} method.
  38.  * These methods are based on Equations (18) and (20) of <i>Covariance Transformations for Satellite
  39.  * Flight Dynamics Operations</i> by David A. SVallado.
  40.  * <p>
  41.  * Finally, covariance orbit type can be changed using the
  42.  * {@link #changeCovarianceType(Orbit, OrbitType, PositionAngleType)} method.
  43.  *
  44.  * @author Bryan Cazabonne
  45.  * @author Vincent Cucchietti
  46.  * @since 11.3
  47.  */
  48. public class StateCovariance implements TimeStamped {

  49.     /** State dimension. */
  50.     public static final int STATE_DIMENSION = 6;

  51.     /** Default position angle for covariance expressed in Cartesian elements. */
  52.     private static final PositionAngleType DEFAULT_POSITION_ANGLE = PositionAngleType.TRUE;

  53.     /** Orbital covariance [6x6]. */
  54.     private final RealMatrix orbitalCovariance;

  55.     /** Covariance frame (can be null if LOF is defined). */
  56.     private final Frame frame;

  57.     /** Covariance LOF type (can be null if frame is defined). */
  58.     private final LOF lof;

  59.     /** Covariance epoch. */
  60.     private final AbsoluteDate epoch;

  61.     /** Covariance orbit type. */
  62.     private final OrbitType orbitType;

  63.     /** Covariance position angle type (not used if orbitType is CARTESIAN). */
  64.     private final PositionAngleType angleType;

  65.     /**
  66.      * Constructor.
  67.      * @param orbitalCovariance 6x6 orbital parameters covariance
  68.      * @param epoch epoch of the covariance
  69.      * @param lof covariance LOF type
  70.      */
  71.     public StateCovariance(final RealMatrix orbitalCovariance, final AbsoluteDate epoch, final LOF lof) {
  72.         this(orbitalCovariance, epoch, null, lof, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
  73.     }

  74.     /**
  75.      * Constructor.
  76.      * @param orbitalCovariance 6x6 orbital parameters covariance
  77.      * @param epoch epoch of the covariance
  78.      * @param covarianceFrame covariance frame (inertial or Earth fixed)
  79.      * @param orbitType orbit type of the covariance (CARTESIAN if covarianceFrame is not pseudo-inertial)
  80.      * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
  81.      */
  82.     public StateCovariance(final RealMatrix orbitalCovariance, final AbsoluteDate epoch,
  83.                            final Frame covarianceFrame,
  84.                            final OrbitType orbitType, final PositionAngleType angleType) {
  85.         this(orbitalCovariance, epoch, covarianceFrame, null, orbitType, angleType);
  86.     }

  87.     /**
  88.      * Private constructor.
  89.      * @param orbitalCovariance 6x6 orbital parameters covariance
  90.      * @param epoch epoch of the covariance
  91.      * @param covarianceFrame covariance frame (inertial or Earth fixed)
  92.      * @param lof covariance LOF type
  93.      * @param orbitType orbit type of the covariance
  94.      * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
  95.      */
  96.     private StateCovariance(final RealMatrix orbitalCovariance, final AbsoluteDate epoch,
  97.                             final Frame covarianceFrame, final LOF lof,
  98.                             final OrbitType orbitType, final PositionAngleType angleType) {

  99.         checkFrameAndOrbitTypeConsistency(covarianceFrame, orbitType);

  100.         this.orbitalCovariance = orbitalCovariance;
  101.         this.epoch = epoch;
  102.         this.frame     = covarianceFrame;
  103.         this.lof       = lof;
  104.         this.orbitType = orbitType;
  105.         this.angleType = angleType;

  106.     }

  107.     /**
  108.      * Check constructor's inputs consistency.
  109.      *
  110.      * @param covarianceFrame covariance frame (inertial or Earth fixed)
  111.      * @param inputType orbit type of the covariance
  112.      *
  113.      * @throws OrekitException if input frame is not pseudo-inertial AND the orbit type is not Cartesian
  114.      */
  115.     public static void checkFrameAndOrbitTypeConsistency(final Frame covarianceFrame, final OrbitType inputType) {

  116.         // State covariance expressed in a celestial body frame
  117.         if (covarianceFrame != null) {

  118.             // Input frame is not pseudo-inertial
  119.             if (!covarianceFrame.isPseudoInertial() && inputType != OrbitType.CARTESIAN) {
  120.                 throw new OrekitException(OrekitMessages.WRONG_ORBIT_PARAMETERS_TYPE,
  121.                                           inputType.name(),
  122.                                           OrbitType.CARTESIAN.name());
  123.             }
  124.         }
  125.     }

  126.     /** {@inheritDoc}. */
  127.     @Override
  128.     public AbsoluteDate getDate() {
  129.         return epoch;
  130.     }

  131.     /**
  132.      * Get the covariance matrix.
  133.      * @return the covariance matrix
  134.      */
  135.     public RealMatrix getMatrix() {
  136.         return orbitalCovariance;
  137.     }

  138.     /**
  139.      * Get the covariance orbit type.
  140.      * @return the covariance orbit type
  141.      */
  142.     public OrbitType getOrbitType() {
  143.         return orbitType;
  144.     }

  145.     /**
  146.      * Get the covariance angle type.
  147.      * @return the covariance angle type
  148.      */
  149.     public PositionAngleType getPositionAngleType() {
  150.         return angleType;
  151.     }

  152.     /**
  153.      * Get the covariance frame.
  154.      * @return the covariance frame (can be null)
  155.      * @see #getLOF()
  156.      */
  157.     public Frame getFrame() {
  158.         return frame;
  159.     }

  160.     /**
  161.      * Get the covariance LOF type.
  162.      * @return the covariance LOF type (can be null)
  163.      * @see #getFrame()
  164.      */
  165.     public LOF getLOF() {
  166.         return lof;
  167.     }

  168.     /**
  169.      * Get the covariance matrix in another orbit type.
  170.      * <p>
  171.      * The covariance orbit type <b>cannot</b> be changed if the covariance
  172.      * matrix is expressed in a {@link LOF local orbital frame} or a
  173.      * non-pseudo inertial frame.
  174.      * <p>
  175.      * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
  176.      * Hence, the current covariance matrix <b>will not exactly match</b> the new linearized case and the
  177.      * distribution will not follow a generalized Gaussian distribution anymore.
  178.      * <p>
  179.      * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
  180.      * dynamics operations."
  181.      *
  182.      * @param orbit orbit to which the covariance matrix should correspond
  183.      * @param outOrbitType target orbit type of the state covariance matrix
  184.      * @param outAngleType target position angle type of the state covariance matrix
  185.      * @return a new covariance state, expressed in the target orbit type with the target position angle
  186.      * @see #changeCovarianceFrame(Orbit, Frame)
  187.      */
  188.     public StateCovariance changeCovarianceType(final Orbit orbit, final OrbitType outOrbitType,
  189.                                                 final PositionAngleType outAngleType) {

  190.         // Handle case where the covariance is already expressed in the output type
  191.         if (outOrbitType == orbitType && (outAngleType == angleType || outOrbitType == OrbitType.CARTESIAN)) {
  192.             if (lof == null) {
  193.                 return new StateCovariance(orbitalCovariance, epoch, frame, orbitType, angleType);
  194.             }
  195.             else {
  196.                 return new StateCovariance(orbitalCovariance, epoch, lof);
  197.             }
  198.         }

  199.         // Check if the covariance is expressed in a celestial body frame
  200.         if (frame != null) {

  201.             // Check if the covariance is defined in an inertial frame
  202.             if (frame.isPseudoInertial()) {
  203.                 return changeTypeAndCreate(orbit, epoch, frame, orbitType, angleType, outOrbitType, outAngleType,
  204.                                            orbitalCovariance);
  205.             }

  206.             // The covariance is not defined in an inertial frame. The orbit type cannot be changed
  207.             throw new OrekitException(OrekitMessages.CANNOT_CHANGE_COVARIANCE_TYPE_IF_DEFINED_IN_NON_INERTIAL_FRAME);

  208.         }

  209.         // The covariance is not expressed in a celestial body frame. The orbit type cannot be changed
  210.         throw new OrekitException(OrekitMessages.CANNOT_CHANGE_COVARIANCE_TYPE_IF_DEFINED_IN_LOF);

  211.     }

  212.     /**
  213.      * Get the covariance in a given local orbital frame.
  214.      * <p>
  215.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  216.      * in covariance orbit type is required.
  217.      * <p>
  218.      * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
  219.      * flight dynamics operations."
  220.      *
  221.      * @param orbit orbit to which the covariance matrix should correspond
  222.      * @param lofOut output local orbital frame
  223.      * @return a new covariance state, expressed in the output local orbital frame
  224.      */
  225.     public StateCovariance changeCovarianceFrame(final Orbit orbit, final LOF lofOut) {

  226.         // Verify current covariance frame
  227.         if (lof != null) {

  228.             // Change the covariance local orbital frame
  229.             return changeFrameAndCreate(orbit, epoch, lof, lofOut, orbitalCovariance);

  230.         } else {

  231.             // Covariance is expressed in celestial body frame
  232.             return changeFrameAndCreate(orbit, epoch, frame, lofOut, orbitalCovariance, orbitType, angleType);

  233.         }

  234.     }

  235.     /**
  236.      * Get the covariance in the output frame.
  237.      * <p>
  238.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  239.      * in covariance orbit type is required.
  240.      * <p>
  241.      * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
  242.      * flight dynamics operations."
  243.      *
  244.      * @param orbit orbit to which the covariance matrix should correspond
  245.      * @param frameOut output frame
  246.      * @return a new covariance state, expressed in the output frame
  247.      */
  248.     public StateCovariance changeCovarianceFrame(final Orbit orbit, final Frame frameOut) {

  249.         // Verify current covariance frame
  250.         if (lof != null) {

  251.             // Covariance is expressed in local orbital frame
  252.             return changeFrameAndCreate(orbit, epoch, lof, frameOut, orbitalCovariance);

  253.         } else {

  254.             // Change covariance frame
  255.             return changeFrameAndCreate(orbit, epoch, frame, frameOut, orbitalCovariance, orbitType, angleType);

  256.         }

  257.     }

  258.     /**
  259.      * Get a time-shifted covariance matrix.
  260.      * <p>
  261.      * The shifting model is a linearized, Keplerian one. In other words, it is based on a state transition matrix that
  262.      * is computed assuming Keplerian motion.
  263.      * <p>
  264.      * Shifting is <em>not</em> intended as a replacement for proper covariance propagation, but should be sufficient
  265.      * for small time shifts or coarse accuracy.
  266.      *
  267.      * @param orbit orbit to which the covariance matrix should correspond
  268.      * @param dt time shift in seconds
  269.      * @return a new covariance state, shifted with respect to the instance
  270.      */
  271.     public StateCovariance shiftedBy(final Orbit orbit, final double dt) {

  272.         // Shifted orbit
  273.         final Orbit shifted = orbit.shiftedBy(dt);

  274.         // State covariance expressed in celestial body frame
  275.         if (frame != null) {

  276.             // State covariance expressed in a pseudo-inertial frame
  277.             if (frame.isPseudoInertial()) {

  278.                 // Compute STM
  279.                 final RealMatrix stm = getStm(orbit, dt);

  280.                 // Convert covariance in STM type (i.e., Equinoctial elements)
  281.                 final StateCovariance inStmType = changeTypeAndCreate(orbit, epoch, frame, orbitType, angleType,
  282.                                                                       OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
  283.                                                                       orbitalCovariance);

  284.                 // Shift covariance by applying the STM
  285.                 final RealMatrix shiftedCov = stm.multiply(inStmType.getMatrix().multiplyTransposed(stm));

  286.                 // Restore the initial covariance type
  287.                 return changeTypeAndCreate(shifted, shifted.getDate(), frame,
  288.                                            OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
  289.                                            orbitType, angleType, shiftedCov);
  290.             }

  291.             // State covariance expressed in a non pseudo-inertial frame
  292.             else {

  293.                 // Convert state covariance to orbit pseudo-inertial frame
  294.                 final StateCovariance inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());

  295.                 // Shift the state covariance
  296.                 final StateCovariance shiftedCovariance = inOrbitFrame.shiftedBy(orbit, dt);

  297.                 // Restore the initial covariance frame
  298.                 return shiftedCovariance.changeCovarianceFrame(shifted, frame);
  299.             }
  300.         }

  301.         // State covariance expressed in a commonly used local orbital frame (LOF)
  302.         else {

  303.             // Convert state covariance to orbit pseudo-inertial frame
  304.             final StateCovariance inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());

  305.             // Shift the state covariance
  306.             final StateCovariance shiftedCovariance = inOrbitFrame.shiftedBy(orbit, dt);

  307.             // Restore the initial covariance frame
  308.             return shiftedCovariance.changeCovarianceFrame(shifted, lof);
  309.         }

  310.     }

  311.     /**
  312.      * Create a covariance matrix in another orbit type.
  313.      * <p>
  314.      * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
  315.      * Hence, the input covariance matrix <b>will not exactly match</b> the new linearized case and the
  316.      * distribution will not follow a generalized Gaussian distribution anymore.
  317.      * <p>
  318.      * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
  319.      * dynamics operations."
  320.      *
  321.      * @param orbit orbit to which the covariance matrix should correspond
  322.      * @param date covariance epoch
  323.      * @param covFrame covariance frame
  324.      * @param inOrbitType initial orbit type of the state covariance matrix
  325.      * @param inAngleType initial position angle type of the state covariance matrix
  326.      * @param outOrbitType target orbit type of the state covariance matrix
  327.      * @param outAngleType target position angle type of the state covariance matrix
  328.      * @param inputCov input covariance
  329.      * @return the covariance expressed in the target orbit type with the target position angle
  330.      */
  331.     private static StateCovariance changeTypeAndCreate(final Orbit orbit, final AbsoluteDate date,
  332.                                                        final Frame covFrame,
  333.                                                        final OrbitType inOrbitType, final PositionAngleType inAngleType,
  334.                                                        final OrbitType outOrbitType, final PositionAngleType outAngleType,
  335.                                                        final RealMatrix inputCov) {

  336.         // Notations:
  337.         // I: Input orbit type
  338.         // O: Output orbit type
  339.         // C: Cartesian parameters

  340.         // Compute dOutputdCartesian
  341.         final double[][] aOC               = new double[STATE_DIMENSION][STATE_DIMENSION];
  342.         final Orbit      orbitInOutputType = outOrbitType.convertType(orbit);
  343.         orbitInOutputType.getJacobianWrtCartesian(outAngleType, aOC);
  344.         final RealMatrix dOdC = new Array2DRowRealMatrix(aOC, false);

  345.         // Compute dCartesiandInput
  346.         final double[][] aCI              = new double[STATE_DIMENSION][STATE_DIMENSION];
  347.         final Orbit      orbitInInputType = inOrbitType.convertType(orbit);
  348.         orbitInInputType.getJacobianWrtParameters(inAngleType, aCI);
  349.         final RealMatrix dCdI = new Array2DRowRealMatrix(aCI, false);

  350.         // Compute dOutputdInput
  351.         final RealMatrix dOdI = dOdC.multiply(dCdI);

  352.         // Conversion of the state covariance matrix in target orbit type
  353.         final RealMatrix outputCov = dOdI.multiply(inputCov.multiplyTransposed(dOdI));

  354.         // Return the converted covariance
  355.         return new StateCovariance(outputCov, date, covFrame, outOrbitType, outAngleType);

  356.     }

  357.     /**
  358.      * Create a covariance matrix from a {@link LOF local orbital frame} to another
  359.      * {@link LOF local orbital frame}.
  360.      * <p>
  361.      * Changing the covariance frame is a linear process, this method does not introduce approximation.
  362.      * <p>
  363.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  364.      * Operations" by David A. Vallado.
  365.      *
  366.      * @param orbit orbit to which the covariance matrix should correspond
  367.      * @param date covariance epoch
  368.      * @param lofIn the local orbital frame in which the input covariance matrix is expressed
  369.      * @param lofOut the target local orbital frame
  370.      * @param inputCartesianCov input covariance {@code CARTESIAN})
  371.      * @return the covariance matrix expressed in the target commonly used local orbital frame in Cartesian elements
  372.      */
  373.     private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
  374.                                                         final LOF lofIn, final LOF lofOut,
  375.                                                         final RealMatrix inputCartesianCov) {

  376.         // Builds the matrix to perform covariance transformation
  377.         final RealMatrix jacobianFromLofInToLofOut =
  378.                 getJacobian(LOF.transformFromLOFInToLOFOut(lofIn, lofOut, date, orbit.getPVCoordinates()));

  379.         // Get the Cartesian covariance matrix converted to frameOut
  380.         final RealMatrix cartesianCovarianceOut =
  381.                 jacobianFromLofInToLofOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToLofOut));

  382.         // Output converted covariance
  383.         return new StateCovariance(cartesianCovarianceOut, date, lofOut);

  384.     }

  385.     /**
  386.      * Convert the covariance matrix from a {@link Frame frame} to a {@link LOF local orbital frame}.
  387.      * <p>
  388.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  389.      * in covariance orbit type is required.
  390.      * <p>
  391.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  392.      * Operations" by David A. Vallado.
  393.      * <p>
  394.      * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
  395.      * and position angle types of the input covariance must be provided.
  396.      * <p>
  397.      * <b>The output covariance matrix will provided in Cartesian orbit type and not converted back to
  398.      * its original expression (if input different from Cartesian elements).</b>
  399.      *
  400.      * @param orbit orbit to which the covariance matrix should correspond
  401.      * @param date covariance epoch
  402.      * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is <b>not</b>
  403.      * pseudo-inertial, the input covariance matrix is expected to be expressed in <b>Cartesian elements</b>.
  404.      * @param lofOut the target local orbital frame
  405.      * @param inputCov input covariance
  406.      * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
  407.      * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used
  408.      * if covOrbitType equals {@code CARTESIAN})
  409.      * @return the covariance matrix expressed in the target local orbital frame in Cartesian elements
  410.      * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
  411.      * <b>not</b> expressed in Cartesian elements.
  412.      */
  413.     private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
  414.                                                         final Frame frameIn, final LOF lofOut,
  415.                                                         final RealMatrix inputCov,
  416.                                                         final OrbitType covOrbitType,
  417.                                                         final PositionAngleType covAngleType) {

  418.         // Input frame is inertial
  419.         if (frameIn.isPseudoInertial()) {

  420.             // Convert input matrix to Cartesian parameters in input frame
  421.             final RealMatrix cartesianCovarianceIn =
  422.                     changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
  423.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN,
  424.                                         inputCov).getMatrix();

  425.             // Builds the matrix to perform covariance transformation
  426.             final RealMatrix jacobianFromFrameInToLofOut =
  427.                             getJacobian(lofOut.transformFromInertial(date, orbit.getPVCoordinates(frameIn)));

  428.             // Get the Cartesian covariance matrix converted to frameOut
  429.             final RealMatrix cartesianCovarianceOut =
  430.                     jacobianFromFrameInToLofOut.multiply(cartesianCovarianceIn.multiplyTransposed(jacobianFromFrameInToLofOut));

  431.             // Return converted covariance matrix expressed in Cartesian elements
  432.             return new StateCovariance(cartesianCovarianceOut, date, lofOut);

  433.         }

  434.         // Input frame is not inertial so the covariance matrix is expected to be in Cartesian elements
  435.         else {

  436.             // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type
  437.             final Frame orbitInertialFrame = orbit.getFrame();

  438.             // Compute rotation matrix from frameIn to orbit inertial frame
  439.             final RealMatrix cartesianCovarianceInOrbitFrame =
  440.                    changeFrameAndCreate(orbit, date, frameIn, orbitInertialFrame, inputCov,
  441.                                          OrbitType.CARTESIAN, PositionAngleType.MEAN).getMatrix();

  442.             // Convert from orbit inertial frame to lofOut
  443.             return changeFrameAndCreate(orbit, date, orbitInertialFrame, lofOut, cartesianCovarianceInOrbitFrame,
  444.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN);

  445.         }

  446.     }

  447.     /**
  448.      * Convert the covariance matrix from a {@link LOF  local orbital frame} to a {@link Frame frame}.
  449.      * <p>
  450.      * Changing the covariance frame is a linear process, this method does not introduce approximation.
  451.      * <p>
  452.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  453.      * Operations" by David A. Vallado.
  454.      * <p>
  455.      * The <u>input</u> covariance matrix is necessarily provided in <b>Cartesian orbit type</b>.
  456.      * <p>
  457.      * The <u>output</u> covariance matrix will be expressed in <b>Cartesian elements</b>.
  458.      *
  459.      * @param orbit orbit to which the covariance matrix should correspond
  460.      * @param date covariance epoch
  461.      * @param lofIn the local orbital frame in which the input covariance matrix is expressed
  462.      * @param frameOut the target frame
  463.      * @param inputCartesianCov input covariance ({@code CARTESIAN})
  464.      * @return the covariance matrix expressed in the target frame in Cartesian elements
  465.      */
  466.     private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
  467.                                                         final LOF lofIn, final Frame frameOut,
  468.                                                         final RealMatrix inputCartesianCov) {

  469.         // Output frame is pseudo-inertial
  470.         if (frameOut.isPseudoInertial()) {

  471.             // Builds the matrix to perform covariance transformation
  472.             final RealMatrix jacobianFromLofInToFrameOut =
  473.                     getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates(frameOut)).getInverse());

  474.             // Transform covariance
  475.             final RealMatrix transformedCovariance =
  476.                     jacobianFromLofInToFrameOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToFrameOut));

  477.             // Get the Cartesian covariance matrix converted to frameOut
  478.             return new StateCovariance(transformedCovariance, date, frameOut, OrbitType.CARTESIAN,
  479.                                        DEFAULT_POSITION_ANGLE);

  480.         }

  481.         // Output frame is not pseudo-inertial
  482.         else {

  483.             // Builds the matrix to perform covariance transformation
  484.             final RealMatrix jacobianFromLofInToOrbitFrame =
  485.                     getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates()).getInverse());

  486.             // Get the Cartesian covariance matrix converted to orbit inertial frame
  487.             final RealMatrix cartesianCovarianceInOrbitFrame = jacobianFromLofInToOrbitFrame.multiply(
  488.                     inputCartesianCov.multiplyTransposed(jacobianFromLofInToOrbitFrame));

  489.             // Get the Cartesian covariance matrix converted to frameOut
  490.             return changeFrameAndCreate(orbit, date, orbit.getFrame(), frameOut, cartesianCovarianceInOrbitFrame,
  491.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN);
  492.         }

  493.     }

  494.     /**
  495.      * Get the covariance matrix in another frame.
  496.      * <p>
  497.      * The transformation is based on Equation (18) of "Covariance Transformations for Satellite Flight Dynamics
  498.      * Operations" by David A. Vallado.
  499.      * <p>
  500.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  501.      * in covariance orbit type is required.
  502.      * <p>
  503.      * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
  504.      * and position angle types of the input covariance must be provided.
  505.      * <p>
  506.      * In case the <u>input</u> frame is <b>not</b> pseudo-inertial, the <u>input</u> covariance matrix is necessarily
  507.      * expressed in <b>Cartesian elements</b>.
  508.      * <p>
  509.      * In case the <u>output</u> frame is <b>not</b> pseudo-inertial, the <u>output</u> covariance matrix will be
  510.      * expressed in <b>Cartesian elements</b>.
  511.      *
  512.      * @param orbit orbit to which the covariance matrix should correspond
  513.      * @param date covariance epoch
  514.      * @param frameIn the frame in which the input covariance matrix is expressed
  515.      * @param frameOut the target frame
  516.      * @param inputCov input covariance
  517.      * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
  518.      * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (<b>not</b>
  519.      * used if covOrbitType equals {@code CARTESIAN})
  520.      * @return the covariance matrix expressed in the target frame
  521.      * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
  522.      * <b>not</b> expressed in Cartesian elements.
  523.      */
  524.     private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
  525.                                                         final Frame frameIn, final Frame frameOut,
  526.                                                         final RealMatrix inputCov,
  527.                                                         final OrbitType covOrbitType,
  528.                                                         final PositionAngleType covAngleType) {

  529.         // Get the transform from the covariance frame to the output frame
  530.         final Transform inToOut = frameIn.getTransformTo(frameOut, orbit.getDate());

  531.         // Matrix to perform the covariance transformation
  532.         final RealMatrix j = getJacobian(inToOut);

  533.         // Input frame pseudo-inertial
  534.         if (frameIn.isPseudoInertial()) {

  535.             // Convert input matrix to Cartesian parameters in input frame
  536.             final RealMatrix cartesianCovarianceIn =
  537.                     changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
  538.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN,
  539.                                         inputCov).getMatrix();

  540.             // Get the Cartesian covariance matrix converted to frameOut
  541.             final RealMatrix cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j));

  542.             // Output frame is pseudo-inertial
  543.             if (frameOut.isPseudoInertial()) {

  544.                 // Convert output Cartesian matrix to initial orbit type and position angle
  545.                 return changeTypeAndCreate(orbit, date, frameOut, OrbitType.CARTESIAN, PositionAngleType.MEAN,
  546.                                            covOrbitType, covAngleType, cartesianCovarianceOut);

  547.             }

  548.             // Output frame is not pseudo-inertial
  549.             else {

  550.                 // Output Cartesian matrix
  551.                 return new StateCovariance(cartesianCovarianceOut, date, frameOut, OrbitType.CARTESIAN,
  552.                                            DEFAULT_POSITION_ANGLE);

  553.             }
  554.         }

  555.         // Input frame is not pseudo-inertial
  556.         else {

  557.             // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type

  558.             // Convert covariance matrix to frameOut
  559.             final RealMatrix covInFrameOut = j.multiply(inputCov.multiplyTransposed(j));

  560.             // Output the Cartesian covariance matrix converted to frameOut
  561.             return new StateCovariance(covInFrameOut, date, frameOut, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);

  562.         }

  563.     }

  564.     /**
  565.      * Builds the matrix to perform covariance frame transformation.
  566.      * @param transform input transformation
  567.      * @return the matrix to perform the covariance frame transformation
  568.      */
  569.     private static RealMatrix getJacobian(final Transform transform) {
  570.         // Get the Jacobian of the transform
  571.         final double[][] jacobian = new double[STATE_DIMENSION][STATE_DIMENSION];
  572.         transform.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
  573.         // Return
  574.         return new Array2DRowRealMatrix(jacobian, false);

  575.     }

  576.     /**
  577.      * Get the state transition matrix considering Keplerian contribution only.
  578.      *
  579.      * @param initialOrbit orbit to which the initial covariance matrix should correspond
  580.      * @param dt time difference between the two orbits
  581.      * @return the state transition matrix used to shift the covariance matrix
  582.      */
  583.     public static RealMatrix getStm(final Orbit initialOrbit, final double dt) {

  584.         // initialize the STM
  585.         final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);

  586.         // State transition matrix using Keplerian contribution only
  587.         final double contribution = initialOrbit.getMeanAnomalyDotWrtA() * dt;
  588.         stm.setEntry(5, 0, contribution);

  589.         // Return
  590.         return stm;

  591.     }

  592. }