1   /* Copyright 2002-2025 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.estimation.sequential;
18  
19  import org.hipparchus.analysis.UnivariateFunction;
20  import org.hipparchus.linear.MatrixUtils;
21  import org.hipparchus.linear.RealMatrix;
22  import org.orekit.frames.LOFType;
23  import org.orekit.orbits.PositionAngleType;
24  import org.orekit.propagation.SpacecraftState;
25  import org.orekit.propagation.StateCovariance;
26  
27  /** Provider for a temporal evolution of the process noise matrix.
28   * All parameters (orbital or propagation) are time dependent and provided as {@link UnivariateFunction}.
29   * The argument of the functions is a duration in seconds (between current and previous spacecraft state).
30   * The output of the functions must be of the dimension of a standard deviation.
31   * The method {@link #getProcessNoiseMatrix} then square the values so that they are consistent with a covariance matrix.
32   * <p>
33   * The orbital parameters evolutions are provided in LOF frame and Cartesian (PV);
34   * then converted in inertial frame and current {@link org.orekit.orbits.OrbitType} and {@link PositionAngleType}
35   * when method {@link #getProcessNoiseMatrix} is called.
36   * </p>
37   * <p>
38   * The time-dependent functions define a process noise matrix that is diagonal
39   * <em>in the Local Orbital Frame</em>, corresponds to Cartesian elements, abd represents
40   * the temporal evolution of (the standard deviation of) the process noise model. The
41   * first function is therefore the standard deviation along the LOF X axis, the second
42   * function represents the standard deviation along the LOF Y axis...
43   * This allows to set up simply a process noise representing an uncertainty that grows
44   * mainly along the track. The 6x6 upper left part of output matrix will however not be
45   * diagonal as it will be converted to the same inertial frame and orbit type as the
46   * {@link SpacecraftState state} used by the {@link KalmanEstimator Kalman estimator}.
47   * </p>
48   * <p>
49   * The propagation and measurements parameters are not associated to a specific frame and
50   * are appended as is in the lower right part diagonal of the output matrix. This implies
51   * this simplified model does not include correlation between the parameters and the orbit,
52   * but only evolution of the parameters themselves. If such correlations are needed, users
53   * must set up a custom {@link CovarianceMatrixProvider covariance matrix provider}. In most
54   * cases, the parameters are constant and their evolution noise is always 0, so the
55   * functions can be set to {@code x -> 0}.
56   * </p>
57   * <p>
58   * This class always provides one initial noise matrix or initial covariance matrix and one process noise matrix.
59   * </p>
60   * @author Luc Maisonobe
61   * @author Maxime Journot
62   * @since 9.2
63   */
64  public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {
65  
66      /** Local Orbital Frame (LOF) type used. */
67      private final LOFType lofType;
68  
69      /** Position angle for the orbital process noise matrix. */
70      private final PositionAngleType positionAngleType;
71  
72      /** Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian formalism. */
73      private final UnivariateFunction[] lofCartesianOrbitalParametersEvolution;
74  
75      /** Array of univariate functions for the propagation parameters process noise evolution. */
76      private final UnivariateFunction[] propagationParametersEvolution;
77  
78      /** Array of univariate functions for the measurements parameters process noise evolution. */
79      private final UnivariateFunction[] measurementsParametersEvolution;
80  
81      /** Simple constructor.
82       * @param initialCovarianceMatrix initial covariance matrix
83       * @param lofType the LOF type used
84       * @param positionAngleType the position angle used for the computation of the process noise
85       * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
86       * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
87       */
88      public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
89                                    final LOFType lofType,
90                                    final PositionAngleType positionAngleType,
91                                    final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
92                                    final UnivariateFunction[] propagationParametersEvolution) {
93  
94          // Call the new constructor with an empty array for measurements parameters
95          this(initialCovarianceMatrix, lofType, positionAngleType, lofCartesianOrbitalParametersEvolution, propagationParametersEvolution, new UnivariateFunction[0]);
96      }
97  
98      /** Simple constructor.
99       * @param initialCovarianceMatrix initial covariance matrix
100      * @param lofType the LOF type used
101      * @param positionAngleType the position angle used for the computation of the process noise
102      * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
103      * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
104      * @param measurementsParametersEvolution Array of univariate functions for the measurements parameters process noise evolution
105      * @since 10.3
106      */
107     public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
108                                   final LOFType lofType,
109                                   final PositionAngleType positionAngleType,
110                                   final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
111                                   final UnivariateFunction[] propagationParametersEvolution,
112                                   final UnivariateFunction[] measurementsParametersEvolution) {
113 
114         super(initialCovarianceMatrix);
115         this.lofType = lofType;
116         this.positionAngleType = positionAngleType;
117         this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution.clone();
118         this.propagationParametersEvolution = propagationParametersEvolution.clone();
119         this.measurementsParametersEvolution = measurementsParametersEvolution.clone();
120     }
121 
122     /** Getter for the lofType.
123      * @return the lofType
124      */
125     public LOFType getLofType() {
126         return lofType;
127     }
128 
129     /** Getter for the positionAngle.
130      * @return the positionAngle
131      */
132     public PositionAngleType getPositionAngleType() {
133         return positionAngleType;
134     }
135 
136     /** Getter for the lofCartesianOrbitalParametersEvolution.
137      * @return the lofCartesianOrbitalParametersEvolution
138      */
139     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
140         return lofCartesianOrbitalParametersEvolution.clone();
141     }
142 
143     /** Getter for the propagationParametersEvolution.
144      * @return the propagationParametersEvolution
145      */
146     public UnivariateFunction[] getPropagationParametersEvolution() {
147         return propagationParametersEvolution.clone();
148     }
149 
150     /** Getter for the measurementsParametersEvolution.
151      * @return the measurementsParametersEvolution
152      */
153     public UnivariateFunction[] getMeasurementsParametersEvolution() {
154         return measurementsParametersEvolution.clone();
155     }
156 
157     /** {@inheritDoc} */
158     @Override
159     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
160                                             final SpacecraftState current) {
161 
162         // Number of estimated parameters
163         final int nbOrb    = lofCartesianOrbitalParametersEvolution.length;
164         final int nbPropag = propagationParametersEvolution.length;
165         final int nbMeas   = measurementsParametersEvolution.length;
166 
167         // Initialize process noise matrix
168         final RealMatrix processNoiseMatrix = MatrixUtils.createRealMatrix(nbOrb + nbPropag + nbMeas,
169                                                                            nbOrb + nbPropag + nbMeas);
170 
171         // Orbital parameters
172         if (nbOrb != 0) {
173             // Orbital parameters process noise matrix in inertial frame and current orbit type
174             final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);
175             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
176         }
177 
178         // Propagation parameters
179         if (nbPropag != 0) {
180             // Propagation parameters process noise matrix
181             final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);
182             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), nbOrb, nbOrb);
183         }
184 
185         // Measurement parameters
186         if (nbMeas != 0) {
187             // Measurement parameters process noise matrix
188             final RealMatrix measurementsProcessNoiseMatrix = getMeasurementsProcessNoiseMatrix(previous, current);
189             processNoiseMatrix.setSubMatrix(measurementsProcessNoiseMatrix.getData(), nbOrb + nbPropag, nbOrb + nbPropag);
190         }
191 
192         return processNoiseMatrix;
193     }
194 
195     /** Get the process noise for the six orbital parameters in current spacecraft inertial frame and current orbit type.
196      * @param previous previous state
197      * @param current current state
198      * @return physical (i.e. non normalized) orbital process noise matrix in inertial frame
199      */
200     private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
201                                                             final SpacecraftState current) {
202 
203         // ΔT = duration in seconds from previous to current spacecraft state
204         final double deltaT = current.getDate().durationFrom(previous.getDate());
205 
206         // Evaluate the functions, using ΔT as argument
207         final int      lofOrbitalProcessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
208         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalProcessNoiseLength];
209 
210         // The function return a value which dimension is that of a standard deviation
211         // It needs to be squared before being put in the process noise covariance matrix
212         for (int i = 0; i < lofOrbitalProcessNoiseLength; i++) {
213             final double functionValue =  lofCartesianOrbitalParametersEvolution[i].value(deltaT);
214             lofOrbitalProcessNoiseValues[i] = functionValue * functionValue;
215         }
216 
217         // Form the diagonal matrix in LOF frame and Cartesian formalism
218         final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);
219 
220         // Create state covariance object in LOF
221         final StateCovariance lofCartesianProcessNoiseCov =
222                         new StateCovariance(lofCartesianProcessNoiseMatrix, current.getDate(), lofType);
223 
224         // Convert to Cartesian in orbital frame
225         final StateCovariance inertialCartesianProcessNoiseCov =
226                         lofCartesianProcessNoiseCov.changeCovarianceFrame(current.getOrbit(), current.getFrame());
227 
228         // Convert to current orbit type and position angle
229         final StateCovariance inertialOrbitalProcessNoiseCov =
230                         inertialCartesianProcessNoiseCov.changeCovarianceType(current.getOrbit(),
231                                                                               current.getOrbit().getType(), positionAngleType);
232         // Return inertial orbital covariance matrix
233         return inertialOrbitalProcessNoiseCov.getMatrix();
234     }
235 
236     /** Get the process noise for the propagation parameters.
237      * @param previous previous state
238      * @param current current state
239      * @return physical (i.e. non normalized) propagation process noise matrix
240      */
241     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
242                                                         final SpacecraftState current) {
243 
244         // ΔT = duration from previous to current spacecraft state (in seconds)
245         final double deltaT = current.getDate().durationFrom(previous.getDate());
246 
247         // Evaluate the functions, using ΔT as argument
248         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
249         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];
250 
251         // The function return a value which dimension is that of a standard deviation
252         // It needs to be squared before being put in the process noise covariance matrix
253         for (int i = 0; i < propagationProcessNoiseLength; i++) {
254             final double functionValue =  propagationParametersEvolution[i].value(deltaT);
255             propagationProcessNoiseValues[i] = functionValue * functionValue;
256         }
257 
258         // Form the diagonal matrix corresponding to propagation parameters process noise
259         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
260     }
261 
262     /** Get the process noise for the measurements parameters.
263      * @param previous previous state
264      * @param current current state
265      * @return physical (i.e. non normalized) measurements process noise matrix
266      */
267     private RealMatrix getMeasurementsProcessNoiseMatrix(final SpacecraftState previous,
268                                                          final SpacecraftState current) {
269 
270         // ΔT = duration from previous to current spacecraft state (in seconds)
271         final double deltaT = current.getDate().durationFrom(previous.getDate());
272 
273         // Evaluate the functions, using ΔT as argument
274         final int      measurementsProcessNoiseLength = measurementsParametersEvolution.length;
275         final double[] measurementsProcessNoiseValues = new double[measurementsProcessNoiseLength];
276 
277         // The function return a value which dimension is that of a standard deviation
278         // It needs to be squared before being put in the process noise covariance matrix
279         for (int i = 0; i < measurementsProcessNoiseLength; i++) {
280             final double functionValue =  measurementsParametersEvolution[i].value(deltaT);
281             measurementsProcessNoiseValues[i] = functionValue * functionValue;
282         }
283 
284         // Form the diagonal matrix corresponding to propagation parameters process noise
285         return MatrixUtils.createRealDiagonalMatrix(measurementsProcessNoiseValues);
286     }
287 
288 }