1   /* Copyright 2002-2015 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.propagation.numerical;
18  
19  import org.apache.commons.math3.linear.Array2DRowRealMatrix;
20  import org.apache.commons.math3.linear.DecompositionSolver;
21  import org.apache.commons.math3.linear.QRDecomposition;
22  import org.apache.commons.math3.linear.RealMatrix;
23  import org.orekit.errors.OrekitException;
24  import org.orekit.orbits.Orbit;
25  import org.orekit.orbits.OrbitType;
26  import org.orekit.orbits.PositionAngle;
27  import org.orekit.propagation.SpacecraftState;
28  
29  /** Mapper between two-dimensional Jacobian matrices and one-dimensional {@link
30   * SpacecraftState#getAdditionalState(String) additional state arrays}.
31   * <p>
32   * This class does not hold the states by itself. Instances of this class are guaranteed
33   * to be immutable.
34   * </p>
35   * @author Luc Maisonobe
36   * @see org.orekit.propagation.numerical.PartialDerivativesEquations
37   * @see org.orekit.propagation.numerical.NumericalPropagator
38   * @see SpacecraftState#getAdditionalState(String)
39   * @see org.orekit.propagation.AbstractPropagator
40   */
41  public class JacobiansMapper {
42  
43      /** Name. */
44      private String name;
45  
46      /** State vector dimension without additional parameters
47       * (either 6 or 7 depending on mass being included or not). */
48      private final int stateDimension;
49  
50      /** Number of Parameters. */
51      private final int parameters;
52  
53      /** Orbit type. */
54      private final OrbitType orbitType;
55  
56      /** Position angle type. */
57      private final PositionAngle angleType;
58  
59      /** Simple constructor.
60       * @param name name of the Jacobians
61       * @param stateDimension dimension of the state (either 6 or 7 depending on mass
62       * being included or not)
63       * @param parameters number of parameters
64       * @param orbitType orbit type
65       * @param angleType position angle type
66       */
67      JacobiansMapper(final String name, final int stateDimension, final int parameters,
68                      final OrbitType orbitType, final PositionAngle angleType) {
69          this.name           = name;
70          this.stateDimension = stateDimension;
71          this.parameters     = parameters;
72          this.orbitType      = orbitType;
73          this.angleType      = angleType;
74      }
75  
76      /** Get the name of the partial Jacobians.
77       * @return name of the Jacobians
78       */
79      public String getName() {
80          return name;
81      }
82  
83      /** Compute the length of the one-dimensional additional state array needed.
84       * @return length of the one-dimensional additional state array
85       */
86      public int getAdditionalStateDimension() {
87          return stateDimension * (stateDimension + parameters);
88      }
89  
90      /** Get the state vector dimension.
91       * @return state vector dimension
92       */
93      public int getStateDimension() {
94          return stateDimension;
95      }
96  
97      /** Get the number of parameters.
98       * @return number of parameters
99       */
100     public int getParameters() {
101         return parameters;
102     }
103 
104     /** Get the conversion Jacobian between state parameters and cartesian parameters.
105      * @param state spacecraft state
106      * @return conversion Jacobian
107      */
108     private double[][] getdYdC(final SpacecraftState state) {
109 
110         final double[][] dYdC = new double[stateDimension][stateDimension];
111 
112         // make sure the state is in the desired orbit type
113         final Orbit orbit = orbitType.convertType(state.getOrbit());
114 
115         // compute the Jacobian, taking the position angle type into account
116         orbit.getJacobianWrtCartesian(angleType, dYdC);
117         for (int i = 6; i < stateDimension; ++i) {
118             dYdC[i][i] = 1.0;
119         }
120 
121         return dYdC;
122 
123     }
124 
125     /** Set the Jacobian with respect to state into a one-dimensional additional state array.
126      * <p>
127      * This method converts the Jacobians to cartesian parameters and put the converted data
128      * in the one-dimensional {@code p} array.
129      * </p>
130      * @param state spacecraft state
131      * @param dY1dY0 Jacobian of current state at time t₁
132      * with respect to state at some previous time t₀
133      * @param dY1dP Jacobian of current state at time t₁
134      * with respect to parameters (may be null if there are no parameters)
135      * @param p placeholder where to put the one-dimensional additional state
136      * @see #getStateJacobian(SpacecraftState, double[][])
137      */
138     void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
139                              final double[][] dY1dP, final double[] p) {
140 
141         // set up a converter between state parameters and cartesian parameters
142         final RealMatrix dY1dC1 = new Array2DRowRealMatrix(getdYdC(state), false);
143         final DecompositionSolver solver = new QRDecomposition(dY1dC1).getSolver();
144 
145         // convert the provided state Jacobian to cartesian parameters
146         final RealMatrix dC1dY0 = solver.solve(new Array2DRowRealMatrix(dY1dY0, false));
147 
148         // map the converted state Jacobian to one-dimensional array
149         int index = 0;
150         for (int i = 0; i < stateDimension; ++i) {
151             for (int j = 0; j < stateDimension; ++j) {
152                 p[index++] = dC1dY0.getEntry(i, j);
153             }
154         }
155 
156         if (parameters > 0) {
157             // convert the provided state Jacobian to cartesian parameters
158             final RealMatrix dC1dP = solver.solve(new Array2DRowRealMatrix(dY1dP, false));
159 
160             // map the converted parameters Jacobian to one-dimensional array
161             for (int i = 0; i < stateDimension; ++i) {
162                 for (int j = 0; j < parameters; ++j) {
163                     p[index++] = dC1dP.getEntry(i, j);
164                 }
165             }
166         }
167 
168     }
169 
170     /** Get the Jacobian with respect to state from a one-dimensional additional state array.
171      * <p>
172      * This method extract the data from the {@code state} and put it in the
173      * {@code dYdY0} array.
174      * </p>
175      * @param state spacecraft state
176      * @param dYdY0 placeholder where to put the Jacobian with respect to state
177      * @exception OrekitException if state does not contain the Jacobian additional state
178      * @see #getParametersJacobian(SpacecraftState, double[][])
179      */
180     public void getStateJacobian(final SpacecraftState state,  final double[][] dYdY0)
181         throws OrekitException {
182 
183         // get the conversion Jacobian between state parameters and cartesian parameters
184         final double[][] dYdC = getdYdC(state);
185 
186         // extract the additional state
187         final double[] p = state.getAdditionalState(name);
188 
189         // compute dYdY0 = dYdC * dCdY0, without allocating new arrays
190         for (int i = 0; i < stateDimension; i++) {
191             final double[] rowC = dYdC[i];
192             final double[] rowD = dYdY0[i];
193             for (int j = 0; j < stateDimension; ++j) {
194                 double sum = 0;
195                 int pIndex = j;
196                 for (int k = 0; k < stateDimension; ++k) {
197                     sum += rowC[k] * p[pIndex];
198                     pIndex += stateDimension;
199                 }
200                 rowD[j] = sum;
201             }
202         }
203 
204     }
205 
206     /** Get theJacobian with respect to parameters from a one-dimensional additional state array.
207      * <p>
208      * This method extract the data from the {@code p} array and put it in the
209      * {@code dYdP} array.
210      * </p>
211      * <p>
212      * If no parameters have been set in the constructor, the method returns immediately and
213      * does not reference {@code dYdP} which can safely be null in this case.
214      * </p>
215      * @param state spacecraft state
216      * @param dYdP placeholder where to put the Jacobian with respect to parameters
217      * @exception OrekitException if state does not contain the Jacobian additional state
218      * @see #getStateJacobian(SpacecraftState, double[][])
219      */
220     public void getParametersJacobian(final SpacecraftState state, final double[][] dYdP)
221         throws OrekitException {
222 
223         if (parameters > 0) {
224 
225             // get the conversion Jacobian between state parameters and cartesian parameters
226             final double[][] dYdC = getdYdC(state);
227 
228             // extract the additional state
229             final double[] p = state.getAdditionalState(name);
230 
231             // compute dYdP = dYdC * dCdP, without allocating new arrays
232             for (int i = 0; i < stateDimension; i++) {
233                 final double[] rowC = dYdC[i];
234                 final double[] rowD = dYdP[i];
235                 for (int j = 0; j < parameters; ++j) {
236                     double sum = 0;
237                     int pIndex = j + stateDimension * stateDimension;
238                     for (int k = 0; k < stateDimension; ++k) {
239                         sum += rowC[k] * p[pIndex];
240                         pIndex += parameters;
241                     }
242                     rowD[j] = sum;
243                 }
244             }
245 
246         }
247 
248     }
249 
250 }