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.geometry.euclidean.threed.FieldVector3D;
21 import org.orekit.attitudes.AttitudeProvider;
22 import org.orekit.forces.ForceModel;
23 import org.orekit.propagation.FieldSpacecraftState;
24
25 import java.util.List;
26
27 /** Generator for State Transition Matrix.
28 * The state is made up of the Cartesian position and velocity vectors.
29 * @author Luc Maisonobe
30 * @author Melina Vanel
31 * @since 11.1
32 */
33 class StateTransitionMatrixGenerator extends AbstractStateTransitionMatrixGenerator {
34
35 /**
36 * State dimension.
37 */
38 public static final int STATE_DIMENSION = 2 * SPACE_DIMENSION;
39
40 /**
41 * Simple constructor.
42 *
43 * @param stmName name of the Cartesian STM additional state
44 * @param forceModels force models used in propagation
45 * @param attitudeProvider attitude provider used in propagation
46 */
47 StateTransitionMatrixGenerator(final String stmName, final List<ForceModel> forceModels,
48 final AttitudeProvider attitudeProvider) {
49 super(stmName, forceModels, attitudeProvider, STATE_DIMENSION);
50 }
51
52 /** {@inheritDoc} */
53 @Override
54 protected void multiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
55 staticMultiplyMatrix(factor, x, y, columns);
56 }
57
58 /** Compute evolution matrix product.
59 * <p>
60 * This method computes \(Y = F \times X\) where the factor matrix is:
61 * \[F = \begin{matrix}
62 * 0 & 0 & 0 & 1 & 0 & 0 \\
63 * 0 & 0 & 0 & 0 & 1 & 0 \\
64 * 0 & 0 & 0 & 0 & 0 & 1 \\
65 * \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}\\
66 * \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}\\
67 * \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}
68 * \end{matrix}\]
69 * </p>
70 * @param factor factor matrix
71 * @param x right factor of the multiplication, as a flatten array in row major order
72 * @param y placeholder where to put the result, as a flatten array in row major order
73 * @param columns number of columns of both x and y (so their dimensions are 6 x columns)
74 */
75 static void staticMultiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
76
77 final int n = SPACE_DIMENSION * columns;
78
79 // handle first three rows by a simple copy
80 System.arraycopy(x, n, y, 0, n);
81
82 // regular multiplication for the last three rows
83 for (int j = 0; j < columns; ++j) {
84 y[n + j ] = factor[ 0] * x[j ] + factor[ 1] * x[j + columns] + factor[ 2] * x[j + 2 * columns] +
85 factor[ 3] * x[j + 3 * columns] + factor[ 4] * x[j + 4 * columns] + factor[ 5] * x[j + 5 * columns];
86 y[n + j + columns] = factor[ 6] * x[j ] + factor[ 7] * x[j + columns] + factor[ 8] * x[j + 2 * columns] +
87 factor[ 9] * x[j + 3 * columns] + factor[10] * x[j + 4 * columns] + factor[11] * x[j + 5 * columns];
88 y[n + j + 2 * columns] = factor[12] * x[j ] + factor[13] * x[j + columns] + factor[14] * x[j + 2 * columns] +
89 factor[15] * x[j + 3 * columns] + factor[16] * x[j + 4 * columns] + factor[17] * x[j + 5 * columns];
90 }
91
92 }
93
94 /** {@inheritDoc} */
95 @Override
96 Gradient[] computeRatesPartialsAndUpdateFactor(final ForceModel forceModel,
97 final FieldSpacecraftState<Gradient> fieldState,
98 final Gradient[] parameters, final double[] factor) {
99 final FieldVector3D<Gradient> acceleration = forceModel.acceleration(fieldState, parameters);
100 final double[] gradX = acceleration.getX().getGradient();
101 final double[] gradY = acceleration.getY().getGradient();
102 final double[] gradZ = acceleration.getZ().getGradient();
103
104 // lower left part of the factor matrix
105 factor[ 0] += gradX[0];
106 factor[ 1] += gradX[1];
107 factor[ 2] += gradX[2];
108 factor[ 6] += gradY[0];
109 factor[ 7] += gradY[1];
110 factor[ 8] += gradY[2];
111 factor[12] += gradZ[0];
112 factor[13] += gradZ[1];
113 factor[14] += gradZ[2];
114
115 if (!forceModel.dependsOnPositionOnly()) {
116 // lower right part of the factor matrix
117 factor[ 3] += gradX[3];
118 factor[ 4] += gradX[4];
119 factor[ 5] += gradX[5];
120 factor[ 9] += gradY[3];
121 factor[10] += gradY[4];
122 factor[11] += gradY[5];
123 factor[15] += gradZ[3];
124 factor[16] += gradZ[4];
125 factor[17] += gradZ[5];
126 }
127 return acceleration.toArray();
128 }
129 }
130