FieldStateCovariance.java

/* Copyright 2002-2025 CS GROUP
 * Licensed to CS GROUP (CS) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
 * this work for additional information regarding copyright ownership.
 * CS licenses this file to You under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with
 * the License.  You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.orekit.propagation;

import org.hipparchus.CalculusFieldElement;
import org.hipparchus.Field;
import org.hipparchus.linear.Array2DRowFieldMatrix;
import org.hipparchus.linear.BlockRealMatrix;
import org.hipparchus.linear.FieldMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.util.MathArrays;
import org.orekit.errors.OrekitException;
import org.orekit.errors.OrekitMessages;
import org.orekit.frames.FieldKinematicTransform;
import org.orekit.frames.Frame;
import org.orekit.frames.LOF;
import org.orekit.orbits.FieldOrbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleType;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.FieldAbsoluteDate;
import org.orekit.time.FieldTimeStamped;

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

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

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

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

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

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

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

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

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

    /**
     * Constructor.
     *
     * @param orbitalCovariance 6x6 orbital parameters covariance
     * @param epoch epoch of the covariance
     * @param lof covariance LOF type
     */
    public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
                                final LOF lof) {
        this(orbitalCovariance, epoch, null, lof, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
    }

    /**
     * Constructor.
     *
     * @param orbitalCovariance 6x6 orbital parameters covariance
     * @param epoch epoch of the covariance
     * @param covarianceFrame covariance frame (inertial or Earth fixed)
     * @param orbitType orbit type of the covariance (CARTESIAN if covarianceFrame is not pseudo-inertial)
     * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
     */
    public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
                                final Frame covarianceFrame,
                                final OrbitType orbitType, final PositionAngleType angleType) {
        this(orbitalCovariance, epoch, covarianceFrame, null, orbitType, angleType);
    }

    /**
     * Private constructor.
     *
     * @param orbitalCovariance 6x6 orbital parameters covariance
     * @param epoch epoch of the covariance
     * @param covarianceFrame covariance frame (inertial or Earth fixed)
     * @param lof covariance LOF type
     * @param orbitType orbit type of the covariance
     * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
     */
    private FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
                                 final Frame covarianceFrame, final LOF lof,
                                 final OrbitType orbitType, final PositionAngleType angleType) {

        StateCovariance.checkFrameAndOrbitTypeConsistency(covarianceFrame, orbitType);

        this.orbitalCovariance = orbitalCovariance;
        this.epoch             = epoch;
        this.frame             = covarianceFrame;
        this.lof               = lof;
        this.orbitType         = orbitType;
        this.angleType         = angleType;

    }

    /** {@inheritDoc}. */
    @Override
    public FieldAbsoluteDate<T> getDate() {
        return epoch;
    }

    /**
     * Get the covariance matrix.
     *
     * @return the covariance matrix
     */
    public FieldMatrix<T> getMatrix() {
        return orbitalCovariance;
    }

    /**
     * Get the covariance orbit type.
     *
     * @return the covariance orbit type
     */
    public OrbitType getOrbitType() {
        return orbitType;
    }

    /**
     * Get the covariance angle type.
     *
     * @return the covariance angle type
     */
    public PositionAngleType getPositionAngleType() {
        return angleType;
    }

    /**
     * Get the covariance frame.
     *
     * @return the covariance frame (can be null)
     *
     * @see #getLOF()
     */
    public Frame getFrame() {
        return frame;
    }

    /**
     * Get the covariance LOF type.
     *
     * @return the covariance LOF type (can be null)
     *
     * @see #getFrame()
     */
    public LOF getLOF() {
        return lof;
    }

    /**
     * Get the covariance matrix in another orbit type.
     * <p>
     * The covariance orbit type <b>cannot</b> be changed if the covariance
     * matrix is expressed in a {@link LOF local orbital frame} or a
     * non-pseudo inertial frame.
     * <p>
     * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
     * Hence, the current covariance matrix <b>will not exactly match</b> the new linearized case and the
     * distribution will not follow a generalized Gaussian distribution anymore.
     * <p>
     * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
     * dynamics operations."
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param outOrbitType target orbit type of the state covariance matrix
     * @param outAngleType target position angle type of the state covariance matrix
     *
     * @return a new covariance state, expressed in the target orbit type with the target position angle
     *
     * @see #changeCovarianceFrame(FieldOrbit, Frame)
     */
    public FieldStateCovariance<T> changeCovarianceType(final FieldOrbit<T> orbit, final OrbitType outOrbitType,
                                                        final PositionAngleType outAngleType) {

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

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

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

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

        }

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

    }

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

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

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

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

        } else {

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

        }

    }

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

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

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

        } else {

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

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

        }

    }

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    }

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

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

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

        // Return new state covariance according to current state covariance definition
        if (frame == null) {
            return new StateCovariance(realMatrix, realDate, lof);
        }
        else {
            return new StateCovariance(realMatrix, realDate, frame, orbitType, angleType);
        }
    }

    /**
     * Create a covariance matrix in another orbit type.
     * <p>
     * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
     * Hence, the input covariance matrix <b>will not exactly match</b> the new linearized case and the
     * distribution will not follow a generalized Gaussian distribution anymore.
     * <p>
     * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
     * dynamics operations."
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param date covariance epoch
     * @param covFrame covariance frame
     * @param inOrbitType initial orbit type of the state covariance matrix
     * @param inAngleType initial position angle type of the state covariance matrix
     * @param outOrbitType target orbit type of the state covariance matrix
     * @param outAngleType target position angle type of the state covariance matrix
     * @param inputCov input covariance
     *
     * @return the covariance expressed in the target orbit type with the target position angle
     */
    private FieldStateCovariance<T> changeTypeAndCreate(final FieldOrbit<T> orbit,
                                                        final FieldAbsoluteDate<T> date,
                                                        final Frame covFrame,
                                                        final OrbitType inOrbitType,
                                                        final PositionAngleType inAngleType,
                                                        final OrbitType outOrbitType,
                                                        final PositionAngleType outAngleType,
                                                        final FieldMatrix<T> inputCov) {

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

        // Notations:
        // I: Input orbit type
        // O: Output orbit type
        // C: Cartesian parameters

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

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

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

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

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

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

    }

    /**
     * Create a covariance matrix from a {@link LOF local orbital frame} to another
     * {@link LOF local orbital frame}.
     * <p>
     * Changing the covariance frame is a linear process, this method does not introduce approximation.
     * <p>
     * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
     * Operations" by David A. Vallado.
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param date covariance epoch
     * @param lofIn the local orbital frame in which the input covariance matrix is expressed
     * @param lofOut the target local orbital frame
     * @param inputCartesianCov input covariance {@code CARTESIAN})
     *
     * @return the covariance matrix expressed in the target commonly used local orbital frame in cartesian elements
     */
    private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
                                                         final FieldAbsoluteDate<T> date,
                                                         final LOF lofIn,
                                                         final LOF lofOut,
                                                         final FieldMatrix<T> inputCartesianCov) {

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

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

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

    }

    /**
     * Convert the covariance matrix from a {@link Frame frame} to a {@link LOF local orbital frame}.
     * <p>
     * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
     * in covariance orbit type is required.
     * <p>
     * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
     * Operations" by David A. Vallado.
     * <p>
     * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
     * and position angle types of the input covariance must be provided.
     * <p>
     * <b>The output covariance matrix will provided in Cartesian orbit type and not converted back to
     * its original expression (if input different from Cartesian elements).</b>
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param date covariance epoch
     * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is <b>not</b>
     * pseudo-inertial, the input covariance matrix is expected to be expressed in <b>Cartesian elements</b>.
     * @param lofOut the target local orbital frame
     * @param inputCov input covariance
     * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
     * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used
     * if covOrbitType equals {@code CARTESIAN})
     * @return the covariance matrix expressed in the target local orbital frame in Cartesian elements
     * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
     * <b>not</b> expressed in Cartesian elements.
     */
    private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
                                                         final FieldAbsoluteDate<T> date,
                                                         final Frame frameIn,
                                                         final LOF lofOut,
                                                         final FieldMatrix<T> inputCov,
                                                         final OrbitType covOrbitType,
                                                         final PositionAngleType covAngleType) {

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

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

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

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

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

        }

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

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

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

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

        }

    }

    /**
     * Convert the covariance matrix from a {@link LOF  local orbital frame} to a {@link Frame frame}.
     * <p>
     * Changing the covariance frame is a linear process, this method does not introduce approximation.
     * <p>
     * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
     * Operations" by David A. Vallado.
     * <p>
     * The <u>input</u> covariance matrix is necessarily provided in <b>Cartesian orbit type</b>.
     * <p>
     * The <u>output</u> covariance matrix will be expressed in <b>Cartesian elements</b>.
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param date covariance epoch
     * @param lofIn the local orbital frame in which the input covariance matrix is expressed
     * @param frameOut the target frame
     * @param inputCartesianCov input covariance ({@code CARTESIAN})
     *
     * @return the covariance matrix expressed in the target frame in cartesian elements
     */
    private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
                                                         final FieldAbsoluteDate<T> date,
                                                         final LOF lofIn,
                                                         final Frame frameOut,
                                                         final FieldMatrix<T> inputCartesianCov) {

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

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

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

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

        }

        // Output frame is not pseudo-inertial
        else {

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

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

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

    }

    /**
     * Get the covariance matrix in another frame.
     * <p>
     * The transformation is based on Equation (18) of "Covariance Transformations for Satellite Flight Dynamics
     * Operations" by David A. Vallado.
     * <p>
     * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
     * in covariance orbit type is required.
     * <p>
     * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
     * and position angle types of the input covariance must be provided.
     * <p>
     * In case the <u>input</u> frame is <b>not</b> pseudo-inertial, the <u>input</u> covariance matrix is necessarily
     * expressed in <b>Cartesian elements</b>.
     * <p>
     * In case the <u>output</u> frame is <b>not</b> pseudo-inertial, the <u>output</u> covariance matrix will be
     * expressed in <b>Cartesian elements</b>.
     *
     * @param orbit orbit to which the covariance matrix should correspond
     * @param date covariance epoch
     * @param frameIn the frame in which the input covariance matrix is expressed
     * @param frameOut the target frame
     * @param inputCov input covariance
     * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
     * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (<b>not</b>
     * used if covOrbitType equals {@code CARTESIAN})
     * @return the covariance matrix expressed in the target frame
     *
     * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
     * <b>not</b> expressed in cartesian elements.
     */
    private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
                                                         final FieldAbsoluteDate<T> date,
                                                         final Frame frameIn,
                                                         final Frame frameOut,
                                                         final FieldMatrix<T> inputCov,
                                                         final OrbitType covOrbitType,
                                                         final PositionAngleType covAngleType) {

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

        // Matrix to perform the covariance transformation
        final FieldMatrix<T> j = getJacobian(inToOut);

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

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

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

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

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

            }

            // Output frame is not pseudo-inertial
            else {

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

            }
        }

        // Input frame is not pseudo-inertial
        else {

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

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

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

        }

    }

    /**
     * Builds the matrix to perform covariance frame transformation.
     *
     * @param transform input transformation
     *
     * @return the matrix to perform the covariance frame transformation
     */
    private FieldMatrix<T> getJacobian(final FieldKinematicTransform<T> transform) {
        return MatrixUtils.createFieldMatrix(transform.getPVJacobian());
    }

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

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

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

        // Return
        return stm;

    }

}