1 /* Copyright 2002-2025 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 org.hipparchus.analysis.differentiation.Gradient;
20 import org.hipparchus.exception.LocalizedCoreFormats;
21 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
22 import org.hipparchus.linear.MatrixUtils;
23 import org.hipparchus.linear.QRDecomposition;
24 import org.hipparchus.linear.RealMatrix;
25 import org.hipparchus.util.Precision;
26 import org.orekit.attitudes.AttitudeProvider;
27 import org.orekit.attitudes.AttitudeProviderModifier;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.forces.ForceModel;
30 import org.orekit.orbits.OrbitType;
31 import org.orekit.orbits.PositionAngleType;
32 import org.orekit.propagation.FieldSpacecraftState;
33 import org.orekit.propagation.SpacecraftState;
34 import org.orekit.propagation.integration.AdditionalDerivativesProvider;
35 import org.orekit.propagation.integration.CombinedDerivatives;
36 import org.orekit.utils.DoubleArrayDictionary;
37 import org.orekit.utils.ParameterDriver;
38 import org.orekit.utils.TimeSpanMap.Span;
39
40 import java.util.HashMap;
41 import java.util.List;
42 import java.util.Map;
43
44 /** Generator for State Transition Matrix.
45 * @author Luc Maisonobe
46 * @author Melina Vanel
47 * @since 11.1
48 */
49 class StateTransitionMatrixGenerator implements AdditionalDerivativesProvider {
50
51 /** Threshold for matrix solving. */
52 private static final double THRESHOLD = Precision.SAFE_MIN;
53
54 /** Space dimension. */
55 private static final int SPACE_DIMENSION = 3;
56
57 /** State dimension. */
58 public static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;
59
60 /** Name of the Cartesian STM additional state. */
61 private final String stmName;
62
63 /** Force models used in propagation. */
64 private final List<ForceModel> forceModels;
65
66 /** Attitude provider used in propagation. */
67 private final AttitudeProvider attitudeProvider;
68
69 /** Observers for partial derivatives. */
70 private final Map<String, PartialsObserver> partialsObservers;
71
72 /** Simple constructor.
73 * @param stmName name of the Cartesian STM additional state
74 * @param forceModels force models used in propagation
75 * @param attitudeProvider attitude provider used in propagation
76 */
77 StateTransitionMatrixGenerator(final String stmName, final List<ForceModel> forceModels,
78 final AttitudeProvider attitudeProvider) {
79 this.stmName = stmName;
80 this.forceModels = forceModels;
81 this.attitudeProvider = attitudeProvider;
82 this.partialsObservers = new HashMap<>();
83 }
84
85 /** Register an observer for partial derivatives.
86 * <p>
87 * The observer {@link PartialsObserver#partialsComputed(SpacecraftState, double[], double[])} partialsComputed}
88 * method will be called when partial derivatives are computed, as a side effect of
89 * calling {@link #computePartials(SpacecraftState)} (SpacecraftState)}
90 * </p>
91 * @param name name of the parameter driver this observer is interested in (may be null)
92 * @param observer observer to register
93 */
94 void addObserver(final String name, final PartialsObserver observer) {
95 partialsObservers.put(name, observer);
96 }
97
98 /** {@inheritDoc} */
99 @Override
100 public String getName() {
101 return stmName;
102 }
103
104 /** {@inheritDoc} */
105 @Override
106 public int getDimension() {
107 return STATE_DIMENSION * STATE_DIMENSION;
108 }
109
110 /** {@inheritDoc} */
111 @Override
112 public boolean yields(final SpacecraftState state) {
113 return !state.hasAdditionalData(getName());
114 }
115
116 /** Set the initial value of the State Transition Matrix.
117 * <p>
118 * The returned state must be added to the propagator.
119 * </p>
120 * @param state initial state
121 * @param dYdY0 initial State Transition Matrix ∂Y/∂Y₀,
122 * if null (which is the most frequent case), assumed to be 6x6 identity
123 * @param orbitType orbit type used for states Y and Y₀ in {@code dYdY0}
124 * @param positionAngleType position angle used states Y and Y₀ in {@code dYdY0}
125 * @return state with initial STM (converted to Cartesian ∂C/∂Y₀) added
126 */
127 SpacecraftState setInitialStateTransitionMatrix(final SpacecraftState state,
128 final RealMatrix dYdY0,
129 final OrbitType orbitType,
130 final PositionAngleType positionAngleType) {
131
132 final RealMatrix nonNullDYdY0;
133 if (dYdY0 == null) {
134 nonNullDYdY0 = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);
135 } else {
136 if (dYdY0.getRowDimension() != STATE_DIMENSION ||
137 dYdY0.getColumnDimension() != STATE_DIMENSION) {
138 throw new OrekitException(LocalizedCoreFormats.DIMENSIONS_MISMATCH_2x2,
139 dYdY0.getRowDimension(), dYdY0.getColumnDimension(),
140 STATE_DIMENSION, STATE_DIMENSION);
141 }
142 nonNullDYdY0 = dYdY0;
143 }
144
145 // convert to Cartesian STM
146 final RealMatrix dCdY0;
147 if (state.isOrbitDefined()) {
148 final double[][] dYdC = new double[STATE_DIMENSION][STATE_DIMENSION];
149 orbitType.convertType(state.getOrbit()).getJacobianWrtCartesian(positionAngleType, dYdC);
150 dCdY0 = new QRDecomposition(MatrixUtils.createRealMatrix(dYdC), THRESHOLD).getSolver().solve(nonNullDYdY0);
151 } else {
152 dCdY0 = nonNullDYdY0;
153 }
154
155 // flatten matrix
156 final double[] flat = new double[STATE_DIMENSION * STATE_DIMENSION];
157 int k = 0;
158 for (int i = 0; i < STATE_DIMENSION; ++i) {
159 for (int j = 0; j < STATE_DIMENSION; ++j) {
160 flat[k++] = dCdY0.getEntry(i, j);
161 }
162 }
163
164 // set additional state
165 return state.addAdditionalData(stmName, flat);
166
167 }
168
169 /** {@inheritDoc} */
170 public CombinedDerivatives combinedDerivatives(final SpacecraftState state) {
171
172 // Assuming position is (px, py, pz), velocity is (vx, vy, vz) and the acceleration
173 // due to the force models is (Σ ax, Σ ay, Σ az), the differential equation for
174 // Cartesian State Transition Matrix ∂C/∂Y₀ for the contribution of all force models is:
175 // [ 0 0 0 1 0 0 ]
176 // [ 0 0 0 0 1 0 ]
177 // d(∂C/∂Y₀)/dt = [ 0 0 0 1 0 1 ] ⨯ ∂C/∂Y₀
178 // [Σ dax/dpx Σ dax/dpy Σ dax/dpz Σ dax/dvx Σ dax/dvy Σ dax/dvz]
179 // [Σ day/dpx Σ day/dpy Σ dax/dpz Σ day/dvx Σ day/dvy Σ dax/dvz]
180 // [Σ daz/dpx Σ daz/dpy Σ dax/dpz Σ daz/dvx Σ daz/dvy Σ dax/dvz]
181 // some force models depend on velocity (either directly or through attitude),
182 // whereas some other force models depend only on position.
183 // For the latter, the lower right part of the matrix is zero
184 final double[] factor = computePartials(state);
185
186 // retrieve current State Transition Matrix
187 final double[] p = state.getAdditionalState(getName());
188 final double[] pDot = new double[p.length];
189
190 // perform multiplication
191 multiplyMatrix(factor, p, pDot, STATE_DIMENSION);
192
193 return new CombinedDerivatives(pDot, null);
194
195 }
196
197 /** Compute evolution matrix product.
198 * <p>
199 * This method computes \(Y = F \times X\) where the factor matrix is:
200 * \[F = \begin{matrix}
201 * 0 & 0 & 0 & 1 & 0 & 0 \\
202 * 0 & 0 & 0 & 0 & 1 & 0 \\
203 * 0 & 0 & 0 & 0 & 0 & 1 \\
204 * \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}\\
205 * \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}\\
206 * \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}
207 * \end{matrix}\]
208 * </p>
209 * @param factor factor matrix
210 * @param x right factor of the multiplication, as a flatten array in row major order
211 * @param y placeholder where to put the result, as a flatten array in row major order
212 * @param columns number of columns of both x and y (so their dimensions are 6 x columns)
213 */
214 static void multiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
215
216 final int n = SPACE_DIMENSION * columns;
217
218 // handle first three rows by a simple copy
219 System.arraycopy(x, n, y, 0, n);
220
221 // regular multiplication for the last three rows
222 for (int j = 0; j < columns; ++j) {
223 y[n + j ] = factor[ 0] * x[j ] + factor[ 1] * x[j + columns] + factor[ 2] * x[j + 2 * columns] +
224 factor[ 3] * x[j + 3 * columns] + factor[ 4] * x[j + 4 * columns] + factor[ 5] * x[j + 5 * columns];
225 y[n + j + columns] = factor[ 6] * x[j ] + factor[ 7] * x[j + columns] + factor[ 8] * x[j + 2 * columns] +
226 factor[ 9] * x[j + 3 * columns] + factor[10] * x[j + 4 * columns] + factor[11] * x[j + 5 * columns];
227 y[n + j + 2 * columns] = factor[12] * x[j ] + factor[13] * x[j + columns] + factor[14] * x[j + 2 * columns] +
228 factor[15] * x[j + 3 * columns] + factor[16] * x[j + 4 * columns] + factor[17] * x[j + 5 * columns];
229 }
230
231 }
232
233 /** Compute the various partial derivatives.
234 * @param state current spacecraft state
235 * @return factor matrix
236 */
237 private double[] computePartials(final SpacecraftState state) {
238
239 // set up containers for partial derivatives
240 final double[] factor = new double[SPACE_DIMENSION * STATE_DIMENSION];
241 final DoubleArrayDictionary accelerationPartials = new DoubleArrayDictionary();
242
243 // evaluate contribution of all force models
244 final AttitudeProvider equivalentAttitudeProvider = wrapAttitudeProviderIfPossible(forceModels, attitudeProvider);
245 final boolean isThereAnyForceNotDependingOnlyOnPosition = forceModels.stream().anyMatch(force -> !force.dependsOnPositionOnly());
246 final NumericalGradientConverter posOnlyConverter = new NumericalGradientConverter(state, SPACE_DIMENSION, equivalentAttitudeProvider);
247 final NumericalGradientConverter fullConverter = isThereAnyForceNotDependingOnlyOnPosition ?
248 new NumericalGradientConverter(state, STATE_DIMENSION, equivalentAttitudeProvider) : posOnlyConverter;
249
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.getParametersAtStateDate(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 // for each span (for each estimated value) corresponding name is added
290 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
291
292 // get the partials derivatives for this driver
293 DoubleArrayDictionary.Entry entry = accelerationPartials.getEntry(span.getData());
294 if (entry == null) {
295 // create an entry filled with zeroes
296 accelerationPartials.put(span.getData(), new double[SPACE_DIMENSION]);
297 entry = accelerationPartials.getEntry(span.getData());
298 }
299
300 // add the contribution of the current force model
301 entry.increment(new double[] {
302 gradX[paramsIndex], gradY[paramsIndex], gradZ[paramsIndex]
303 });
304 ++paramsIndex;
305 }
306 }
307 }
308
309 // notify observers
310 for (Map.Entry<String, PartialsObserver> observersEntry : partialsObservers.entrySet()) {
311 final DoubleArrayDictionary.Entry entry = accelerationPartials.getEntry(observersEntry.getKey());
312 observersEntry.getValue().partialsComputed(state, factor, entry == null ? new double[SPACE_DIMENSION] : entry.getValue());
313 }
314
315 }
316
317 return factor;
318
319 }
320
321 /**
322 * Method that first checks if it is possible to replace the attitude provider with a computationally cheaper one
323 * to evaluate. If applicable, the new provider only computes the rotation and uses dummy rate and acceleration,
324 * since they should not be used later on.
325 * @param forceModels list of forces
326 * @param attitudeProvider original attitude provider
327 * @return same provider if at least one forces used attitude derivatives, otherwise one wrapping the old one for
328 * the rotation
329 */
330 private static AttitudeProvider wrapAttitudeProviderIfPossible(final List<ForceModel> forceModels,
331 final AttitudeProvider attitudeProvider) {
332 if (forceModels.stream().anyMatch(ForceModel::dependsOnAttitudeRate)) {
333 // at least one force uses an attitude rate, need to keep the original provider
334 return attitudeProvider;
335 } else {
336 // the original provider can be replaced by a lighter one for performance
337 return AttitudeProviderModifier.getFrozenAttitudeProvider(attitudeProvider);
338 }
339 }
340
341 /** Interface for observing partials derivatives. */
342 @FunctionalInterface
343 public interface PartialsObserver {
344
345 /** Callback called when partial derivatives have been computed.
346 * <p>
347 * The factor matrix is:
348 * \[F = \begin{matrix}
349 * 0 & 0 & 0 & 1 & 0 & 0 \\
350 * 0 & 0 & 0 & 0 & 1 & 0 \\
351 * 0 & 0 & 0 & 0 & 0 & 1 \\
352 * \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}\\
353 * \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}\\
354 * \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}
355 * \end{matrix}\]
356 * </p>
357 * @param state current spacecraft state
358 * @param factor factor matrix, flattened along rows
359 * @param accelerationPartials partials derivatives of acceleration with respect to the parameter driver
360 * that was registered (zero if no parameters were not selected or parameter is unknown)
361 */
362 void partialsComputed(SpacecraftState state, double[] factor, double[] accelerationPartials);
363
364 }
365
366 }
367