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  
19  import org.hipparchus.linear.MatrixUtils;
20  import org.hipparchus.linear.RealMatrix;
21  import org.orekit.frames.Frame;
22  import org.orekit.orbits.Orbit;
23  import org.orekit.orbits.OrbitType;
24  import org.orekit.orbits.PositionAngleType;
25  import org.orekit.time.AbsoluteDate;
26  
27  /**
28   * Additional state provider for state covariance matrix.
29   * <p>
30   * This additional state provider allows computing a propagated covariance matrix based on a user defined input state
31   * covariance matrix. The computation of the propagated covariance matrix uses the State Transition Matrix between the
32   * propagated spacecraft state and the initial state. As a result, the user must define the name
33   * {@link #stmName of the provider for the State Transition Matrix}.
34   * <p>
35   * As the State Transition Matrix and the input state covariance matrix can be expressed in different orbit types, the
36   * user must specify both orbit types when building the covariance provider. In addition, the position angle used in
37   * both matrices must also be specified.
38   * <p>
39   * In order to add this additional state provider to an orbit propagator, user must use the
40   * {@link Propagator#addAdditionalStateProvider(AdditionalStateProvider)} method.
41   * <p>
42   * For a given propagated spacecraft {@code state}, the propagated state covariance matrix is accessible through the
43   * method {@link #getStateCovariance(SpacecraftState)}
44   *
45   * @author Bryan Cazabonne
46   * @author Vincent Cucchietti
47   * @since 11.3
48   */
49  public class StateCovarianceMatrixProvider implements AdditionalStateProvider {
50  
51      /** Dimension of the state. */
52      private static final int STATE_DIMENSION = 6;
53  
54      /** Name of the state for State Transition Matrix. */
55      private final String stmName;
56  
57      /** Matrix harvester to access the State Transition Matrix. */
58      private final MatricesHarvester harvester;
59  
60      /** Name of the additional state. */
61      private final String additionalName;
62  
63      /** Orbit type used for the State Transition Matrix. */
64      private final OrbitType stmOrbitType;
65  
66      /** Position angle used for State Transition Matrix. */
67      private final PositionAngleType stmAngleType;
68  
69      /** Orbit type for the covariance matrix. */
70      private final OrbitType covOrbitType;
71  
72      /** Position angle used for the covariance matrix. */
73      private final PositionAngleType covAngleType;
74  
75      /** Initial state covariance. */
76      private StateCovariance covInit;
77  
78      /** Initial state covariance matrix. */
79      private RealMatrix covMatrixInit;
80  
81      /**
82       * Constructor.
83       *
84       * @param additionalName name of the additional state
85       * @param stmName name of the state for State Transition Matrix
86       * @param harvester matrix harvester as returned by
87       * {@code propagator.setupMatricesComputation(stmName, null, null)}
88       * @param covInit initial state covariance
89       */
90      public StateCovarianceMatrixProvider(final String additionalName, final String stmName,
91                                           final MatricesHarvester harvester, final StateCovariance covInit) {
92          // Initialize fields
93          this.additionalName = additionalName;
94          this.stmName = stmName;
95          this.harvester = harvester;
96          this.covInit = covInit;
97          this.covOrbitType = covInit.getOrbitType();
98          this.covAngleType = covInit.getPositionAngleType();
99          this.stmOrbitType = harvester.getOrbitType();
100         this.stmAngleType = harvester.getPositionAngleType();
101     }
102 
103     /** {@inheritDoc} */
104     @Override
105     public String getName() {
106         return additionalName;
107     }
108 
109     /** {@inheritDoc} */
110     @Override
111     public void init(final SpacecraftState initialState, final AbsoluteDate target) {
112         // Convert the initial state covariance in the same orbit type as the STM
113         covInit = covInit.changeCovarianceType(initialState.getOrbit(), stmOrbitType, stmAngleType);
114 
115         // Express covariance matrix in the same frame as the STM
116         final Orbit           initialOrbit      = initialState.getOrbit();
117         final StateCovariance covInitInSTMFrame = covInit.changeCovarianceFrame(initialOrbit, initialOrbit.getFrame());
118 
119         covMatrixInit = covInitInSTMFrame.getMatrix();
120     }
121 
122     /**
123      * {@inheritDoc}
124      * <p>
125      * The covariance matrix can be computed only if the State Transition Matrix state is available.
126      * </p>
127      */
128     @Override
129     public boolean yields(final SpacecraftState state) {
130         return !state.hasAdditionalState(stmName);
131     }
132 
133     /** {@inheritDoc} */
134     @Override
135     public double[] getAdditionalState(final SpacecraftState state) {
136 
137         // State transition matrix for the input state
138         final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);
139 
140         // Compute the propagated covariance matrix
141         RealMatrix propCov = dYdY0.multiply(covMatrixInit.multiplyTransposed(dYdY0));
142         final StateCovariance propagated = new StateCovariance(propCov, state.getDate(), state.getFrame(), stmOrbitType, stmAngleType);
143 
144         // Update to the user defined type
145         propCov = propagated.changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType).getMatrix();
146 
147         // Return the propagated covariance matrix
148         return toArray(propCov);
149 
150     }
151 
152     /**
153      * Get the orbit type in which the covariance matrix is expressed.
154      *
155      * @return the orbit type
156      */
157     public OrbitType getCovarianceOrbitType() {
158         return covOrbitType;
159     }
160 
161     /**
162      * Get the state covariance in the same frame/local orbital frame, orbit type and position angle as the initial
163      * covariance.
164      *
165      * @param state spacecraft state to which the covariance matrix should correspond
166      * @return the state covariance
167      * @see #getStateCovariance(SpacecraftState, Frame)
168      * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
169      */
170     public StateCovariance getStateCovariance(final SpacecraftState state) {
171 
172         // Get the current propagated covariance
173         final RealMatrix covarianceMatrix = toRealMatrix(getAdditionalState(state));
174 
175         // Create associated state covariance
176         final StateCovariance covariance =
177                 new StateCovariance(covarianceMatrix, state.getDate(), state.getFrame(), covOrbitType, covAngleType);
178 
179         // Return the state covariance in same frame/lof as initial covariance
180         if (covInit.getLOF() == null) {
181             return covariance;
182         }
183         else {
184             return covariance.changeCovarianceFrame(state.getOrbit(), covInit.getLOF());
185         }
186 
187     }
188 
189     /**
190      * Get the state covariance expressed in a given frame.
191      * <p>
192      * The output covariance matrix is expressed in the same orbit type as {@link #getCovarianceOrbitType()}.
193      *
194      * @param state spacecraft state to which the covariance matrix should correspond
195      * @param frame output frame for which the output covariance matrix must be expressed (must be inertial)
196      * @return the state covariance expressed in <code>frame</code>
197      * @see #getStateCovariance(SpacecraftState)
198      * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
199      */
200     public StateCovariance getStateCovariance(final SpacecraftState state, final Frame frame) {
201         // Return the converted covariance
202         return getStateCovariance(state).changeCovarianceFrame(state.getOrbit(), frame);
203     }
204 
205     /**
206      * Get the state covariance expressed in a given orbit type.
207      *
208      * @param state spacecraft state to which the covariance matrix should correspond
209      * @param orbitType output orbit type
210      * @param angleType output position angle (not used if orbitType equals {@code CARTESIAN})
211      * @return the state covariance in <code>orbitType</code> and <code>angleType</code>
212      * @see #getStateCovariance(SpacecraftState)
213      * @see #getStateCovariance(SpacecraftState, Frame)
214      */
215     public StateCovariance getStateCovariance(final SpacecraftState state, final OrbitType orbitType,
216                                               final PositionAngleType angleType) {
217         // Return the converted covariance
218         return getStateCovariance(state).changeCovarianceType(state.getOrbit(), orbitType, angleType);
219     }
220 
221     /**
222      * Set the covariance data into an array.
223      *
224      * @param covariance covariance matrix
225      * @return an array containing the covariance data
226      */
227     private double[] toArray(final RealMatrix covariance) {
228         final double[] array = new double[STATE_DIMENSION * STATE_DIMENSION];
229         int            index = 0;
230         for (int i = 0; i < STATE_DIMENSION; ++i) {
231             for (int j = 0; j < STATE_DIMENSION; ++j) {
232                 array[index++] = covariance.getEntry(i, j);
233             }
234         }
235         return array;
236     }
237 
238     /**
239      * Convert an array to a matrix (6x6 dimension).
240      *
241      * @param array input array
242      * @return the corresponding matrix
243      */
244     private RealMatrix toRealMatrix(final double[] array) {
245         final RealMatrix matrix = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
246         int              index  = 0;
247         for (int i = 0; i < STATE_DIMENSION; ++i) {
248             for (int j = 0; j < STATE_DIMENSION; ++j) {
249                 matrix.setEntry(i, j, array[index++]);
250             }
251         }
252         return matrix;
253 
254     }
255 
256 }