1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.propagation.integration;
18
19 import java.util.ArrayList;
20 import java.util.List;
21
22 import org.hipparchus.Field;
23 import org.hipparchus.analysis.differentiation.Gradient;
24 import org.hipparchus.analysis.differentiation.GradientField;
25 import org.hipparchus.geometry.euclidean.threed.FieldRotation;
26 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
27 import org.hipparchus.geometry.euclidean.threed.Vector3D;
28 import org.orekit.attitudes.AttitudeProvider;
29 import org.orekit.attitudes.FieldAttitude;
30 import org.orekit.orbits.OrbitType;
31 import org.orekit.orbits.FieldCartesianOrbit;
32 import org.orekit.orbits.FieldEquinoctialOrbit;
33 import org.orekit.orbits.FieldOrbit;
34 import org.orekit.orbits.PositionAngleType;
35 import org.orekit.propagation.FieldSpacecraftState;
36 import org.orekit.propagation.SpacecraftState;
37 import org.orekit.time.AbsoluteDate;
38 import org.orekit.time.FieldAbsoluteDate;
39 import org.orekit.utils.FieldAngularCoordinates;
40 import org.orekit.utils.FieldPVCoordinates;
41 import org.orekit.utils.FieldAbsolutePVCoordinates;
42 import org.orekit.utils.ParameterDriver;
43 import org.orekit.utils.ParameterDriversProvider;
44 import org.orekit.utils.TimeStampedFieldAngularCoordinates;
45 import org.orekit.utils.TimeStampedFieldPVCoordinates;
46 import org.orekit.utils.TimeSpanMap.Span;
47
48
49
50
51
52
53 public abstract class AbstractGradientConverter {
54
55
56 private final int freeStateParameters;
57
58
59 private final List<FieldSpacecraftState<Gradient>> gStates;
60
61
62
63
64 protected AbstractGradientConverter(final int freeStateParameters) {
65 this.freeStateParameters = freeStateParameters;
66 this.gStates = new ArrayList<>();
67 }
68
69
70
71
72 public int getFreeStateParameters() {
73 return freeStateParameters;
74 }
75
76
77
78
79
80 protected void initStates(final FieldSpacecraftState<Gradient> zeroParametersState) {
81 gStates.clear();
82 gStates.add(zeroParametersState);
83 }
84
85
86
87
88
89
90 protected Gradient extend(final Gradient original, final int freeParameters) {
91 final double[] originalDerivatives = original.getGradient();
92 final double[] extendedDerivatives = new double[freeParameters];
93 System.arraycopy(originalDerivatives, 0, extendedDerivatives, 0, originalDerivatives.length);
94 return new Gradient(original.getValue(), extendedDerivatives);
95 }
96
97
98
99
100
101
102
103
104 protected FieldAbsoluteDate<Gradient> extend(final FieldAbsoluteDate<Gradient> original, final int freeParameters) {
105 final AbsoluteDate date = original.toAbsoluteDate();
106 final Gradient gradient = original.durationFrom(date);
107 return new FieldAbsoluteDate<>(date, extend(gradient, freeParameters));
108 }
109
110
111
112
113
114
115 protected FieldVector3D<Gradient> extend(final FieldVector3D<Gradient> original, final int freeParameters) {
116 return new FieldVector3D<>(extend(original.getX(), freeParameters),
117 extend(original.getY(), freeParameters),
118 extend(original.getZ(), freeParameters));
119 }
120
121
122
123
124
125
126 protected FieldRotation<Gradient> extend(final FieldRotation<Gradient> original, final int freeParameters) {
127 return new FieldRotation<>(extend(original.getQ0(), freeParameters),
128 extend(original.getQ1(), freeParameters),
129 extend(original.getQ2(), freeParameters),
130 extend(original.getQ3(), freeParameters),
131 false);
132 }
133
134
135
136
137
138
139
140
141 protected static FieldSpacecraftState<Gradient> buildBasicGradientSpacecraftState(final SpacecraftState state,
142 final int freeStateParameters,
143 final AttitudeProvider provider) {
144
145
146 final Field<Gradient> field = GradientField.getField(freeStateParameters);
147
148
149 final Vector3D pos = state.getPosition();
150 final FieldVector3D<Gradient> posG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 0, pos.getX()),
151 Gradient.variable(freeStateParameters, 1, pos.getY()),
152 Gradient.variable(freeStateParameters, 2, pos.getZ()));
153
154
155 final Vector3D vel = state.getPVCoordinates().getVelocity();
156 final FieldVector3D<Gradient> velG;
157 if (freeStateParameters > 3) {
158 velG = new FieldVector3D<>(Gradient.variable(freeStateParameters, 3, vel.getX()),
159 Gradient.variable(freeStateParameters, 4, vel.getY()),
160 Gradient.variable(freeStateParameters, 5, vel.getZ()));
161 } else {
162 velG = new FieldVector3D<>(field, vel);
163 }
164
165
166 final Vector3D acc = state.getPVCoordinates().getAcceleration();
167 final FieldVector3D<Gradient> accG = new FieldVector3D<>(field, acc);
168
169
170 final Gradient gMass = Gradient.constant(freeStateParameters, state.getMass());
171
172 final TimeStampedFieldPVCoordinates<Gradient> timeStampedFieldPVCoordinates =
173 new TimeStampedFieldPVCoordinates<>(state.getDate(), posG, velG, accG);
174
175 final FieldCartesianOrbit<Gradient> gOrbit;
176 final FieldAbsolutePVCoordinates<Gradient> gAbsolutePV;
177 if (state.isOrbitDefined()) {
178 final Gradient gMu = Gradient.constant(freeStateParameters, state.getOrbit().getMu());
179 gOrbit = new FieldCartesianOrbit<>(timeStampedFieldPVCoordinates, state.getFrame(), gMu);
180 gAbsolutePV = null;
181 } else {
182 gOrbit = null;
183 gAbsolutePV = new FieldAbsolutePVCoordinates<>(state.getFrame(), timeStampedFieldPVCoordinates);
184 }
185
186 final FieldAttitude<Gradient> gAttitude;
187 if (freeStateParameters > 3) {
188
189 gAttitude = provider.getAttitude((state.isOrbitDefined()) ? gOrbit : gAbsolutePV,
190 timeStampedFieldPVCoordinates.getDate(), state.getFrame());
191 } else {
192
193 gAttitude = new FieldAttitude<>(field, state.getAttitude());
194 }
195
196 if (state.isOrbitDefined()) {
197 return new FieldSpacecraftState<>(gOrbit, gAttitude).withMass(gMass);
198 } else {
199 return new FieldSpacecraftState<>(gAbsolutePV, gAttitude).withMass(gMass);
200 }
201
202 }
203
204
205
206
207
208
209 public FieldSpacecraftState<Gradient> getState(final ParameterDriversProvider parametricModel) {
210
211
212 int nbParams = 0;
213 for (final ParameterDriver driver : parametricModel.getParametersDrivers()) {
214 if (driver.isSelected()) {
215 nbParams += driver.getNbOfValues();
216 }
217 }
218
219
220 while (gStates.size() < nbParams + 1) {
221 gStates.add(null);
222 }
223
224 if (gStates.get(nbParams) == null) {
225
226
227 final int freeParameters = freeStateParameters + nbParams;
228 final FieldSpacecraftState<Gradient> s0 = gStates.get(0);
229 final AbsoluteDate date = s0.getDate().toAbsoluteDate();
230
231
232 final FieldAngularCoordinates<Gradient> ac0 = s0.getAttitude().getOrientation();
233 final FieldAttitude<Gradient> gAttitude =
234 new FieldAttitude<>(s0.getAttitude().getReferenceFrame(),
235 new TimeStampedFieldAngularCoordinates<>(date,
236 extend(ac0.getRotation(), freeParameters),
237 extend(ac0.getRotationRate(), freeParameters),
238 extend(ac0.getRotationAcceleration(), freeParameters)));
239
240
241 final Gradient gMass = extend(s0.getMass(), freeParameters);
242
243
244 final FieldPVCoordinates<Gradient> pv0 = s0.getPVCoordinates();
245 final TimeStampedFieldPVCoordinates<Gradient> timeStampedFieldPVCoordinates = new TimeStampedFieldPVCoordinates<>(
246 date,
247 extend(pv0.getPosition(), freeParameters),
248 extend(pv0.getVelocity(), freeParameters),
249 extend(pv0.getAcceleration(), freeParameters));
250 final FieldSpacecraftState<Gradient> spacecraftState;
251 if (s0.isOrbitDefined()) {
252 final FieldOrbit<Gradient> orbit = s0.getOrbit();
253 if (orbit.getType().equals(OrbitType.EQUINOCTIAL)) {
254
255 spacecraftState =
256 new FieldSpacecraftState<>(new FieldEquinoctialOrbit<>(extend(orbit.getA(), freeParameters),
257 extend(orbit.getEquinoctialEx(), freeParameters),
258 extend(orbit.getEquinoctialEy(), freeParameters),
259 extend(orbit.getHx(), freeParameters),
260 extend(orbit.getHy(), freeParameters),
261 extend(orbit.getLM(), freeParameters),
262 PositionAngleType.MEAN,
263 s0.getFrame(),
264 extend(s0.getDate(), freeParameters),
265 extend(orbit.getMu(), freeParameters)),
266 gAttitude).withMass(gMass);
267 } else {
268 spacecraftState =
269 new FieldSpacecraftState<>(new FieldCartesianOrbit<>(timeStampedFieldPVCoordinates,
270 s0.getFrame(),
271 extend(orbit.getMu(), freeParameters)),
272 gAttitude).withMass(gMass);
273 }
274 } else {
275 spacecraftState =
276 new FieldSpacecraftState<>(new FieldAbsolutePVCoordinates<>(s0.getFrame(),
277 timeStampedFieldPVCoordinates),
278 gAttitude).withMass(gMass);
279 }
280
281 gStates.set(nbParams, spacecraftState);
282
283 }
284
285 return gStates.get(nbParams);
286
287 }
288
289
290
291
292
293
294
295
296
297
298 public Gradient[] getParameters(final FieldSpacecraftState<Gradient> state,
299 final ParameterDriversProvider parametricModel) {
300 final int freeParameters = state.getMass().getFreeParameters();
301 final List<ParameterDriver> drivers = parametricModel.getParametersDrivers();
302 int sizeDrivers = 0;
303 for ( ParameterDriver driver : drivers) {
304 sizeDrivers += driver.getNbOfValues();
305 }
306 final Gradient[] parameters = new Gradient[sizeDrivers];
307 int index = freeStateParameters;
308 int i = 0;
309 for (ParameterDriver driver : drivers) {
310
311 for (Span<Double> span = driver.getValueSpanMap().getFirstSpan(); span != null; span = span.next()) {
312 parameters[i++] = driver.isSelected() ?
313 Gradient.variable(freeParameters, index++, span.getData()) :
314 Gradient.constant(freeParameters, span.getData());
315 }
316 }
317 return parameters;
318 }
319
320
321
322
323
324
325
326
327
328
329 public Gradient[] getParametersAtStateDate(final FieldSpacecraftState<Gradient> state,
330 final ParameterDriversProvider parametricModel) {
331 final int freeParameters = state.getMass().getFreeParameters();
332 final List<ParameterDriver> drivers = parametricModel.getParametersDrivers();
333
334 final Gradient[] parameters = new Gradient[drivers.size()];
335 int index = freeStateParameters;
336 int i = 0;
337 for (ParameterDriver driver : drivers) {
338 for (Span<String> span = driver.getNamesSpanMap().getFirstSpan(); span != null; span = span.next()) {
339 if (span.getData().equals(driver.getNameSpan(state.getDate().toAbsoluteDate()))) {
340 parameters[i++] = driver.isSelected() ?
341 Gradient.variable(freeParameters, index, driver.getValue(state.getDate().toAbsoluteDate())) :
342 Gradient.constant(freeParameters, driver.getValue(state.getDate().toAbsoluteDate()));
343 }
344 index = driver.isSelected() ? index + 1 : index;
345 }
346 }
347 return parameters;
348 }
349
350
351 }