FieldStateCovariance.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.CalculusFieldElement;
  19. import org.hipparchus.Field;
  20. import org.hipparchus.linear.Array2DRowFieldMatrix;
  21. import org.hipparchus.linear.BlockRealMatrix;
  22. import org.hipparchus.linear.FieldMatrix;
  23. import org.hipparchus.linear.MatrixUtils;
  24. import org.hipparchus.util.MathArrays;
  25. import org.orekit.errors.OrekitException;
  26. import org.orekit.errors.OrekitMessages;
  27. import org.orekit.frames.FieldTransform;
  28. import org.orekit.frames.Frame;
  29. import org.orekit.frames.LOF;
  30. import org.orekit.orbits.FieldOrbit;
  31. import org.orekit.orbits.OrbitType;
  32. import org.orekit.orbits.PositionAngleType;
  33. import org.orekit.time.AbsoluteDate;
  34. import org.orekit.time.FieldAbsoluteDate;
  35. import org.orekit.time.FieldTimeStamped;
  36. import org.orekit.utils.CartesianDerivativesFilter;

  37. /**
  38.  * This class is the representation of a covariance matrix at a given date.
  39.  * <p>
  40.  * Currently, the covariance only represents the orbital elements.
  41.  * <p>
  42.  * It is possible to change the covariance frame by using the {@link #changeCovarianceFrame(FieldOrbit, Frame)} or
  43.  * {@link #changeCovarianceFrame(FieldOrbit, LOF)} method. These methods are based on Equations (18) and (20) of
  44.  * <i>Covariance Transformations for Satellite Flight Dynamics Operations</i> by David A. SVallado.
  45.  * <p>
  46.  * Finally, covariance orbit type can be changed using the
  47.  * {@link #changeCovarianceType(FieldOrbit, OrbitType, PositionAngleType) changeCovarianceType(FieldOrbit, OrbitType,
  48.  * PositionAngle)} method.
  49.  * </p>
  50.  *
  51.  * @author Bryan Cazabonne
  52.  * @author Vincent Cucchietti
  53.  * @since 12.0
  54.  * @param <T> type of the field elements
  55.  */
  56. public class FieldStateCovariance<T extends CalculusFieldElement<T>> implements FieldTimeStamped<T> {

  57.     /** State dimension. */
  58.     private static final int STATE_DIMENSION = 6;

  59.     /** Default position angle for covariance expressed in cartesian elements. */
  60.     private static final PositionAngleType DEFAULT_POSITION_ANGLE = PositionAngleType.TRUE;

  61.     /** Orbital covariance [6x6]. */
  62.     private final FieldMatrix<T> orbitalCovariance;

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

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

  67.     /** Covariance epoch. */
  68.     private final FieldAbsoluteDate<T> epoch;

  69.     /** Covariance orbit type. */
  70.     private final OrbitType orbitType;

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

  73.     /**
  74.      * Constructor.
  75.      *
  76.      * @param orbitalCovariance 6x6 orbital parameters covariance
  77.      * @param epoch epoch of the covariance
  78.      * @param lof covariance LOF type
  79.      */
  80.     public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
  81.                                 final LOF lof) {
  82.         this(orbitalCovariance, epoch, null, lof, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
  83.     }

  84.     /**
  85.      * Constructor.
  86.      *
  87.      * @param orbitalCovariance 6x6 orbital parameters covariance
  88.      * @param epoch epoch of the covariance
  89.      * @param covarianceFrame covariance frame (inertial or Earth fixed)
  90.      * @param orbitType orbit type of the covariance (CARTESIAN if covarianceFrame is not pseudo-inertial)
  91.      * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
  92.      */
  93.     public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
  94.                                 final Frame covarianceFrame,
  95.                                 final OrbitType orbitType, final PositionAngleType angleType) {
  96.         this(orbitalCovariance, epoch, covarianceFrame, null, orbitType, angleType);
  97.     }

  98.     /**
  99.      * Private constructor.
  100.      *
  101.      * @param orbitalCovariance 6x6 orbital parameters covariance
  102.      * @param epoch epoch of the covariance
  103.      * @param covarianceFrame covariance frame (inertial or Earth fixed)
  104.      * @param lof covariance LOF type
  105.      * @param orbitType orbit type of the covariance
  106.      * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
  107.      */
  108.     private FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
  109.                                  final Frame covarianceFrame, final LOF lof,
  110.                                  final OrbitType orbitType, final PositionAngleType angleType) {

  111.         StateCovariance.checkFrameAndOrbitTypeConsistency(covarianceFrame, orbitType);

  112.         this.orbitalCovariance = orbitalCovariance;
  113.         this.epoch             = epoch;
  114.         this.frame             = covarianceFrame;
  115.         this.lof               = lof;
  116.         this.orbitType         = orbitType;
  117.         this.angleType         = angleType;

  118.     }

  119.     /** {@inheritDoc}. */
  120.     @Override
  121.     public FieldAbsoluteDate<T> getDate() {
  122.         return epoch;
  123.     }

  124.     /**
  125.      * Get the covariance matrix.
  126.      *
  127.      * @return the covariance matrix
  128.      */
  129.     public FieldMatrix<T> getMatrix() {
  130.         return orbitalCovariance;
  131.     }

  132.     /**
  133.      * Get the covariance orbit type.
  134.      *
  135.      * @return the covariance orbit type
  136.      */
  137.     public OrbitType getOrbitType() {
  138.         return orbitType;
  139.     }

  140.     /**
  141.      * Get the covariance angle type.
  142.      *
  143.      * @return the covariance angle type
  144.      */
  145.     public PositionAngleType getPositionAngleType() {
  146.         return angleType;
  147.     }

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

  158.     /**
  159.      * Get the covariance LOF type.
  160.      *
  161.      * @return the covariance LOF type (can be null)
  162.      *
  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.      *
  186.      * @return a new covariance state, expressed in the target orbit type with the target position angle
  187.      *
  188.      * @see #changeCovarianceFrame(FieldOrbit, Frame)
  189.      */
  190.     public FieldStateCovariance<T> changeCovarianceType(final FieldOrbit<T> orbit, final OrbitType outOrbitType,
  191.                                                         final PositionAngleType outAngleType) {

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

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

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

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

  210.         }

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

  213.     }

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

  229.         // Verify current covariance frame
  230.         if (lof != null) {

  231.             // Check specific case where output covariance will be the same
  232.             if (lofOut == lof) {
  233.                 return new FieldStateCovariance<>(orbitalCovariance, epoch, lof);
  234.             }

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

  237.         } else {

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

  240.         }

  241.     }

  242.     /**
  243.      * Get the covariance in the output frame.
  244.      * <p>
  245.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  246.      * in covariance orbit type is required.
  247.      * <p>
  248.      * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
  249.      * flight dynamics operations."
  250.      *
  251.      * @param orbit orbit to which the covariance matrix should correspond
  252.      * @param frameOut output frame
  253.      *
  254.      * @return a new covariance state, expressed in the output frame
  255.      */
  256.     public FieldStateCovariance<T> changeCovarianceFrame(final FieldOrbit<T> orbit, final Frame frameOut) {

  257.         // Verify current covariance frame
  258.         if (lof != null) {

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

  261.         } else {

  262.             // Check specific case where output covariance will be the same
  263.             if (frame == frameOut) {
  264.                 return new FieldStateCovariance<>(orbitalCovariance, epoch, frame, orbitType, angleType);
  265.             }

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

  268.         }

  269.     }

  270.     /**
  271.      * Get a time-shifted covariance matrix.
  272.      * <p>
  273.      * The shifting model is a linearized, Keplerian one. In other words, it is based on a state transition matrix that
  274.      * is computed assuming Keplerian motion.
  275.      * <p>
  276.      * Shifting is <em>not</em> intended as a replacement for proper covariance propagation, but should be sufficient
  277.      * for small time shifts or coarse accuracy.
  278.      *
  279.      * @param field to which the elements belong
  280.      * @param orbit orbit to which the covariance matrix should correspond
  281.      * @param dt time shift in seconds
  282.      *
  283.      * @return a new covariance state, shifted with respect to the instance
  284.      */
  285.     public FieldStateCovariance<T> shiftedBy(final Field<T> field, final FieldOrbit<T> orbit, final T dt) {

  286.         // Shifted orbit
  287.         final FieldOrbit<T> shifted = orbit.shiftedBy(dt);

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

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

  292.                 // Compute STM
  293.                 final FieldMatrix<T> stm = getStm(field, orbit, dt);

  294.                 // Convert covariance in STM type (i.e., Equinoctial elements)
  295.                 final FieldStateCovariance<T> inStmType = changeTypeAndCreate(orbit, epoch, frame, orbitType, angleType,
  296.                                                                               OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
  297.                                                                               orbitalCovariance);

  298.                 // Shift covariance by applying the STM
  299.                 final FieldMatrix<T> shiftedCov = stm.multiply(inStmType.getMatrix().multiplyTransposed(stm));

  300.                 // Restore the initial covariance type
  301.                 return changeTypeAndCreate(shifted, shifted.getDate(), frame,
  302.                                            OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
  303.                                            orbitType, angleType, shiftedCov);
  304.             }

  305.             // State covariance expressed in a non pseudo-inertial frame
  306.             else {

  307.                 // Convert state covariance to orbit pseudo-inertial frame
  308.                 final FieldStateCovariance<T> inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());

  309.                 // Shift the state covariance
  310.                 final FieldStateCovariance<T> shiftedCovariance = inOrbitFrame.shiftedBy(field, orbit, dt);

  311.                 // Restore the initial covariance frame
  312.                 return shiftedCovariance.changeCovarianceFrame(shifted, frame);
  313.             }
  314.         }

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

  317.             // Convert state covariance to orbit pseudo-inertial frame
  318.             final FieldStateCovariance<T> inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());

  319.             // Shift the state covariance
  320.             final FieldStateCovariance<T> shiftedCovariance = inOrbitFrame.shiftedBy(field, orbit, dt);

  321.             // Restore the initial covariance frame
  322.             return shiftedCovariance.changeCovarianceFrame(shifted, lof);
  323.         }

  324.     }

  325.     /** Get new state covariance instance.
  326.      * @return new state covariance instance.
  327.      */
  328.     public StateCovariance toStateCovariance() {

  329.         // Extract data
  330.         final T[][] data      = orbitalCovariance.getData();
  331.         final int   rowDim    = data.length;
  332.         final int   columnDim = data.length;

  333.         // Convert to non-field version
  334.         final double[][] realData = new double[rowDim][columnDim];
  335.         for (int i = 0; i < rowDim; i++) {
  336.             for (int j = 0; j < columnDim; j++) {
  337.                 realData[i][j] = data[i][j].getReal();
  338.             }
  339.         }
  340.         final BlockRealMatrix realMatrix = new BlockRealMatrix(realData);
  341.         final AbsoluteDate    realDate   = epoch.toAbsoluteDate();

  342.         // Return new state covariance according to current state covariance definition
  343.         if (frame == null) {
  344.             return new StateCovariance(realMatrix, realDate, lof);
  345.         }
  346.         else {
  347.             return new StateCovariance(realMatrix, realDate, frame, orbitType, angleType);
  348.         }
  349.     }

  350.     /**
  351.      * Create a covariance matrix in another orbit type.
  352.      * <p>
  353.      * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
  354.      * Hence, the input covariance matrix <b>will not exactly match</b> the new linearized case and the
  355.      * distribution will not follow a generalized Gaussian distribution anymore.
  356.      * <p>
  357.      * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
  358.      * dynamics operations."
  359.      *
  360.      * @param orbit orbit to which the covariance matrix should correspond
  361.      * @param date covariance epoch
  362.      * @param covFrame covariance frame
  363.      * @param inOrbitType initial orbit type of the state covariance matrix
  364.      * @param inAngleType initial position angle type of the state covariance matrix
  365.      * @param outOrbitType target orbit type of the state covariance matrix
  366.      * @param outAngleType target position angle type of the state covariance matrix
  367.      * @param inputCov input covariance
  368.      *
  369.      * @return the covariance expressed in the target orbit type with the target position angle
  370.      */
  371.     private FieldStateCovariance<T> changeTypeAndCreate(final FieldOrbit<T> orbit,
  372.                                                         final FieldAbsoluteDate<T> date,
  373.                                                         final Frame covFrame,
  374.                                                         final OrbitType inOrbitType,
  375.                                                         final PositionAngleType inAngleType,
  376.                                                         final OrbitType outOrbitType,
  377.                                                         final PositionAngleType outAngleType,
  378.                                                         final FieldMatrix<T> inputCov) {

  379.         // Check if type change is really necessary, if not then return input covariance
  380.         if (StateCovariance.inputAndOutputOrbitTypesAreCartesian(inOrbitType, outOrbitType) ||
  381.             StateCovariance.inputAndOutputAreIdentical(inOrbitType, inAngleType, outOrbitType, outAngleType)) {
  382.             return new FieldStateCovariance<>(inputCov, date, covFrame, inOrbitType, inAngleType);
  383.         }

  384.         // Notations:
  385.         // I: Input orbit type
  386.         // O: Output orbit type
  387.         // C: Cartesian parameters

  388.         // Extract field
  389.         final Field<T> field = date.getField();

  390.         // Compute dOutputdCartesian
  391.         final T[][]         aOC               = MathArrays.buildArray(field, STATE_DIMENSION, STATE_DIMENSION);
  392.         final FieldOrbit<T> orbitInOutputType = outOrbitType.convertType(orbit);
  393.         orbitInOutputType.getJacobianWrtCartesian(outAngleType, aOC);
  394.         final FieldMatrix<T> dOdC = new Array2DRowFieldMatrix<>(aOC, false);

  395.         // Compute dCartesiandInput
  396.         final T[][]         aCI              = MathArrays.buildArray(field, STATE_DIMENSION, STATE_DIMENSION);
  397.         final FieldOrbit<T> orbitInInputType = inOrbitType.convertType(orbit);
  398.         orbitInInputType.getJacobianWrtParameters(inAngleType, aCI);
  399.         final FieldMatrix<T> dCdI = new Array2DRowFieldMatrix<>(aCI, false);

  400.         // Compute dOutputdInput
  401.         final FieldMatrix<T> dOdI = dOdC.multiply(dCdI);

  402.         // Conversion of the state covariance matrix in target orbit type
  403.         final FieldMatrix<T> outputCov = dOdI.multiply(inputCov.multiplyTransposed(dOdI));

  404.         // Return the converted covariance
  405.         return new FieldStateCovariance<>(outputCov, date, covFrame, outOrbitType, outAngleType);

  406.     }

  407.     /**
  408.      * Create a covariance matrix from a {@link LOF local orbital frame} to another
  409.      * {@link LOF local orbital frame}.
  410.      * <p>
  411.      * Changing the covariance frame is a linear process, this method does not introduce approximation.
  412.      * <p>
  413.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  414.      * Operations" by David A. Vallado.
  415.      *
  416.      * @param orbit orbit to which the covariance matrix should correspond
  417.      * @param date covariance epoch
  418.      * @param lofIn the local orbital frame in which the input covariance matrix is expressed
  419.      * @param lofOut the target local orbital frame
  420.      * @param inputCartesianCov input covariance {@code CARTESIAN})
  421.      *
  422.      * @return the covariance matrix expressed in the target commonly used local orbital frame in cartesian elements
  423.      */
  424.     private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
  425.                                                          final FieldAbsoluteDate<T> date,
  426.                                                          final LOF lofIn,
  427.                                                          final LOF lofOut,
  428.                                                          final FieldMatrix<T> inputCartesianCov) {

  429.         // Builds the matrix to perform covariance transformation
  430.         final FieldMatrix<T> jacobianFromLofInToLofOut =
  431.                 getJacobian(date.getField(),
  432.                             LOF.transformFromLOFInToLOFOut(lofIn, lofOut, date, orbit.getPVCoordinates()));

  433.         // Get the Cartesian covariance matrix converted to frameOut
  434.         final FieldMatrix<T> cartesianCovarianceOut =
  435.                 jacobianFromLofInToLofOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToLofOut));

  436.         // Output converted covariance
  437.         return new FieldStateCovariance<>(cartesianCovarianceOut, date, lofOut);

  438.     }

  439.     /**
  440.      * Convert the covariance matrix from a {@link Frame frame} to a {@link LOF local orbital frame}.
  441.      * <p>
  442.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  443.      * in covariance orbit type is required.
  444.      * <p>
  445.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  446.      * Operations" by David A. Vallado.
  447.      * <p>
  448.      * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
  449.      * and position angle types of the input covariance must be provided.
  450.      * <p>
  451.      * <b>The output covariance matrix will provided in Cartesian orbit type and not converted back to
  452.      * its original expression (if input different from Cartesian elements).</b>
  453.      *
  454.      * @param orbit orbit to which the covariance matrix should correspond
  455.      * @param date covariance epoch
  456.      * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is <b>not</b>
  457.      * pseudo-inertial, the input covariance matrix is expected to be expressed in <b>Cartesian elements</b>.
  458.      * @param lofOut the target local orbital frame
  459.      * @param inputCov input covariance
  460.      * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
  461.      * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used
  462.      * if covOrbitType equals {@code CARTESIAN})
  463.      * @return the covariance matrix expressed in the target local orbital frame in Cartesian elements
  464.      * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
  465.      * <b>not</b> expressed in Cartesian elements.
  466.      */
  467.     private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
  468.                                                          final FieldAbsoluteDate<T> date,
  469.                                                          final Frame frameIn,
  470.                                                          final LOF lofOut,
  471.                                                          final FieldMatrix<T> inputCov,
  472.                                                          final OrbitType covOrbitType,
  473.                                                          final PositionAngleType covAngleType) {

  474.         // Input frame is inertial
  475.         if (frameIn.isPseudoInertial()) {

  476.             // Convert input matrix to Cartesian parameters in input frame
  477.             final FieldMatrix<T> cartesianCovarianceIn =
  478.                     changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
  479.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN,
  480.                                         inputCov).getMatrix();

  481.             // Builds the matrix to perform covariance transformation
  482.             final FieldMatrix<T> jacobianFromFrameInToLofOut =
  483.                     getJacobian(date.getField(), lofOut.transformFromInertial(date, orbit.getPVCoordinates(frameIn)));

  484.             // Get the Cartesian covariance matrix converted to frameOut
  485.             final FieldMatrix<T> cartesianCovarianceOut =
  486.                     jacobianFromFrameInToLofOut.multiply(
  487.                             cartesianCovarianceIn.multiplyTransposed(jacobianFromFrameInToLofOut));

  488.             // Return converted covariance matrix expressed in cartesian elements
  489.             return new FieldStateCovariance<>(cartesianCovarianceOut, date, lofOut);

  490.         }

  491.         // Input frame is not inertial so the covariance matrix is expected to be in cartesian elements
  492.         else {

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

  495.             // Compute rotation matrix from frameIn to orbit inertial frame
  496.             final FieldMatrix<T> cartesianCovarianceInOrbitFrame =
  497.                     changeFrameAndCreate(orbit, date, frameIn, orbitInertialFrame, inputCov,
  498.                                          OrbitType.CARTESIAN, PositionAngleType.MEAN).getMatrix();

  499.             // Convert from orbit inertial frame to lofOut
  500.             return changeFrameAndCreate(orbit, date, orbitInertialFrame, lofOut, cartesianCovarianceInOrbitFrame,
  501.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN);

  502.         }

  503.     }

  504.     /**
  505.      * Convert the covariance matrix from a {@link LOF  local orbital frame} to a {@link Frame frame}.
  506.      * <p>
  507.      * Changing the covariance frame is a linear process, this method does not introduce approximation.
  508.      * <p>
  509.      * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
  510.      * Operations" by David A. Vallado.
  511.      * <p>
  512.      * The <u>input</u> covariance matrix is necessarily provided in <b>Cartesian orbit type</b>.
  513.      * <p>
  514.      * The <u>output</u> covariance matrix will be expressed in <b>Cartesian elements</b>.
  515.      *
  516.      * @param orbit orbit to which the covariance matrix should correspond
  517.      * @param date covariance epoch
  518.      * @param lofIn the local orbital frame in which the input covariance matrix is expressed
  519.      * @param frameOut the target frame
  520.      * @param inputCartesianCov input covariance ({@code CARTESIAN})
  521.      *
  522.      * @return the covariance matrix expressed in the target frame in cartesian elements
  523.      */
  524.     private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
  525.                                                          final FieldAbsoluteDate<T> date,
  526.                                                          final LOF lofIn,
  527.                                                          final Frame frameOut,
  528.                                                          final FieldMatrix<T> inputCartesianCov) {

  529.         // Output frame is pseudo-inertial
  530.         if (frameOut.isPseudoInertial()) {

  531.             // Builds the matrix to perform covariance transformation
  532.             final FieldMatrix<T> jacobianFromLofInToFrameOut =
  533.                     getJacobian(date.getField(),
  534.                                 lofIn.transformFromInertial(date, orbit.getPVCoordinates(frameOut)).getInverse());

  535.             // Transform covariance
  536.             final FieldMatrix<T> transformedCovariance =
  537.                     jacobianFromLofInToFrameOut.multiply(
  538.                             inputCartesianCov.multiplyTransposed(jacobianFromLofInToFrameOut));

  539.             // Get the Cartesian covariance matrix converted to frameOut
  540.             return new FieldStateCovariance<>(transformedCovariance, date, frameOut, OrbitType.CARTESIAN,
  541.                                               DEFAULT_POSITION_ANGLE);

  542.         }

  543.         // Output frame is not pseudo-inertial
  544.         else {

  545.             // Builds the matrix to perform covariance transformation
  546.             final FieldMatrix<T> jacobianFromLofInToOrbitFrame =
  547.                     getJacobian(date.getField(),
  548.                                 lofIn.transformFromInertial(date, orbit.getPVCoordinates()).getInverse());

  549.             // Get the Cartesian covariance matrix converted to orbit inertial frame
  550.             final FieldMatrix<T> cartesianCovarianceInOrbitFrame = jacobianFromLofInToOrbitFrame.multiply(
  551.                     inputCartesianCov.multiplyTransposed(jacobianFromLofInToOrbitFrame));

  552.             // Get the Cartesian covariance matrix converted to frameOut
  553.             return changeFrameAndCreate(orbit, date, orbit.getFrame(), frameOut, cartesianCovarianceInOrbitFrame,
  554.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN);
  555.         }

  556.     }

  557.     /**
  558.      * Get the covariance matrix in another frame.
  559.      * <p>
  560.      * The transformation is based on Equation (18) of "Covariance Transformations for Satellite Flight Dynamics
  561.      * Operations" by David A. Vallado.
  562.      * <p>
  563.      * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
  564.      * in covariance orbit type is required.
  565.      * <p>
  566.      * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
  567.      * and position angle types of the input covariance must be provided.
  568.      * <p>
  569.      * In case the <u>input</u> frame is <b>not</b> pseudo-inertial, the <u>input</u> covariance matrix is necessarily
  570.      * expressed in <b>Cartesian elements</b>.
  571.      * <p>
  572.      * In case the <u>output</u> frame is <b>not</b> pseudo-inertial, the <u>output</u> covariance matrix will be
  573.      * expressed in <b>Cartesian elements</b>.
  574.      *
  575.      * @param orbit orbit to which the covariance matrix should correspond
  576.      * @param date covariance epoch
  577.      * @param frameIn the frame in which the input covariance matrix is expressed
  578.      * @param frameOut the target frame
  579.      * @param inputCov input covariance
  580.      * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
  581.      * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (<b>not</b>
  582.      * used if covOrbitType equals {@code CARTESIAN})
  583.      * @return the covariance matrix expressed in the target frame
  584.      *
  585.      * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
  586.      * <b>not</b> expressed in cartesian elements.
  587.      */
  588.     private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
  589.                                                          final FieldAbsoluteDate<T> date,
  590.                                                          final Frame frameIn,
  591.                                                          final Frame frameOut,
  592.                                                          final FieldMatrix<T> inputCov,
  593.                                                          final OrbitType covOrbitType,
  594.                                                          final PositionAngleType covAngleType) {

  595.         // Get the transform from the covariance frame to the output frame
  596.         final FieldTransform<T> inToOut = frameIn.getTransformTo(frameOut, orbit.getDate());

  597.         // Matrix to perform the covariance transformation
  598.         final FieldMatrix<T> j = getJacobian(date.getField(), inToOut);

  599.         // Input frame pseudo-inertial
  600.         if (frameIn.isPseudoInertial()) {

  601.             // Convert input matrix to Cartesian parameters in input frame
  602.             final FieldMatrix<T> cartesianCovarianceIn =
  603.                     changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
  604.                                         OrbitType.CARTESIAN, PositionAngleType.MEAN,
  605.                                         inputCov).getMatrix();

  606.             // Get the Cartesian covariance matrix converted to frameOut
  607.             final FieldMatrix<T> cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j));

  608.             // Output frame is pseudo-inertial
  609.             if (frameOut.isPseudoInertial()) {

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

  613.             }

  614.             // Output frame is not pseudo-inertial
  615.             else {

  616.                 // Output Cartesian matrix
  617.                 return new FieldStateCovariance<>(cartesianCovarianceOut, date, frameOut, OrbitType.CARTESIAN,
  618.                                                   DEFAULT_POSITION_ANGLE);

  619.             }
  620.         }

  621.         // Input frame is not pseudo-inertial
  622.         else {

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

  624.             // Convert covariance matrix to frameOut
  625.             final FieldMatrix<T> covInFrameOut = j.multiply(inputCov.multiplyTransposed(j));

  626.             // Output the Cartesian covariance matrix converted to frameOut
  627.             return new FieldStateCovariance<>(covInFrameOut, date, frameOut, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);

  628.         }

  629.     }

  630.     /**
  631.      * Builds the matrix to perform covariance frame transformation.
  632.      *
  633.      * @param field to which the elements belong
  634.      * @param transform input transformation
  635.      *
  636.      * @return the matrix to perform the covariance frame transformation
  637.      */
  638.     private FieldMatrix<T> getJacobian(final Field<T> field, final FieldTransform<T> transform) {
  639.         // Get the Jacobian of the transform
  640.         final T[][] jacobian = MathArrays.buildArray(field, STATE_DIMENSION, STATE_DIMENSION);
  641.         transform.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
  642.         // Return
  643.         return new Array2DRowFieldMatrix<>(jacobian, false);

  644.     }

  645.     /**
  646.      * Get the state transition matrix considering Keplerian contribution only.
  647.      *
  648.      * @param field to which the elements belong
  649.      * @param initialOrbit orbit to which the initial covariance matrix should correspond
  650.      * @param dt time difference between the two orbits
  651.      *
  652.      * @return the state transition matrix used to shift the covariance matrix
  653.      */
  654.     private FieldMatrix<T> getStm(final Field<T> field, final FieldOrbit<T> initialOrbit, final T dt) {

  655.         // initialize the STM
  656.         final FieldMatrix<T> stm = MatrixUtils.createFieldIdentityMatrix(field, STATE_DIMENSION);

  657.         // State transition matrix using Keplerian contribution only
  658.         final T mu           = initialOrbit.getMu();
  659.         final T sma          = initialOrbit.getA();
  660.         final T contribution = mu.divide(sma.pow(5)).sqrt().multiply(dt).multiply(-1.5);
  661.         stm.setEntry(5, 0, contribution);

  662.         // Return
  663.         return stm;

  664.     }

  665. }