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 for (int i = 0; i < STATE_DIMENSION; i++) {
112 final double[] row = dYdY0[i];
113 for (int j = 0; j < STATE_DIMENSION; j++) {
114 row[j] = stateTransition[i * STATE_DIMENSION + j];
115 }
116 }
117 }
118
119
120
121 @Override
122 public void getParametersJacobian(final SpacecraftState state, final double[][] dYdP) {
123
124 if (parameters.getNbParams() != 0) {
125
126 for (int i = 0; i < STATE_DIMENSION; i++) {
127 final double[] row = dYdP[i];
128 for (int j = 0; j < parameters.getNbParams(); j++) {
129 row[j] = stateTransition[STATE_DIMENSION * STATE_DIMENSION + (j + parameters.getNbParams() * i)];
130 }
131 }
132
133 }
134
135 }
136
137
138 @Override
139 public void analyticalDerivatives(final SpacecraftState s) {
140
141
142 final int dim = STATE_DIMENSION;
143 final int paramDim = parameters.getNbParams();
144 final double[][] stateGrad = new double[dim][dim];
145 final double[][] paramGrad = new double[dim][paramDim];
146
147
148 if (stateTransition == null) {
149 stateTransition = s.getAdditionalState(getName());
150 }
151
152
153 final AbsoluteDate target = s.getDate();
154 final FieldAbsoluteDate<Gradient> init = gPropagator.getTLE().getDate();
155 final double dt = target.durationFrom(init.toAbsoluteDate());
156 final FieldOrbit<Gradient> gOrbit = gPropagator.propagateOrbit(init.shiftedBy(dt), gParameters);
157 final FieldPVCoordinates<Gradient> gPv = gOrbit.getPVCoordinates();
158
159 final double[] derivativesX = gPv.getPosition().getX().getGradient();
160 final double[] derivativesY = gPv.getPosition().getY().getGradient();
161 final double[] derivativesZ = gPv.getPosition().getZ().getGradient();
162 final double[] derivativesVx = gPv.getVelocity().getX().getGradient();
163 final double[] derivativesVy = gPv.getVelocity().getY().getGradient();
164 final double[] derivativesVz = gPv.getVelocity().getZ().getGradient();
165
166
167 addToRow(derivativesX, 0, stateGrad);
168 addToRow(derivativesY, 1, stateGrad);
169 addToRow(derivativesZ, 2, stateGrad);
170 addToRow(derivativesVx, 3, stateGrad);
171 addToRow(derivativesVy, 4, stateGrad);
172 addToRow(derivativesVz, 5, stateGrad);
173
174 int index = TLEGradientConverter.FREE_STATE_PARAMETERS;
175 int parameterIndex = 0;
176 for (ParameterDriver driver : parameters.getDrivers()) {
177 if (driver.isSelected()) {
178 paramGrad[0][parameterIndex] += derivativesX[index];
179 paramGrad[1][parameterIndex] += derivativesY[index];
180 paramGrad[2][parameterIndex] += derivativesZ[index];
181 paramGrad[3][parameterIndex] += derivativesVx[index];
182 paramGrad[4][parameterIndex] += derivativesVy[index];
183 paramGrad[5][parameterIndex] += derivativesVz[index];
184 ++index;
185 }
186 ++parameterIndex;
187 }
188
189
190 for (int i = 0; i < dim; i++) {
191 for (int j = 0; j < dim; j++) {
192 stateTransition[j + dim * i] = stateGrad[i][j];
193 }
194 }
195
196
197 final int columnTop = dim * dim;
198 for (int k = 0; k < paramDim; k++) {
199 for (int i = 0; i < dim; ++i) {
200 stateTransition[columnTop + (i + dim * k)] = paramGrad[i][k];
201 }
202 }
203
204 }
205
206
207
208
209
210
211 private void addToRow(final double[] derivatives, final int index,
212 final double[][] grad) {
213 for (int i = 0; i < 6; i++) {
214 grad[index][i] += derivatives[i];
215 }
216 }
217
218 }