1   /* Copyright 2002-2018 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 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   * <p>
64   * WARNING: as of release 9.2, this feature is still considered experimental
65   * </p>
66   * @author Luc Maisonobe
67   * @author Maxime Journot
68   * @since 9.2
69   */
70  public class UnivariateProcessNoise extends AbstractCovarianceMatrixProvider {
71  
72      /** Local Orbital Frame (LOF) type used. */
73      private final LOFType lofType;
74  
75      /** Position angle for the orbital process noise matrix. */
76      private final PositionAngle positionAngle;
77  
78      /** Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian formalism. */
79      private final UnivariateFunction[] lofCartesianOrbitalParametersEvolution;
80  
81      /** Array of univariate functions for the propagation parameters process noise evolution. */
82      private final UnivariateFunction[] propagationParametersEvolution;
83  
84      /** Simple constructor.
85       * @param initialCovarianceMatrix initial covariance matrix
86       * @param lofType the LOF type used
87       * @param positionAngle the position angle used for the computation of the process noise
88       * @param lofCartesianOrbitalParametersEvolution Array of univariate functions for the six orbital parameters process noise evolution in LOF frame and Cartesian orbit type
89       * @param propagationParametersEvolution Array of univariate functions for the propagation parameters process noise evolution
90       * @throws OrekitException if lofOrbitalParametersEvolution array size is different from 6
91       */
92      public UnivariateProcessNoise(final RealMatrix initialCovarianceMatrix,
93                                    final LOFType lofType,
94                                    final PositionAngle positionAngle,
95                                    final UnivariateFunction[] lofCartesianOrbitalParametersEvolution,
96                                    final UnivariateFunction[] propagationParametersEvolution)
97          throws OrekitException {
98          super(initialCovarianceMatrix);
99          this.lofType = lofType;
100         this.positionAngle = positionAngle;
101         this.lofCartesianOrbitalParametersEvolution  = lofCartesianOrbitalParametersEvolution;
102         this.propagationParametersEvolution = propagationParametersEvolution;
103 
104         // Ensure that the orbital evolution array size is 6
105         if (lofCartesianOrbitalParametersEvolution.length != 6) {
106             throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH,
107                                       lofCartesianOrbitalParametersEvolution, 6);
108         }
109     }
110 
111     /** Getter for the lofType.
112      * @return the lofType
113      */
114     public LOFType getLofType() {
115         return lofType;
116     }
117 
118     /** Getter for the positionAngle.
119      * @return the positionAngle
120      */
121     public PositionAngle getPositionAngle() {
122         return positionAngle;
123     }
124 
125     /** Getter for the lofCartesianOrbitalParametersEvolution.
126      * @return the lofCartesianOrbitalParametersEvolution
127      */
128     public UnivariateFunction[] getLofCartesianOrbitalParametersEvolution() {
129         return lofCartesianOrbitalParametersEvolution;
130     }
131 
132     /** Getter for the propagationParametersEvolution.
133      * @return the propagationParametersEvolution
134      */
135     public UnivariateFunction[] getPropagationParametersEvolution() {
136         return propagationParametersEvolution;
137     }
138 
139     /** {@inheritDoc} */
140     @Override
141     public RealMatrix getProcessNoiseMatrix(final SpacecraftState previous,
142                                             final SpacecraftState current) {
143 
144         // Orbital parameters process noise matrix in inertial frame and current orbit type
145         final RealMatrix inertialOrbitalProcessNoiseMatrix = getInertialOrbitalProcessNoiseMatrix(previous, current);
146 
147         if (propagationParametersEvolution.length == 0) {
148 
149             // no propagation parameters contribution, just return the orbital part
150             return inertialOrbitalProcessNoiseMatrix;
151 
152         } else {
153 
154             // Propagation parameters process noise matrix
155             final RealMatrix propagationProcessNoiseMatrix = getPropagationProcessNoiseMatrix(previous, current);
156 
157             // Concatenate the matrices
158             final int        orbitalMatrixSize     = lofCartesianOrbitalParametersEvolution.length;
159             final int        propagationMatrixSize = propagationParametersEvolution.length;
160             final RealMatrix processNoiseMatrix    = MatrixUtils.createRealMatrix(orbitalMatrixSize + propagationMatrixSize,
161                                                                                   orbitalMatrixSize + propagationMatrixSize);
162             processNoiseMatrix.setSubMatrix(inertialOrbitalProcessNoiseMatrix.getData(), 0, 0);
163             processNoiseMatrix.setSubMatrix(propagationProcessNoiseMatrix.getData(), orbitalMatrixSize, orbitalMatrixSize);
164             return processNoiseMatrix;
165 
166         }
167     }
168 
169     /** Get the process noise for the six orbital parameters in current spacecraft inertial frame and current orbit type.
170      * @param previous previous state
171      * @param current current state
172      * @return physical (i.e. non normalized) orbital process noise matrix in inertial frame
173      */
174     private RealMatrix getInertialOrbitalProcessNoiseMatrix(final SpacecraftState previous,
175                                                             final SpacecraftState current) {
176 
177         // ΔT = duration in seconds from previous to current spacecraft state
178         final double deltaT = current.getDate().durationFrom(previous.getDate());
179 
180         // Evaluate the functions, using ΔT as argument
181         final int      lofOrbitalprocessNoiseLength = lofCartesianOrbitalParametersEvolution.length;
182         final double[] lofOrbitalProcessNoiseValues = new double[lofOrbitalprocessNoiseLength];
183 
184         // The function return a value which dimension is that of a standard deviation
185         // It needs to be squared before being put in the process noise covariance matrix
186         for (int i = 0; i < lofOrbitalprocessNoiseLength; i++) {
187             final double functionValue =  lofCartesianOrbitalParametersEvolution[i].value(deltaT);
188             lofOrbitalProcessNoiseValues[i] = functionValue * functionValue;
189         }
190 
191         // Form the diagonal matrix in LOF frame and Cartesian formalism
192         final RealMatrix lofCartesianProcessNoiseMatrix = MatrixUtils.createRealDiagonalMatrix(lofOrbitalProcessNoiseValues);
193 
194         // Get the rotation matrix from LOF to inertial frame
195         final double[][] lofToInertialRotation = lofType.rotationFromInertial(current.getPVCoordinates()).
196                                                  revert().getMatrix();
197 
198         // Jacobian from LOF to inertial frame
199         final RealMatrix jacLofToInertial = MatrixUtils.createRealMatrix(6, 6);
200         jacLofToInertial.setSubMatrix(lofToInertialRotation, 0, 0);
201         jacLofToInertial.setSubMatrix(lofToInertialRotation, 3, 3);
202 
203         // FIXME: Trying to fix the transform from LOF to inertial
204         final Transform lofToInertial = lofType.transformFromInertial(current.getDate(), current.getPVCoordinates()).getInverse();
205         final Vector3D OM = lofToInertial.getRotationRate().negate();
206         final double[][] MOM = new double[3][3];
207         MOM[0][1] = -OM.getZ();
208         MOM[0][2] = OM.getY();
209         MOM[1][0] = OM.getZ();
210         MOM[1][2] = -OM.getX();
211         MOM[2][0] = -OM.getY();
212         MOM[2][1] = OM.getX();
213         //jacLofToInertial.setSubMatrix(MOM, 3, 0);
214         //debug
215 
216         // Jacobian of orbit parameters with respect to Cartesian parameters
217         final double[][] dYdC = new double[6][6];
218         current.getOrbit().getJacobianWrtCartesian(positionAngle, dYdC);
219         final RealMatrix jacParametersWrtCartesian = MatrixUtils.createRealMatrix(dYdC);
220 
221         // Complete Jacobian of the transformation
222         final RealMatrix jacobian = jacParametersWrtCartesian.multiply(jacLofToInertial);
223 
224         // Return the orbital process noise matrix in inertial frame and proper orbit type
225         final RealMatrix inertialOrbitalProcessNoiseMatrix = jacobian.
226                          multiply(lofCartesianProcessNoiseMatrix).
227                          multiply(jacobian.transpose());
228         return inertialOrbitalProcessNoiseMatrix;
229     }
230 
231     /** Get the process noise for the propagation parameters.
232      * @param previous previous state
233      * @param current current state
234      * @return physical (i.e. non normalized) propagation process noise matrix
235      */
236     private RealMatrix getPropagationProcessNoiseMatrix(final SpacecraftState previous,
237                                                         final SpacecraftState current) {
238 
239         // ΔT = duration from previous to current spacecraft state (in seconds)
240         final double deltaT = current.getDate().durationFrom(previous.getDate());
241 
242         // Evaluate the functions, using ΔT as argument
243         final int      propagationProcessNoiseLength = propagationParametersEvolution.length;
244         final double[] propagationProcessNoiseValues = new double[propagationProcessNoiseLength];
245 
246         // The function return a value which dimension is that of a standard deviation
247         // It needs to be squared before being put in the process noise covariance matrix
248         for (int i = 0; i < propagationProcessNoiseLength; i++) {
249             final double functionValue =  propagationParametersEvolution[i].value(deltaT);
250             propagationProcessNoiseValues[i] = functionValue * functionValue;
251         }
252 
253         // Form the diagonal matrix corresponding to propagation parameters process noise
254         return MatrixUtils.createRealDiagonalMatrix(propagationProcessNoiseValues);
255     }
256 
257 }