1   /* Copyright 2002-2021 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.PositionAngle;
24  import org.orekit.propagation.SpacecraftState;
25  import org.orekit.utils.CartesianDerivativesFilter;
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 PositionAngle}
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 PositionAngle positionAngle;
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 positionAngle 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 PositionAngle positionAngle,
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, positionAngle, 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 positionAngle 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 PositionAngle positionAngle,
110                                   final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
111                                   final UnivariateFunction[] propagationParametersEvolution,
112                                   final UnivariateFunction[] measurementsParametersEvolution) {
113 
114         super(initialCovarianceMatrix);
115         this.lofType = lofType;
116         this.positionAngle = positionAngle;
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 PositionAngle getPositionAngle() {
133         return positionAngle;
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         // Get the Jacobian from LOF to Inertial
221         final double[][] dLofdInertial = new double[6][6];
222         lofType.transformFromInertial(current.getDate(),
223                                       current.getPVCoordinates()).getInverse().getJacobian(CartesianDerivativesFilter.USE_PV,
224                                                                                            dLofdInertial);
225         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(dLofdInertial);
226 
227         // Jacobian of orbit parameters with respect to Cartesian parameters
228         final double[][] dYdC = new double[6][6];
229         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
230         final RealMatrix jacOrbitWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
231 
232         // Complete Jacobian of the transformation
233         final RealMatrix jacobian = jacOrbitWrtCartesian.multiply(jacLofToInertial);
234 
235         // Return the orbital process noise matrix in inertial frame and proper orbit type
236         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
237                          multiply(lofCartesianProcessNoiseMatrix).
238                          multiplyTransposed(jacobian);
239         return inertialOrbitalProcessNoiseMatrix;
240     }
241 
242     /** Get the process noise for the propagation parameters.
243      * @param previous previous state
244      * @param current current state
245      * @return physical (i.e. non normalized) propagation process noise matrix
246      */
247     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
248                                                         final SpacecraftState current) {
249 
250         // ΔT = duration from previous to current spacecraft state (in seconds)
251         final double deltaT = current.getDate().durationFrom(previous.getDate());
252 
253         // Evaluate the functions, using ΔT as argument
254         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
255         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];
256 
257         // The function return a value which dimension is that of a standard deviation
258         // It needs to be squared before being put in the process noise covariance matrix
259         for (int i = 0; i < propagationProcessNoiseLength; i++) {
260             final double functionValue =  propagationParametersEvolution[i].value(deltaT);
261             propagationProcessNoiseValues[i] = functionValue * functionValue;
262         }
263 
264         // Form the diagonal matrix corresponding to propagation parameters process noise
265         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
266     }
267 
268     /** Get the process noise for the measurements parameters.
269      * @param previous previous state
270      * @param current current state
271      * @return physical (i.e. non normalized) measurements process noise matrix
272      */
273     private RealMatrix getMeasurementsProcessNoiseMatrix(final SpacecraftState previous,
274                                                          final SpacecraftState current) {
275 
276         // ΔT = duration from previous to current spacecraft state (in seconds)
277         final double deltaT = current.getDate().durationFrom(previous.getDate());
278 
279         // Evaluate the functions, using ΔT as argument
280         final int      measurementsProcessNoiseLength = measurementsParametersEvolution.length;
281         final double[] measurementsProcessNoiseValues = new double[measurementsProcessNoiseLength];
282 
283         // The function return a value which dimension is that of a standard deviation
284         // It needs to be squared before being put in the process noise covariance matrix
285         for (int i = 0; i < measurementsProcessNoiseLength; i++) {
286             final double functionValue =  measurementsParametersEvolution[i].value(deltaT);
287             measurementsProcessNoiseValues[i] = functionValue * functionValue;
288         }
289 
290         // Form the diagonal matrix corresponding to propagation parameters process noise
291         return MatrixUtils.createRealDiagonalMatrix(measurementsProcessNoiseValues);
292     }
293 
294 }