1 /* Copyright 2002-2022 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.propagation.numerical;
18
19 import java.util.HashMap;
20 import java.util.List;
21 import java.util.Map;
22
23 import org.hipparchus.analysis.differentiation.Gradient;
24 import org.hipparchus.exception.LocalizedCoreFormats;
25 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.linear.QRDecomposition;
28 import org.hipparchus.linear.RealMatrix;
29 import org.hipparchus.util.Precision;
30 import org.orekit.attitudes.AttitudeProvider;
31 import org.orekit.errors.OrekitException;
32 import org.orekit.forces.ForceModel;
33 import org.orekit.orbits.OrbitType;
34 import org.orekit.orbits.PositionAngle;
35 import org.orekit.propagation.FieldSpacecraftState;
36 import org.orekit.propagation.SpacecraftState;
37 import org.orekit.propagation.integration.AdditionalDerivativesProvider;
38 import org.orekit.utils.DoubleArrayDictionary;
39 import org.orekit.utils.ParameterDriver;
40
41 /** Generator for State Transition Matrix.
42 * @author Luc Maisonobe
43 * @since 11.1
44 */
45 class StateTransitionMatrixGenerator implements AdditionalDerivativesProvider {
46
47 /** Threshold for matrix solving. */
48 private static final double THRESHOLD = Precision.SAFE_MIN;
49
50 /** Space dimension. */
51 private static final int SPACE_DIMENSION = 3;
52
53 /** State dimension. */
54 public static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;
55
56 /** Name of the Cartesian STM additional state. */
57 private final String stmName;
58
59 /** Force models used in propagation. */
60 private final List<ForceModel> forceModels;
61
62 /** Attitude provider used in propagation. */
63 private final AttitudeProvider attitudeProvider;
64
65 /** Observers for partial derivatives. */
66 private final Map<String, PartialsObserver> partialsObservers;
67
68 /** Simple constructor.
69 * @param stmName name of the Cartesian STM additional state
70 * @param forceModels force models used in propagation
71 * @param attitudeProvider attitude provider used in propagation
72 */
73 StateTransitionMatrixGenerator(final String stmName, final List<ForceModel> forceModels,
74 final AttitudeProvider attitudeProvider) {
75 this.stmName = stmName;
76 this.forceModels = forceModels;
77 this.attitudeProvider = attitudeProvider;
78 this.partialsObservers = new HashMap<>();
79 }
80
81 /** Register an observer for partial derivatives.
82 * <p>
83 * The observer {@link PartialsObserver#partialsComputed(double[], double[]) partialsComputed}
84 * method will be called when partial derivatives are computed, as a side effect of
85 * calling {@link #generate(SpacecraftState)}
86 * </p>
87 * @param name name of the parameter driver this observer is interested in (may be null)
88 * @param observer observer to register
89 */
90 void addObserver(final String name, final PartialsObserver observer) {
91 partialsObservers.put(name, observer);
92 }
93
94 /** {@inheritDoc} */
95 @Override
96 public String getName() {
97 return stmName;
98 }
99
100 /** {@inheritDoc} */
101 @Override
102 public int getDimension() {
103 return STATE_DIMENSION * STATE_DIMENSION;
104 }
105
106 /** {@inheritDoc} */
107 @Override
108 public boolean yield(final SpacecraftState state) {
109 return !state.hasAdditionalState(getName());
110 }
111
112 /** Set the initial value of the State Transition Matrix.
113 * <p>
114 * The returned state must be added to the propagator.
115 * </p>
116 * @param state initial state
117 * @param dYdY0 initial State Transition Matrix ∂Y/∂Y₀,
118 * if null (which is the most frequent case), assumed to be 6x6 identity
119 * @param orbitType orbit type used for states Y and Y₀ in {@code dYdY0}
120 * @param positionAngle position angle used states Y and Y₀ in {@code dYdY0}
121 * @return state with initial STM (converted to Cartesian ∂C/∂Y₀) added
122 */
123 SpacecraftState setInitialStateTransitionMatrix(final SpacecraftState state,
124 final RealMatrix dYdY0,
125 final OrbitType orbitType,
126 final PositionAngle positionAngle) {
127
128 final RealMatrix nonNullDYdY0;
129 if (dYdY0 == null) {
130 nonNullDYdY0 = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);
131 } else {
132 if (dYdY0.getRowDimension() != STATE_DIMENSION ||
133 dYdY0.getColumnDimension() != STATE_DIMENSION) {
134 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
135 dYdY0.getRowDimension(), dYdY0.getColumnDimension(),
136 STATE_DIMENSION, STATE_DIMENSION);
137 }
138 nonNullDYdY0 = dYdY0;
139 }
140
141 // convert to Cartesian STM
142 final RealMatrix dCdY0;
143 if (state.isOrbitDefined()) {
144 final double[][] dYdC = new double[STATE_DIMENSION][STATE_DIMENSION];
145 orbitType.convertType(state.getOrbit()).getJacobianWrtCartesian(positionAngle, dYdC);
146 dCdY0 = new QRDecomposition(MatrixUtils.createRealMatrix(dYdC), THRESHOLD).getSolver().solve(nonNullDYdY0);
147 } else {
148 dCdY0 = nonNullDYdY0;
149 }
150
151 // flatten matrix
152 final double[] flat = new double[STATE_DIMENSION * STATE_DIMENSION];
153 int k = 0;
154 for (int i = 0; i < STATE_DIMENSION; ++i) {
155 for (int j = 0; j < STATE_DIMENSION; ++j) {
156 flat[k++] = dCdY0.getEntry(i, j);
157 }
158 }
159
160 // set additional state
161 return state.addAdditionalState(stmName, flat);
162
163 }
164
165 /** {@inheritDoc} */
166 public double[] derivatives(final SpacecraftState state) {
167
168 // Assuming position is (px, py, pz), velocity is (vx, vy, vz) and the acceleration
169 // due to the force models is (Σ ax, Σ ay, Σ az), the differential equation for
170 // Cartesian State Transition Matrix ∂C/∂Y₀ for the contribution of all force models is:
171 // [ 0 0 0 1 0 0 ]
172 // [ 0 0 0 0 1 0 ]
173 // d(∂C/∂Y₀)/dt = [ 0 0 0 1 0 1 ] ⨯ ∂C/∂Y₀
174 // [Σ dax/dpx Σ dax/dpy Σ dax/dpz Σ dax/dvx Σ dax/dvy Σ dax/dvz]
175 // [Σ day/dpx Σ day/dpy Σ dax/dpz Σ day/dvx Σ day/dvy Σ dax/dvz]
176 // [Σ daz/dpx Σ daz/dpy Σ dax/dpz Σ daz/dvx Σ daz/dvy Σ dax/dvz]
177 // some force models depend on velocity (either directly or through attitude),
178 // whereas some other force models depend only on position.
179 // For the latter, the lower right part of the matrix is zero
180 final double[] factor = computePartials(state);
181
182 // retrieve current State Transition Matrix
183 final double[] p = state.getAdditionalState(getName());
184 final double[] pDot = new double[p.length];
185
186 // perform multiplication
187 multiplyMatrix(factor, p, pDot, STATE_DIMENSION);
188
189 return pDot;
190
191 }
192
193 /** Compute evolution matrix product.
194 * <p>
195 * This method computes \(Y = F \times X\) where the factor matrix is:
196 * \[F = \begin{matrix}
197 * 0 & 0 & 0 & 1 & 0 & 0 \\
198 * 0 & 0 & 0 & 0 & 1 & 0 \\
199 * 0 & 0 & 0 & 0 & 0 & 1 \\
200 * \sum \frac{da_x}{dp_x} & \sum\frac{da_x}{dp_y} & \sum\frac{da_x}{dp_z} & \sum\frac{da_x}{dv_x} & \sum\frac{da_x}{dv_y} & \sum\frac{da_x}{dv_z}\\
201 * \sum \frac{da_y}{dp_x} & \sum\frac{da_y}{dp_y} & \sum\frac{da_y}{dp_z} & \sum\frac{da_y}{dv_x} & \sum\frac{da_y}{dv_y} & \sum\frac{da_y}{dv_z}\\
202 * \sum \frac{da_z}{dp_x} & \sum\frac{da_z}{dp_y} & \sum\frac{da_z}{dp_z} & \sum\frac{da_z}{dv_x} & \sum\frac{da_z}{dv_y} & \sum\frac{da_z}{dv_z}
203 * \end{matrix}\]
204 * </p>
205 * @param factor factor matrix
206 * @param x right factor of the multiplication, as a flatten array in row major order
207 * @param y placeholder where to put the result, as a flatten array in row major order
208 * @param columns number of columns of both x and y (so their dimensions are 6 x columns)
209 */
210 static void multiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
211
212 final int n = SPACE_DIMENSION * columns;
213
214 // handle first three rows by a simple copy
215 System.arraycopy(x, n, y, 0, n);
216
217 // regular multiplication for the last three rows
218 for (int j = 0; j < columns; ++j) {
219 y[n + j ] = factor[ 0] * x[j ] + factor[ 1] * x[j + columns] + factor[ 2] * x[j + 2 * columns] +
220 factor[ 3] * x[j + 3 * columns] + factor[ 4] * x[j + 4 * columns] + factor[ 5] * x[j + 5 * columns];
221 y[n + j + columns] = factor[ 6] * x[j ] + factor[ 7] * x[j + columns] + factor[ 8] * x[j + 2 * columns] +
222 factor[ 9] * x[j + 3 * columns] + factor[10] * x[j + 4 * columns] + factor[11] * x[j + 5 * columns];
223 y[n + j + 2 * columns] = factor[12] * x[j ] + factor[13] * x[j + columns] + factor[14] * x[j + 2 * columns] +
224 factor[15] * x[j + 3 * columns] + factor[16] * x[j + 4 * columns] + factor[17] * x[j + 5 * columns];
225 }
226
227 }
228
229 /** Compute the various partial derivatives.
230 * @param state current spacecraft state
231 * @return factor matrix
232 */
233 private double[] computePartials(final SpacecraftState state) {
234
235 // set up containers for partial derivatives
236 final double[] factor = new double[SPACE_DIMENSION * STATE_DIMENSION];
237 final DoubleArrayDictionary accelerationPartials = new DoubleArrayDictionary();
238
239 // evaluate contribution of all force models
240 final NumericalGradientConverter fullConverter = new NumericalGradientConverter(state, STATE_DIMENSION, attitudeProvider);
241 final NumericalGradientConverter posOnlyConverter = new NumericalGradientConverter(state, SPACE_DIMENSION, attitudeProvider);
242 for (final ForceModel forceModel : forceModels) {
243
244 final NumericalGradientConverter converter = forceModel.dependsOnPositionOnly() ? posOnlyConverter : fullConverter;
245 final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
246 final Gradient[] parameters = converter.getParameters(dsState, forceModel);
247 final FieldVector3D<Gradient> acceleration = forceModel.acceleration(dsState, parameters);
248 final double[] gradX = acceleration.getX().getGradient();
249 final double[] gradY = acceleration.getY().getGradient();
250 final double[] gradZ = acceleration.getZ().getGradient();
251
252 // lower left part of the factor matrix
253 factor[ 0] += gradX[0];
254 factor[ 1] += gradX[1];
255 factor[ 2] += gradX[2];
256 factor[ 6] += gradY[0];
257 factor[ 7] += gradY[1];
258 factor[ 8] += gradY[2];
259 factor[12] += gradZ[0];
260 factor[13] += gradZ[1];
261 factor[14] += gradZ[2];
262
263 if (!forceModel.dependsOnPositionOnly()) {
264 // lower right part of the factor matrix
265 factor[ 3] += gradX[3];
266 factor[ 4] += gradX[4];
267 factor[ 5] += gradX[5];
268 factor[ 9] += gradY[3];
269 factor[10] += gradY[4];
270 factor[11] += gradY[5];
271 factor[15] += gradZ[3];
272 factor[16] += gradZ[4];
273 factor[17] += gradZ[5];
274 }
275
276 // partials derivatives with respect to parameters
277 int paramsIndex = converter.getFreeStateParameters();
278 for (ParameterDriver driver : forceModel.getParametersDrivers()) {
279 if (driver.isSelected()) {
280
281 // get the partials derivatives for this driver
282 DoubleArrayDictionary.Entry entry = accelerationPartials.getEntry(driver.getName());
283 if (entry == null) {
284 // create an entry filled with zeroes
285 accelerationPartials.put(driver.getName(), new double[SPACE_DIMENSION]);
286 entry = accelerationPartials.getEntry(driver.getName());
287 }
288
289 // add the contribution of the current force model
290 entry.increment(new double[] {
291 gradX[paramsIndex], gradY[paramsIndex], gradZ[paramsIndex]
292 });
293 ++paramsIndex;
294
295 }
296 }
297
298 // notify observers
299 for (Map.Entry<String, PartialsObserver> observersEntry : partialsObservers.entrySet()) {
300 final DoubleArrayDictionary.Entry entry = accelerationPartials.getEntry(observersEntry.getKey());
301 observersEntry.getValue().partialsComputed(state, factor, entry == null ? new double[SPACE_DIMENSION] : entry.getValue());
302 }
303
304 }
305
306 return factor;
307
308 }
309
310 /** Interface for observing partials derivatives. */
311 public interface PartialsObserver {
312
313 /** Callback called when partial derivatives have been computed.
314 * <p>
315 * The factor matrix is:
316 * \[F = \begin{matrix}
317 * 0 & 0 & 0 & 1 & 0 & 0 \\
318 * 0 & 0 & 0 & 0 & 1 & 0 \\
319 * 0 & 0 & 0 & 0 & 0 & 1 \\
320 * \sum \frac{da_x}{dp_x} & \sum\frac{da_x}{dp_y} & \sum\frac{da_x}{dp_z} & \sum\frac{da_x}{dv_x} & \sum\frac{da_x}{dv_y} & \sum\frac{da_x}{dv_z}\\
321 * \sum \frac{da_y}{dp_x} & \sum\frac{da_y}{dp_y} & \sum\frac{da_y}{dp_z} & \sum\frac{da_y}{dv_x} & \sum\frac{da_y}{dv_y} & \sum\frac{da_y}{dv_z}\\
322 * \sum \frac{da_z}{dp_x} & \sum\frac{da_z}{dp_y} & \sum\frac{da_z}{dp_z} & \sum\frac{da_z}{dv_x} & \sum\frac{da_z}{dv_y} & \sum\frac{da_z}{dv_z}
323 * \end{matrix}\]
324 * </p>
325 * @param state current spacecraft state
326 * @param factor factor matrix, flattened along rows
327 * @param accelerationPartials partials derivatives of acceleration with respect to the parameter driver
328 * that was registered (zero if no parameters were not selected or parameter is unknown)
329 */
330 void partialsComputed(SpacecraftState state, double[] factor, double[] accelerationPartials);
331
332 }
333
334 }
335