1   /* Copyright 2002-2019 CS Systèmes d'Information
2    * Licensed to CS Systèmes d'Information (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.exception.LocalizedCoreFormats;
21  import org.hipparchus.geometry.euclidean.threed.Vector3D;
22  import org.hipparchus.linear.MatrixUtils;
23  import org.hipparchus.linear.RealMatrix;
24  import org.orekit.errors.OrekitException;
25  import org.orekit.frames.LOFType;
26  import org.orekit.frames.Transform;
27  import org.orekit.orbits.PositionAngle;
28  import org.orekit.propagation.SpacecraftState;
29  
30  /** Provider for a temporal evolution of the process noise matrix.
31   * All parameters (orbital or propagation) are time dependent and provided as {@link UnivariateFunction}.
32   * The argument of the functions is a duration in seconds (between current and previous spacecraft state).
33   * The output of the functions must be of the dimension of a standard deviation.
34   * The method {@link #getProcessNoiseMatrix} then square the values so that they are consistent with a covariance matrix.
35   * <p>
36   * The orbital parameters evolutions are provided in LOF frame and Cartesian (PV);
37   * then converted in inertial frame and current {@link org.orekit.orbits.OrbitType} and {@link PositionAngle}
38   * when method {@link #getProcessNoiseMatrix} is called.
39   * </p>
40   * <p>
41   * The time-dependent functions define a process noise matrix that is diagonal
42   * <em>in the Local Orbital Frame</em>, corresponds to Cartesian elements, abd represents
43   * the temporal evolution of (the standard deviation of) the process noise model. The
44   * first function is therefore the standard deviation along the LOF X axis, the second
45   * function represents the standard deviation along the LOF Y axis...
46   * This allows to set up simply a process noise representing an uncertainty that grows
47   * mainly along the track. The 6x6 upper left part of output matrix will however not be
48   * diagonal as it will be converted to the same inertial frame and orbit type as the
49   * {@link SpacecraftState state} used by the {@link KalmanEstimator Kalman estimator}.
50   * </p>
51   * <p>
52   * The propagation parameters are not associated to a specific frame and are appended as
53   * is in the lower right part diagonal of the output matrix. This implies this simplified
54   * model does not include correlation between the parameters and the orbit, but only
55   * evolution of the parameters themselves. If such correlations are needed, users must
56   * set up a custom {@link CovarianceMatrixProvider covariance matrix provider}. In most
57   * cases, the parameters are constant and their evolution noise is always 0, so the
58   * functions can be set to {@code x -> 0}.
59   * </p>
60   * <p>
61   * This class always provides one initial noise matrix or initial covariance matrix and one process noise matrix.
62   * </p>
63   * @author Luc Maisonobe
64   * @author Maxime Journot
65   * @since 9.2
66   */
67  public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {
68  
69      /** Local Orbital Frame (LOF) type used. */
70      private final LOFType lofType;
71  
72      /** Position angle for the orbital process noise matrix. */
73      private final PositionAngle positionAngle;
74  
75      /** Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian formalism. */
76      private final UnivariateFunction[] lofCartesianOrbitalParametersEvolution;
77  
78      /** Array of univariate functions for the propagation parameters process noise evolution. */
79      private final UnivariateFunction[] propagationParametersEvolution;
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          super(initialCovarianceMatrix);
94          this.lofType = lofType;
95          this.positionAngle = positionAngle;
96          this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution;
97          this.propagationParametersEvolution = propagationParametersEvolution;
98  
99          // Ensure that the orbital evolution array size is 6
100         if (lofCartesianOrbitalParametersEvolution.length != 6) {
101             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
102                                       lofCartesianOrbitalParametersEvolution, 6);
103         }
104     }
105 
106     /** Getter for the lofType.
107      * @return the lofType
108      */
109     public LOFType getLofType() {
110         return lofType;
111     }
112 
113     /** Getter for the positionAngle.
114      * @return the positionAngle
115      */
116     public PositionAngle getPositionAngle() {
117         return positionAngle;
118     }
119 
120     /** Getter for the lofCartesianOrbitalParametersEvolution.
121      * @return the lofCartesianOrbitalParametersEvolution
122      */
123     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
124         return lofCartesianOrbitalParametersEvolution;
125     }
126 
127     /** Getter for the propagationParametersEvolution.
128      * @return the propagationParametersEvolution
129      */
130     public UnivariateFunction[] getPropagationParametersEvolution() {
131         return propagationParametersEvolution;
132     }
133 
134     /** {@inheritDoc} */
135     @Override
136     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
137                                             final SpacecraftState current) {
138 
139         // Orbital parameters process noise matrix in inertial frame and current orbit type
140         final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);
141 
142         if (propagationParametersEvolution.length == 0) {
143 
144             // no propagation parameters contribution, just return the orbital part
145             return inertialOrbitalProcessNoiseMatrix;
146 
147         } else {
148 
149             // Propagation parameters process noise matrix
150             final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);
151 
152             // Concatenate the matrices
153             final int        orbitalMatrixSize     = lofCartesianOrbitalParametersEvolution.length;
154             final int        propagationMatrixSize = propagationParametersEvolution.length;
155             final RealMatrix processNoiseMatrix    = MatrixUtils.createRealMatrix(orbitalMatrixSize + propagationMatrixSize,
156                                                                                   orbitalMatrixSize + propagationMatrixSize);
157             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
158             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), orbitalMatrixSize, orbitalMatrixSize);
159             return processNoiseMatrix;
160 
161         }
162     }
163 
164     /** Get the process noise for the six orbital parameters in current spacecraft inertial frame and current orbit type.
165      * @param previous previous state
166      * @param current current state
167      * @return physical (i.e. non normalized) orbital process noise matrix in inertial frame
168      */
169     private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
170                                                             final SpacecraftState current) {
171 
172         // ΔT = duration in seconds from previous to current spacecraft state
173         final double deltaT = current.getDate().durationFrom(previous.getDate());
174 
175         // Evaluate the functions, using ΔT as argument
176         final int      lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
177         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];
178 
179         // The function return a value which dimension is that of a standard deviation
180         // It needs to be squared before being put in the process noise covariance matrix
181         for (int i = 0; i < lofOrbitalprocessNoiseLength; i++) {
182             final double functionValue =  lofCartesianOrbitalParametersEvolution[i].value(deltaT);
183             lofOrbitalProcessNoiseValues[i] = functionValue * functionValue;
184         }
185 
186         // Form the diagonal matrix in LOF frame and Cartesian formalism
187         final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);
188 
189         // Get the rotation matrix from LOF to inertial frame
190         final double[][] lofToInertialRotation = lofType.rotationFromInertial(current.getPVCoordinates()).
191                                                  revert().getMatrix();
192 
193         // Jacobian from LOF to inertial frame
194         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
195         jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
196         jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);
197 
198         // FIXME: Trying to fix the transform from LOF to inertial
199         final Transform lofToInertial = lofType.transformFromInertial(current.getDate(), current.getPVCoordinates()).getInverse();
200         final Vector3D OM = lofToInertial.getRotationRate().negate();
201         final double[][] MOM = new double[3][3];
202         MOM[0][1] = -OM.getZ();
203         MOM[0][2] = OM.getY();
204         MOM[1][0] = OM.getZ();
205         MOM[1][2] = -OM.getX();
206         MOM[2][0] = -OM.getY();
207         MOM[2][1] = OM.getX();
208         //jacLofToInertial.setSubMatrix(MOM, 3, 0);
209         //debug
210 
211         // Jacobian of orbit parameters with respect to Cartesian parameters
212         final double[][] dYdC = new double[6][6];
213         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
214         final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
215 
216         // Complete Jacobian of the transformation
217         final RealMatrix jacobian = jacParametersWrtCartesian.multiply(jacLofToInertial);
218 
219         // Return the orbital process noise matrix in inertial frame and proper orbit type
220         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
221                          multiply(lofCartesianProcessNoiseMatrix).
222                          multiply(jacobian.transpose());
223         return inertialOrbitalProcessNoiseMatrix;
224     }
225 
226     /** Get the process noise for the propagation parameters.
227      * @param previous previous state
228      * @param current current state
229      * @return physical (i.e. non normalized) propagation process noise matrix
230      */
231     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
232                                                         final SpacecraftState current) {
233 
234         // ΔT = duration from previous to current spacecraft state (in seconds)
235         final double deltaT = current.getDate().durationFrom(previous.getDate());
236 
237         // Evaluate the functions, using ΔT as argument
238         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
239         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];
240 
241         // The function return a value which dimension is that of a standard deviation
242         // It needs to be squared before being put in the process noise covariance matrix
243         for (int i = 0; i < propagationProcessNoiseLength; i++) {
244             final double functionValue =  propagationParametersEvolution[i].value(deltaT);
245             propagationProcessNoiseValues[i] = functionValue * functionValue;
246         }
247 
248         // Form the diagonal matrix corresponding to propagation parameters process noise
249         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
250     }
251 
252 }