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
178 @Deprecated
179 public void setShortPeriodJacobians(final SpacecraftState s) {
180 setReferenceState(s);
181 }
182
183
184 @SuppressWarnings("unchecked")
185 @Override
186 public void setReferenceState(final SpacecraftState reference) {
187
188 final double[] p = reference.getAdditionalState(name);
189 if (shortPeriodDerivatives == null) {
190 shortPeriodDerivatives = new double[p.length];
191 }
192
193 switch (propagationType) {
194 case MEAN :
195 break;
196 case OSCULATING :
197
198 final int paramDim = parameters.getNbParams();
199 final int dim = 6;
200 final double[][] dShortPerioddState = new double[dim][dim];
201 final double[][] dShortPerioddParam = new double[dim][paramDim];
202 final DSSTGradientConverter converter = new DSSTGradientConverter(reference, propagator.getAttitudeProvider());
203
204
205 for (final DSSTForceModel forceModel : propagator.getAllForceModels()) {
206
207 final FieldSpacecraftState<Gradient> dsState = converter.getState(forceModel);
208 final Gradient[] dsParameters = converter.getParameters(dsState, forceModel);
209 final FieldAuxiliaryElements<Gradient> auxiliaryElements = new FieldAuxiliaryElements<>(dsState.getOrbit(), I);
210
211 final Gradient zero = dsState.getDate().getField().getZero();
212 final List<FieldShortPeriodTerms<Gradient>> shortPeriodTerms = new ArrayList<>();
213 shortPeriodTerms.addAll(forceModel.initializeShortPeriodTerms(auxiliaryElements, propagationType, dsParameters));
214 forceModel.updateShortPeriodTerms(dsParameters, dsState);
215 final Gradient[] shortPeriod = new Gradient[6];
216 Arrays.fill(shortPeriod, zero);
217 for (final FieldShortPeriodTerms<Gradient> spt : shortPeriodTerms) {
218 final Gradient[] spVariation = spt.value(dsState.getOrbit());
219 for (int i = 0; i < spVariation .length; i++) {
220 shortPeriod[i] = shortPeriod[i].add(spVariation[i]);
221 }
222 }
223
224 final double[] derivativesASP = shortPeriod[0].getGradient();
225 final double[] derivativesExSP = shortPeriod[1].getGradient();
226 final double[] derivativesEySP = shortPeriod[2].getGradient();
227 final double[] derivativesHxSP = shortPeriod[3].getGradient();
228 final double[] derivativesHySP = shortPeriod[4].getGradient();
229 final double[] derivativesLSP = shortPeriod[5].getGradient();
230
231
232 addToRow(derivativesASP, 0, dShortPerioddState);
233 addToRow(derivativesExSP, 1, dShortPerioddState);
234 addToRow(derivativesEySP, 2, dShortPerioddState);
235 addToRow(derivativesHxSP, 3, dShortPerioddState);
236 addToRow(derivativesHySP, 4, dShortPerioddState);
237 addToRow(derivativesLSP, 5, dShortPerioddState);
238
239 int index = converter.getFreeStateParameters();
240 for (ParameterDriver driver : forceModel.getParametersDrivers()) {
241 if (driver.isSelected()) {
242 final int parameterIndex = map.get(driver);
243 dShortPerioddParam[0][parameterIndex] += derivativesASP[index];
244 dShortPerioddParam[1][parameterIndex] += derivativesExSP[index];
245 dShortPerioddParam[2][parameterIndex] += derivativesEySP[index];
246 dShortPerioddParam[3][parameterIndex] += derivativesHxSP[index];
247 dShortPerioddParam[4][parameterIndex] += derivativesHySP[index];
248 dShortPerioddParam[5][parameterIndex] += derivativesLSP[index];
249 ++index;
250 }
251 }
252 }
253
254
255 for (int i = 0; i < dim; i++) {
256 for (int j = 0; j < dim; j++) {
257 shortPeriodDerivatives[j + dim * i] = dShortPerioddState[i][j];
258 }
259 }
260
261
262 final int columnTop = dim * dim;
263 for (int k = 0; k < paramDim; k++) {
264 for (int i = 0; i < dim; ++i) {
265 shortPeriodDerivatives[columnTop + (i + dim * k)] = dShortPerioddParam[i][k];
266 }
267 }
268 break;
269 default:
270 throw new OrekitInternalError(null);
271 }
272 }
273
274
275
276
277
278
279 private void addToRow(final double[] derivatives, final int index,
280 final double[][] dMeanElementRatedElement) {
281
282 for (int i = 0; i < 6; i++) {
283 dMeanElementRatedElement[index][i] += derivatives[i];
284 }
285
286 }
287
288 }