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