1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.analytical.tle;
18
19 import org.hipparchus.analysis.differentiation.Gradient;
20 import org.orekit.orbits.FieldOrbit;
21 import org.orekit.propagation.FieldSpacecraftState;
22 import org.orekit.propagation.SpacecraftState;
23 import org.orekit.propagation.integration.AbstractJacobiansMapper;
24 import org.orekit.time.AbsoluteDate;
25 import org.orekit.time.FieldAbsoluteDate;
26 import org.orekit.utils.FieldPVCoordinates;
27 import org.orekit.utils.ParameterDriver;
28 import org.orekit.utils.ParameterDriversList;
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45 public class TLEJacobiansMapper extends AbstractJacobiansMapper {
46
47
48 public static final int STATE_DIMENSION = 6;
49
50
51 private final ParameterDriversList parameters;
52
53
54 private final FieldTLEPropagator<Gradient> gPropagator;
55
56
57 private final Gradient[] gParameters;
58
59
60 private double[] stateTransition;
61
62
63
64
65
66
67 public TLEJacobiansMapper(final String name,
68 final ParameterDriversList parameters,
69 final TLEPropagator propagator) {
70 super(name, parameters);
71
72
73 this.parameters = parameters;
74 this.stateTransition = null;
75
76
77 final TLEGradientConverter converter = new TLEGradientConverter(propagator);
78 final FieldSpacecraftState<Gradient> gState = converter.getState();
79 this.gParameters = converter.getParameters(gState);
80 this.gPropagator = converter.getPropagator(gState, gParameters);
81 }
82
83
84 @Override
85 public void setInitialJacobians(final SpacecraftState state, final double[][] dY1dY0,
86 final double[][] dY1dP, final double[] p) {
87
88
89 int index = 0;
90 for (int i = 0; i < STATE_DIMENSION; ++i) {
91 for (int j = 0; j < STATE_DIMENSION; ++j) {
92 p[index++] = (i == j) ? 1.0 : 0.0;
93 }
94 }
95
96 if (parameters.getNbParams() != 0) {
97
98
99 for (int i = 0; i < STATE_DIMENSION; ++i) {
100 for (int j = 0; j < parameters.getNbParams(); ++j) {
101 p[index++] = dY1dP[i][j];
102 }
103 }
104 }
105
106 }
107
108
109 @Override
110 public void getStateJacobian(final SpacecraftState state, final double[][] dYdY0) {
111 computeDerivatives(state);
112 for (int i = 0; i < STATE_DIMENSION; i++) {
113 final double[] row = dYdY0[i];
114 for (int j = 0; j < STATE_DIMENSION; j++) {
115 row[j] = stateTransition[i * STATE_DIMENSION + j];
116 }
117 }
118 }
119
120
121
122 @Override
123 public void getParametersJacobian(final SpacecraftState state, final double[][] dYdP) {
124
125 if (parameters.getNbParams() != 0) {
126
127 computeDerivatives(state);
128 for (int i = 0; i < STATE_DIMENSION; i++) {
129 final double[] row = dYdP[i];
130 for (int j = 0; j < parameters.getNbParams(); j++) {
131 row[j] = stateTransition[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)];
132 }
133 }
134
135 }
136
137 }
138
139
140
141
142 @Deprecated
143 public void analyticalDerivatives(final SpacecraftState s) {
144 computeDerivatives(s);
145 }
146
147
148
149
150
151 private void computeDerivatives(final SpacecraftState state) {
152
153
154 final int dim = STATE_DIMENSION;
155 final int paramDim = parameters.getNbParams();
156 final double[][] stateGrad = new double[dim][dim];
157 final double[][] paramGrad = new double[dim][paramDim];
158
159
160 if (stateTransition == null) {
161 stateTransition = state.getAdditionalState(getName());
162 }
163
164
165 final AbsoluteDate target = state.getDate();
166 final FieldAbsoluteDate<Gradient> init = gPropagator.getTLE().getDate();
167 final double dt = target.durationFrom(init.toAbsoluteDate());
168 final FieldOrbit<Gradient> gOrbit = gPropagator.propagateOrbit(init.shiftedBy(dt), gParameters);
169 final FieldPVCoordinates<Gradient> gPv = gOrbit.getPVCoordinates();
170
171 final double[] derivativesX = gPv.getPosition().getX().getGradient();
172 final double[] derivativesY = gPv.getPosition().getY().getGradient();
173 final double[] derivativesZ = gPv.getPosition().getZ().getGradient();
174 final double[] derivativesVx = gPv.getVelocity().getX().getGradient();
175 final double[] derivativesVy = gPv.getVelocity().getY().getGradient();
176 final double[] derivativesVz = gPv.getVelocity().getZ().getGradient();
177
178
179 addToRow(derivativesX, 0, stateGrad);
180 addToRow(derivativesY, 1, stateGrad);
181 addToRow(derivativesZ, 2, stateGrad);
182 addToRow(derivativesVx, 3, stateGrad);
183 addToRow(derivativesVy, 4, stateGrad);
184 addToRow(derivativesVz, 5, stateGrad);
185
186 int index = TLEGradientConverter.FREE_STATE_PARAMETERS;
187 int parameterIndex = 0;
188 for (ParameterDriver driver : parameters.getDrivers()) {
189 if (driver.isSelected()) {
190 paramGrad[0][parameterIndex] += derivativesX[index];
191 paramGrad[1][parameterIndex] += derivativesY[index];
192 paramGrad[2][parameterIndex] += derivativesZ[index];
193 paramGrad[3][parameterIndex] += derivativesVx[index];
194 paramGrad[4][parameterIndex] += derivativesVy[index];
195 paramGrad[5][parameterIndex] += derivativesVz[index];
196 ++index;
197 }
198 ++parameterIndex;
199 }
200
201
202 for (int i = 0; i < dim; i++) {
203 for (int j = 0; j < dim; j++) {
204 stateTransition[j + dim * i] = stateGrad[i][j];
205 }
206 }
207
208
209 final int columnTop = dim * dim;
210 for (int k = 0; k < paramDim; k++) {
211 for (int i = 0; i < dim; ++i) {
212 stateTransition[columnTop + (i + dim * k)] = paramGrad[i][k];
213 }
214 }
215
216 }
217
218
219
220
221
222
223 private void addToRow(final double[] derivatives, final int index,
224 final double[][] grad) {
225 for (int i = 0; i < 6; i++) {
226 grad[index][i] += derivatives[i];
227 }
228 }
229
230 }