StateCovarianceMatrixProvider.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.MatrixUtils;
  19. import org.hipparchus.linear.RealMatrix;
  20. import org.orekit.frames.Frame;
  21. import org.orekit.orbits.Orbit;
  22. import org.orekit.orbits.OrbitType;
  23. import org.orekit.orbits.PositionAngleType;
  24. import org.orekit.time.AbsoluteDate;

  25. /**
  26.  * Additional state provider for state covariance matrix.
  27.  * <p>
  28.  * This additional state provider allows computing a propagated covariance matrix based on a user defined input state
  29.  * covariance matrix. The computation of the propagated covariance matrix uses the State Transition Matrix between the
  30.  * propagated spacecraft state and the initial state. As a result, the user must define the name
  31.  * {@link #stmName of the provider for the State Transition Matrix}.
  32.  * <p>
  33.  * As the State Transition Matrix and the input state covariance matrix can be expressed in different orbit types, the
  34.  * user must specify both orbit types when building the covariance provider. In addition, the position angle used in
  35.  * both matrices must also be specified.
  36.  * <p>
  37.  * In order to add this additional state provider to an orbit propagator, user must use the
  38.  * {@link Propagator#addAdditionalStateProvider(AdditionalStateProvider)} method.
  39.  * <p>
  40.  * For a given propagated spacecraft {@code state}, the propagated state covariance matrix is accessible through the
  41.  * method {@link #getStateCovariance(SpacecraftState)}
  42.  *
  43.  * @author Bryan Cazabonne
  44.  * @author Vincent Cucchietti
  45.  * @since 11.3
  46.  */
  47. public class StateCovarianceMatrixProvider implements AdditionalStateProvider {

  48.     /** Dimension of the state. */
  49.     private static final int STATE_DIMENSION = 6;

  50.     /** Name of the state for State Transition Matrix. */
  51.     private final String stmName;

  52.     /** Matrix harvester to access the State Transition Matrix. */
  53.     private final MatricesHarvester harvester;

  54.     /** Name of the additional state. */
  55.     private final String additionalName;

  56.     /** Orbit type used for the State Transition Matrix. */
  57.     private final OrbitType stmOrbitType;

  58.     /** Position angle used for State Transition Matrix. */
  59.     private final PositionAngleType stmAngleType;

  60.     /** Orbit type for the covariance matrix. */
  61.     private final OrbitType covOrbitType;

  62.     /** Position angle used for the covariance matrix. */
  63.     private final PositionAngleType covAngleType;

  64.     /** Initial state covariance. */
  65.     private StateCovariance covInit;

  66.     /** Initial state covariance matrix. */
  67.     private RealMatrix covMatrixInit;

  68.     /**
  69.      * Constructor.
  70.      *
  71.      * @param additionalName name of the additional state
  72.      * @param stmName name of the state for State Transition Matrix
  73.      * @param harvester matrix harvester as returned by
  74.      * {@code propagator.setupMatricesComputation(stmName, null, null)}
  75.      * @param covInit initial state covariance
  76.      */
  77.     public StateCovarianceMatrixProvider(final String additionalName, final String stmName,
  78.                                          final MatricesHarvester harvester, final StateCovariance covInit) {
  79.         // Initialize fields
  80.         this.additionalName = additionalName;
  81.         this.stmName = stmName;
  82.         this.harvester = harvester;
  83.         this.covInit = covInit;
  84.         this.covOrbitType = covInit.getOrbitType();
  85.         this.covAngleType = covInit.getPositionAngleType();
  86.         this.stmOrbitType = harvester.getOrbitType();
  87.         this.stmAngleType = harvester.getPositionAngleType();
  88.     }

  89.     /** {@inheritDoc} */
  90.     @Override
  91.     public String getName() {
  92.         return additionalName;
  93.     }

  94.     /** {@inheritDoc} */
  95.     @Override
  96.     public void init(final SpacecraftState initialState, final AbsoluteDate target) {
  97.         // Convert the initial state covariance in the same orbit type as the STM
  98.         covInit = covInit.changeCovarianceType(initialState.getOrbit(), stmOrbitType, stmAngleType);

  99.         // Express covariance matrix in the same frame as the STM
  100.         final Orbit           initialOrbit      = initialState.getOrbit();
  101.         final StateCovariance covInitInSTMFrame = covInit.changeCovarianceFrame(initialOrbit, initialOrbit.getFrame());

  102.         covMatrixInit = covInitInSTMFrame.getMatrix();
  103.     }

  104.     /**
  105.      * {@inheritDoc}
  106.      * <p>
  107.      * The covariance matrix can be computed only if the State Transition Matrix state is available.
  108.      * </p>
  109.      */
  110.     @Override
  111.     public boolean yields(final SpacecraftState state) {
  112.         return !state.hasAdditionalState(stmName);
  113.     }

  114.     /** {@inheritDoc} */
  115.     @Override
  116.     public double[] getAdditionalState(final SpacecraftState state) {

  117.         // State transition matrix for the input state
  118.         final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);

  119.         // Compute the propagated covariance matrix
  120.         RealMatrix propCov = dYdY0.multiply(covMatrixInit.multiplyTransposed(dYdY0));
  121.         final StateCovariance propagated = new StateCovariance(propCov, state.getDate(), state.getFrame(), stmOrbitType, stmAngleType);

  122.         // Update to the user defined type
  123.         propCov = propagated.changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType).getMatrix();

  124.         // Return the propagated covariance matrix
  125.         return toArray(propCov);

  126.     }

  127.     /**
  128.      * Get the orbit type in which the covariance matrix is expressed.
  129.      *
  130.      * @return the orbit type
  131.      */
  132.     public OrbitType getCovarianceOrbitType() {
  133.         return covOrbitType;
  134.     }

  135.     /**
  136.      * Get the state covariance in the same frame/local orbital frame, orbit type and position angle as the initial
  137.      * covariance.
  138.      *
  139.      * @param state spacecraft state to which the covariance matrix should correspond
  140.      * @return the state covariance
  141.      * @see #getStateCovariance(SpacecraftState, Frame)
  142.      * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
  143.      */
  144.     public StateCovariance getStateCovariance(final SpacecraftState state) {

  145.         // Get the current propagated covariance
  146.         final RealMatrix covarianceMatrix = toRealMatrix(getAdditionalState(state));

  147.         // Create associated state covariance
  148.         final StateCovariance covariance =
  149.                 new StateCovariance(covarianceMatrix, state.getDate(), state.getFrame(), covOrbitType, covAngleType);

  150.         // Return the state covariance in same frame/lof as initial covariance
  151.         if (covInit.getLOF() == null) {
  152.             return covariance;
  153.         }
  154.         else {
  155.             return covariance.changeCovarianceFrame(state.getOrbit(), covInit.getLOF());
  156.         }

  157.     }

  158.     /**
  159.      * Get the state covariance expressed in a given frame.
  160.      * <p>
  161.      * The output covariance matrix is expressed in the same orbit type as {@link #getCovarianceOrbitType()}.
  162.      *
  163.      * @param state spacecraft state to which the covariance matrix should correspond
  164.      * @param frame output frame for which the output covariance matrix must be expressed (must be inertial)
  165.      * @return the state covariance expressed in <code>frame</code>
  166.      * @see #getStateCovariance(SpacecraftState)
  167.      * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
  168.      */
  169.     public StateCovariance getStateCovariance(final SpacecraftState state, final Frame frame) {
  170.         // Return the converted covariance
  171.         return getStateCovariance(state).changeCovarianceFrame(state.getOrbit(), frame);
  172.     }

  173.     /**
  174.      * Get the state covariance expressed in a given orbit type.
  175.      *
  176.      * @param state spacecraft state to which the covariance matrix should correspond
  177.      * @param orbitType output orbit type
  178.      * @param angleType output position angle (not used if orbitType equals {@code CARTESIAN})
  179.      * @return the state covariance in <code>orbitType</code> and <code>angleType</code>
  180.      * @see #getStateCovariance(SpacecraftState)
  181.      * @see #getStateCovariance(SpacecraftState, Frame)
  182.      */
  183.     public StateCovariance getStateCovariance(final SpacecraftState state, final OrbitType orbitType,
  184.                                               final PositionAngleType angleType) {
  185.         // Return the converted covariance
  186.         return getStateCovariance(state).changeCovarianceType(state.getOrbit(), orbitType, angleType);
  187.     }

  188.     /**
  189.      * Set the covariance data into an array.
  190.      *
  191.      * @param covariance covariance matrix
  192.      * @return an array containing the covariance data
  193.      */
  194.     private double[] toArray(final RealMatrix covariance) {
  195.         final double[] array = new double[STATE_DIMENSION * STATE_DIMENSION];
  196.         int            index = 0;
  197.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  198.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  199.                 array[index++] = covariance.getEntry(i, j);
  200.             }
  201.         }
  202.         return array;
  203.     }

  204.     /**
  205.      * Convert an array to a matrix (6x6 dimension).
  206.      *
  207.      * @param array input array
  208.      * @return the corresponding matrix
  209.      */
  210.     private RealMatrix toRealMatrix(final double[] array) {
  211.         final RealMatrix matrix = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
  212.         int              index  = 0;
  213.         for (int i = 0; i < STATE_DIMENSION; ++i) {
  214.             for (int j = 0; j < STATE_DIMENSION; ++j) {
  215.                 matrix.setEntry(i, j, array[index++]);
  216.             }
  217.         }
  218.         return matrix;

  219.     }

  220. }