1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.semianalytical.dsst;
18
19 import java.util.ArrayList;
20 import java.util.Arrays;
21 import java.util.List;
22 import java.util.Map;
23
24 import org.hipparchus.analysis.differentiation.Gradient;
25 import org.orekit.errors.OrekitInternalError;
26 import org.orekit.propagation.FieldSpacecraftState;
27 import org.orekit.propagation.PropagationType;
28 import org.orekit.propagation.SpacecraftState;
29 import org.orekit.propagation.integration.AbstractJacobiansMapper;
30 import org.orekit.propagation.semianalytical.dsst.forces.DSSTForceModel;
31 import org.orekit.propagation.semianalytical.dsst.forces.FieldShortPeriodTerms;
32 import org.orekit.propagation.semianalytical.dsst.utilities.FieldAuxiliaryElements;
33 import org.orekit.utils.ParameterDriver;
34 import org.orekit.utils.ParameterDriversList;
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49 public class DSSTJacobiansMapper extends AbstractJacobiansMapper {
50
51
52
53
54 public static final int STATE_DIMENSION = 6;
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69 private static final int I = 1;
70
71
72 private String name;
73
74
75 private final ParameterDriversList parameters;
76
77
78 private Map<ParameterDriver, Integer> map;
79
80
81 private final DSSTPropagator propagator;
82
83
84 private double[] shortPeriodDerivatives;
85
86
87 private PropagationType propagationType;
88
89
90
91
92
93
94
95
96 DSSTJacobiansMapper(final String name,
97 final ParameterDriversList parameters,
98 final DSSTPropagator propagator,
99 final Map<ParameterDriver, Integer> map,
100 final PropagationType propagationType) {
101
102 super(name, parameters);
103
104 shortPeriodDerivatives = null;
105
106 this.parameters = parameters;
107 this.name = name;
108 this.propagator = propagator;
109 this.map = map;
110 this.propagationType = propagationType;
111
112 }
113
114
115 public void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
116 final double[][] dY1dP, final double[] p) {
117
118
119 int index = 0;
120 for (int i = 0; i < STATE_DIMENSION; ++i) {
121 for (int j = 0; j < STATE_DIMENSION; ++j) {
122 p[index++] = (i == j) ? 1.0 : 0.0;
123 }
124 }
125
126 if (parameters.getNbParams() != 0) {
127
128
129 for (int i = 0; i < STATE_DIMENSION; ++i) {
130 for (int j = 0; j < parameters.getNbParams(); ++j) {
131 p[index++] = dY1dP[i][j];
132 }
133 }
134 }
135
136 }
137
138
139 public void getStateJacobian(final SpacecraftState state, final double[][] dYdY0) {
140
141
142 final double[] p = state.getAdditionalState(name);
143
144 for (int i = 0; i < STATE_DIMENSION; i++) {
145 final double[] row = dYdY0[i];
146 for (int j = 0; j < STATE_DIMENSION; j++) {
147 row[j] = p[i * STATE_DIMENSION + j] + shortPeriodDerivatives[i * STATE_DIMENSION + j];
148 }
149 }
150
151 }
152
153
154
155 public void getParametersJacobian(final SpacecraftState state, final double[][] dYdP) {
156
157 if (parameters.getNbParams() != 0) {
158
159
160 final double[] p = state.getAdditionalState(name);
161
162 for (int i = 0; i < STATE_DIMENSION; i++) {
163 final double[] row = dYdP[i];
164 for (int j = 0; j < parameters.getNbParams(); j++) {
165 row[j] = p[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)] +
166 shortPeriodDerivatives[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)];
167 }
168 }
169
170 }
171
172 }
173
174
175
176
177 @SuppressWarnings("unchecked")
178 public void setShortPeriodJacobians(final SpacecraftState s) {
179
180 final double[] p = s.getAdditionalState(name);
181 if (shortPeriodDerivatives == null) {
182 shortPeriodDerivatives = new double[p.length];
183 }
184
185 switch (propagationType) {
186 case MEAN :
187 break;
188 case OSCULATING :
189
190 final int paramDim = parameters.getNbParams();
191 final int dim = 6;
192 final double[][] dShortPerioddState = new double[dim][dim];
193 final double[][] dShortPerioddParam = new double[dim][paramDim];
194 final DSSTGradientConverter converter = new DSSTGradientConverter(s, propagator.getAttitudeProvider());
195
196
197 for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {
198
199 final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
200 final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);
201 final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);
202
203 final Gradient zero = dsState.getDate().getField().getZero();
204 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<>();
205 shortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(auxiliaryElements, propagationType, dsParameters));
206 forceModel.updateShortPeriodTerms(dsParameters, dsState);
207 final Gradient[] shortPeriod = new Gradient[6];
208 Arrays.fill(shortPeriod, zero);
209 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
210 final Gradient[] spVariation = spt.value(dsState.getOrbit());
211 for (int i = 0; i < spVariation .length; i++) {
212 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
213 }
214 }
215
216 final double[] derivativesASP = shortPeriod[0].getGradient();
217 final double[] derivativesExSP = shortPeriod[1].getGradient();
218 final double[] derivativesEySP = shortPeriod[2].getGradient();
219 final double[] derivativesHxSP = shortPeriod[3].getGradient();
220 final double[] derivativesHySP = shortPeriod[4].getGradient();
221 final double[] derivativesLSP = shortPeriod[5].getGradient();
222
223
224 addToRow(derivativesASP, 0, dShortPerioddState);
225 addToRow(derivativesExSP, 1, dShortPerioddState);
226 addToRow(derivativesEySP, 2, dShortPerioddState);
227 addToRow(derivativesHxSP, 3, dShortPerioddState);
228 addToRow(derivativesHySP, 4, dShortPerioddState);
229 addToRow(derivativesLSP, 5, dShortPerioddState);
230
231 int index = converter.getFreeStateParameters();
232 for (ParameterDriver driver : forceModel.getParametersDrivers()) {
233 if (driver.isSelected()) {
234 final int parameterIndex = map.get(driver);
235 dShortPerioddParam[0][parameterIndex] += derivativesASP[index];
236 dShortPerioddParam[1][parameterIndex] += derivativesExSP[index];
237 dShortPerioddParam[2][parameterIndex] += derivativesEySP[index];
238 dShortPerioddParam[3][parameterIndex] += derivativesHxSP[index];
239 dShortPerioddParam[4][parameterIndex] += derivativesHySP[index];
240 dShortPerioddParam[5][parameterIndex] += derivativesLSP[index];
241 ++index;
242 }
243 }
244 }
245
246
247 for (int i = 0; i < dim; i++) {
248 for (int j = 0; j < dim; j++) {
249 shortPeriodDerivatives[j + dim * i] = dShortPerioddState[i][j];
250 }
251 }
252
253
254 final int columnTop = dim * dim;
255 for (int k = 0; k < paramDim; k++) {
256 for (int i = 0; i < dim; ++i) {
257 shortPeriodDerivatives[columnTop + (i + dim * k)] = dShortPerioddParam[i][k];
258 }
259 }
260 break;
261 default:
262 throw new OrekitInternalError(null);
263 }
264 }
265
266
267
268
269
270
271 private void addToRow(final double[] derivatives, final int index,
272 final double[][] dMeanElementRatedElement) {
273
274 for (int i = 0; i < 6; i++) {
275 dMeanElementRatedElement[index][i] += derivatives[i];
276 }
277
278 }
279
280 }