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.hipparchus.util.MathArrays;
22 import org.orekit.attitudes.AttitudeProvider;
23 import org.orekit.forces.ForceModel;
24 import org.orekit.propagation.FieldSpacecraftState;
25
26 import java.util.List;
27
28 /** Generator for State Transition Matrix with the mass included on top of the Cartesian variables.
29 * @author Romain Serra
30 * @since 13.1
31 */
32 class ExtendedStateTransitionMatrixGenerator extends AbstractStateTransitionMatrixGenerator {
33
34 /** Positional state dimension. */
35 private static final int EXTENDED_STATE_DIMENSION = SPACE_DIMENSION * 2 + 1;
36
37 /** Simple constructor.
38 * @param stmName name of the Cartesian STM additional state
39 * @param forceModels force models used in propagation
40 * @param attitudeProvider attitude provider used in propagation
41 */
42 ExtendedStateTransitionMatrixGenerator(final String stmName, final List<ForceModel> forceModels,
43 final AttitudeProvider attitudeProvider) {
44 super(stmName, forceModels, attitudeProvider, EXTENDED_STATE_DIMENSION);
45 }
46
47 /** {@inheritDoc} */
48 @Override
49 void multiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
50 staticMultiplyMatrix(factor, x, y, columns);
51 }
52
53 /** Compute evolution matrix product.
54 * <p>
55 * This method computes \(Y = F \times X\) where the factor matrix is:
56 * \[F = \begin{matrix}
57 * 0 & 0 & 0 & 1 & 0 & 0 & 0 \\
58 * 0 & 0 & 0 & 0 & 1 & 0 & 0 \\
59 * 0 & 0 & 0 & 0 & 0 & 1 & 0 \\
60 * \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} & \sum\frac{da_x}{dm}\\
61 * \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} & \sum\frac{da_y}{dm}\\
62 * \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} & \sum\frac{da_z}{dm}\\
63 * \sum \frac{dmr}{dp_x} & \sum\frac{dmr}{dp_y} & \sum\frac{dmr}{dp_z} & \sum\frac{dmr}{dv_x} & \sum\frac{dmr}{dv_y} & \sum\frac{dmr}{dv_z}
64 * \end{matrix}\]
65 * </p>
66 * @param factor factor matrix
67 * @param x right factor of the multiplication, as a flatten array in row major order
68 * @param y placeholder where to put the result, as a flatten array in row major order
69 * @param columns number of columns of both x and y (so their dimensions are 7 x columns)
70 */
71 static void staticMultiplyMatrix(final double[] factor, final double[] x, final double[] y, final int columns) {
72
73 final int n = SPACE_DIMENSION * columns;
74
75 // handle first three rows by a simple copy
76 System.arraycopy(x, n, y, 0, n);
77
78 // regular multiplication for the last rows
79 for (int j = 0; j < columns; ++j) {
80 y[n + j ] = factor[ 0] * x[j ] + factor[ 1] * x[j + columns] + factor[ 2] * x[j + 2 * columns] +
81 factor[ 3] * x[j + 3 * columns] + factor[ 4] * x[j + 4 * columns] + factor[ 5] * x[j + 5 * columns] +
82 factor[ 6] * x[j + 6 * columns];
83 y[n + j + columns] = factor[ 7] * x[j ] + factor[ 8] * x[j + columns] + factor[ 9] * x[j + 2 * columns] +
84 factor[10] * x[j + 3 * columns] + factor[11] * x[j + 4 * columns] + factor[12] * x[j + 5 * columns] +
85 factor[13] * x[j + 6 * columns];
86 y[n + j + 2 * columns] = factor[14] * x[j ] + factor[15] * x[j + columns] + factor[16] * x[j + 2 * columns] +
87 factor[17] * x[j + 3 * columns] + factor[18] * x[j + 4 * columns] + factor[19] * x[j + 5 * columns] +
88 factor[20] * x[j + 6 * columns];
89 y[n + j + 3 * columns] = factor[21] * x[j ] + factor[22] * x[j + columns] + factor[23] * x[j + 2 * columns] +
90 factor[24] * x[j + 3 * columns] + factor[25] * x[j + 4 * columns] + factor[26] * x[j + 5 * columns] +
91 factor[27] * x[j + 6 * columns];
92 }
93
94 }
95
96 /** {@inheritDoc} */
97 @Override
98 Gradient[] computeRatesPartialsAndUpdateFactor(final ForceModel forceModel,
99 final FieldSpacecraftState<Gradient> fieldState,
100 final Gradient[] parameters, final double[] factor) {
101 final FieldVector3D<Gradient> acceleration = forceModel.acceleration(fieldState, parameters);
102 final double[] gradX = acceleration.getX().getGradient();
103 final double[] gradY = acceleration.getY().getGradient();
104 final double[] gradZ = acceleration.getZ().getGradient();
105
106 // lower left part of the factor matrix
107 factor[ 0] += gradX[0];
108 factor[ 1] += gradX[1];
109 factor[ 2] += gradX[2];
110 factor[ 7] += gradY[0];
111 factor[ 8] += gradY[1];
112 factor[ 9] += gradY[2];
113 factor[14] += gradZ[0];
114 factor[15] += gradZ[1];
115 factor[16] += gradZ[2];
116
117 if (!forceModel.dependsOnPositionOnly()) {
118 // lower right part of the factor matrix
119 factor[ 3] += gradX[3];
120 factor[ 4] += gradX[4];
121 factor[ 5] += gradX[5];
122 factor[ 6] += gradX[6];
123 factor[10] += gradY[3];
124 factor[11] += gradY[4];
125 factor[12] += gradY[5];
126 factor[13] += gradY[6];
127 factor[17] += gradZ[3];
128 factor[18] += gradZ[4];
129 factor[19] += gradZ[5];
130 factor[20] += gradZ[6];
131 }
132
133 // deal with mass w.r.t. state
134 final Gradient massRate = forceModel.getMassDerivative(fieldState, parameters);
135 if (massRate.getValue() != 0.) {
136 final double[] massRateDerivatives = massRate.getGradient();
137 for (int i = 0; i < EXTENDED_STATE_DIMENSION; i++) {
138 factor[21 + i] += massRateDerivatives[i];
139 }
140 }
141
142
143 // stack result
144 final Gradient[] partials = MathArrays.buildArray(massRate.getField(), 4);
145 partials[0] = acceleration.getX();
146 partials[1] = acceleration.getY();
147 partials[2] = acceleration.getZ();
148 partials[3] = massRate;
149 return partials;
150 }
151 }
152