1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17 package org.orekit.orbits;
18
19 import java.util.Arrays;
20
21 import org.hipparchus.RealFieldElement;
22 import org.hipparchus.geometry.euclidean.threed.FieldVector3D;
23 import org.hipparchus.geometry.euclidean.threed.Vector3D;
24 import org.hipparchus.util.FastMath;
25 import org.orekit.errors.OrekitException;
26 import org.orekit.errors.OrekitMessages;
27 import org.orekit.frames.Frame;
28 import org.orekit.time.AbsoluteDate;
29 import org.orekit.time.FieldAbsoluteDate;
30 import org.orekit.utils.FieldPVCoordinates;
31 import org.orekit.utils.PVCoordinates;
32 import org.orekit.utils.ParameterDriver;
33 import org.orekit.utils.ParameterDriversList;
34 import org.orekit.utils.TimeStampedFieldPVCoordinates;
35
36
37
38 public enum OrbitType {
39
40
41 CARTESIAN {
42
43
44 @Override
45 public CartesianOrbit convertType(final Orbit orbit) {
46 return (orbit.getType() == this) ? (CartesianOrbit) orbit : new CartesianOrbit(orbit);
47 }
48
49
50 @Override
51 public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
52 final double[] stateVector, final double[] stateVectorDot) {
53
54 final PVCoordinates pv = orbit.getPVCoordinates();
55 final Vector3D p = pv.getPosition();
56 final Vector3D v = pv.getVelocity();
57
58 stateVector[0] = p.getX();
59 stateVector[1] = p.getY();
60 stateVector[2] = p.getZ();
61 stateVector[3] = v.getX();
62 stateVector[4] = v.getY();
63 stateVector[5] = v.getZ();
64
65 if (stateVectorDot != null) {
66 final Vector3D a = pv.getAcceleration();
67 stateVectorDot[0] = v.getX();
68 stateVectorDot[1] = v.getY();
69 stateVectorDot[2] = v.getZ();
70 stateVectorDot[3] = a.getX();
71 stateVectorDot[4] = a.getY();
72 stateVectorDot[5] = a.getZ();
73 }
74
75 }
76
77
78 @Override
79 public CartesianOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
80 final AbsoluteDate date, final double mu, final Frame frame) {
81
82 final Vector3D p = new Vector3D(stateVector[0], stateVector[1], stateVector[2]);
83 final Vector3D v = new Vector3D(stateVector[3], stateVector[4], stateVector[5]);
84 final Vector3D a;
85 if (stateVectorDot == null) {
86
87 return new CartesianOrbit(new PVCoordinates(p, v), frame, date, mu);
88 } else {
89
90 a = new Vector3D(stateVectorDot[3], stateVectorDot[4], stateVectorDot[5]);
91 return new CartesianOrbit(new PVCoordinates(p, v, a), frame, date, mu);
92 }
93
94 }
95
96
97 @Override
98 public <T extends RealFieldElement<T>> FieldCartesianOrbit<T> convertType(final FieldOrbit<T> orbit) {
99 return (orbit.getType() == this) ? (FieldCartesianOrbit<T>) orbit : new FieldCartesianOrbit<>(orbit);
100 }
101
102
103 @Override
104 public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
105 final PositionAngle type,
106 final T[] stateVector,
107 final T[] stateVectorDot) {
108
109 final TimeStampedFieldPVCoordinates<T> pv = orbit.getPVCoordinates();
110 final FieldVector3D<T> p = pv.getPosition();
111 final FieldVector3D<T> v = pv.getVelocity();
112
113 stateVector[0] = p.getX();
114 stateVector[1] = p.getY();
115 stateVector[2] = p.getZ();
116 stateVector[3] = v.getX();
117 stateVector[4] = v.getY();
118 stateVector[5] = v.getZ();
119
120 if (stateVectorDot != null) {
121 final FieldVector3D<T> a = pv.getAcceleration();
122 stateVectorDot[0] = v.getX();
123 stateVectorDot[1] = v.getY();
124 stateVectorDot[2] = v.getZ();
125 stateVectorDot[3] = a.getX();
126 stateVectorDot[4] = a.getY();
127 stateVectorDot[5] = a.getZ();
128 }
129
130 }
131
132
133 @Override
134 public <T extends RealFieldElement<T>> FieldCartesianOrbit<T> mapArrayToOrbit(final T[] stateVector,
135 final T[] stateVectorDot,
136 final PositionAngle type,
137 final FieldAbsoluteDate<T> date,
138 final double mu, final Frame frame) {
139 final FieldVector3D<T> p = new FieldVector3D<>(stateVector[0], stateVector[1], stateVector[2]);
140 final FieldVector3D<T> v = new FieldVector3D<>(stateVector[3], stateVector[4], stateVector[5]);
141 final FieldVector3D<T> a;
142 if (stateVectorDot == null) {
143
144 return new FieldCartesianOrbit<>(new FieldPVCoordinates<>(p, v), frame, date, mu);
145 } else {
146
147 a = new FieldVector3D<>(stateVectorDot[3], stateVectorDot[4], stateVectorDot[5]);
148 return new FieldCartesianOrbit<>(new FieldPVCoordinates<>(p, v, a), frame, date, mu);
149 }
150
151 }
152
153
154 @Override
155 public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type)
156 throws OrekitException {
157 final ParameterDriversList drivers = new ParameterDriversList();
158 final double[] array = new double[6];
159 mapOrbitToArray(orbit, type, array, null);
160 final double[] scale = scale(dP, orbit);
161 drivers.add(new ParameterDriver(POS_X, array[0], scale[0], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
162 drivers.add(new ParameterDriver(POS_Y, array[1], scale[1], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
163 drivers.add(new ParameterDriver(POS_Z, array[2], scale[2], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
164 drivers.add(new ParameterDriver(VEL_X, array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
165 drivers.add(new ParameterDriver(VEL_Y, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
166 drivers.add(new ParameterDriver(VEL_Z, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
167 return drivers;
168 }
169
170 },
171
172
173 CIRCULAR {
174
175
176 @Override
177 public CircularOrbit convertType(final Orbit orbit) {
178 return (orbit.getType() == this) ? (CircularOrbit) orbit : new CircularOrbit(orbit);
179 }
180
181
182 @Override
183 public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
184 final double[] stateVector, final double[] stateVectorDot) {
185
186 final CircularOrbit circularOrbit = (CircularOrbit) OrbitType.CIRCULAR.convertType(orbit);
187
188 stateVector[0] = circularOrbit.getA();
189 stateVector[1] = circularOrbit.getCircularEx();
190 stateVector[2] = circularOrbit.getCircularEy();
191 stateVector[3] = circularOrbit.getI();
192 stateVector[4] = circularOrbit.getRightAscensionOfAscendingNode();
193 stateVector[5] = circularOrbit.getAlpha(type);
194
195 if (stateVectorDot != null) {
196 if (orbit.hasDerivatives()) {
197 stateVectorDot[0] = circularOrbit.getADot();
198 stateVectorDot[1] = circularOrbit.getCircularExDot();
199 stateVectorDot[2] = circularOrbit.getCircularEyDot();
200 stateVectorDot[3] = circularOrbit.getIDot();
201 stateVectorDot[4] = circularOrbit.getRightAscensionOfAscendingNodeDot();
202 stateVectorDot[5] = circularOrbit.getAlphaDot(type);
203 } else {
204 Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
205 }
206 }
207
208 }
209
210
211 @Override
212 public CircularOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
213 final AbsoluteDate date, final double mu, final Frame frame) {
214 if (stateVectorDot == null) {
215
216 return new CircularOrbit(stateVector[0], stateVector[1], stateVector[2],
217 stateVector[3], stateVector[4], stateVector[5],
218 type, frame, date, mu);
219 } else {
220
221 return new CircularOrbit(stateVector[0], stateVector[1], stateVector[2],
222 stateVector[3], stateVector[4], stateVector[5],
223 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
224 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
225 type, frame, date, mu);
226 }
227 }
228
229
230 @Override
231 public <T extends RealFieldElement<T>> FieldCircularOrbit<T> convertType(final FieldOrbit<T> orbit) {
232 return (orbit.getType() == this) ? (FieldCircularOrbit<T>) orbit : new FieldCircularOrbit<>(orbit);
233 }
234
235
236 @Override
237 public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
238 final PositionAngle type,
239 final T[] stateVector,
240 final T[] stateVectorDot) {
241
242 final FieldCircularOrbit<T> circularOrbit = (FieldCircularOrbit<T>) OrbitType.CIRCULAR.convertType(orbit);
243
244 stateVector[0] = circularOrbit.getA();
245 stateVector[1] = circularOrbit.getCircularEx();
246 stateVector[2] = circularOrbit.getCircularEy();
247 stateVector[3] = circularOrbit.getI();
248 stateVector[4] = circularOrbit.getRightAscensionOfAscendingNode();
249 stateVector[5] = circularOrbit.getAlpha(type);
250
251 if (stateVectorDot != null) {
252 if (orbit.hasDerivatives()) {
253 stateVectorDot[0] = circularOrbit.getADot();
254 stateVectorDot[1] = circularOrbit.getCircularExDot();
255 stateVectorDot[2] = circularOrbit.getCircularEyDot();
256 stateVectorDot[3] = circularOrbit.getIDot();
257 stateVectorDot[4] = circularOrbit.getRightAscensionOfAscendingNodeDot();
258 stateVectorDot[5] = circularOrbit.getAlphaDot(type);
259 } else {
260 Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
261 }
262 }
263
264 }
265
266
267 @Override
268 public <T extends RealFieldElement<T>> FieldCircularOrbit<T> mapArrayToOrbit(final T[] stateVector,
269 final T[] stateVectorDot, final PositionAngle type,
270 final FieldAbsoluteDate<T> date,
271 final double mu, final Frame frame) {
272 if (stateVectorDot == null) {
273
274 return new FieldCircularOrbit<>(stateVector[0], stateVector[1], stateVector[2],
275 stateVector[3], stateVector[4], stateVector[5],
276 type, frame, date, mu);
277 } else {
278
279 return new FieldCircularOrbit<>(stateVector[0], stateVector[1], stateVector[2],
280 stateVector[3], stateVector[4], stateVector[5],
281 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
282 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
283 type, frame, date, mu);
284 }
285 }
286
287
288 @Override
289 public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type)
290 throws OrekitException {
291 final ParameterDriversList drivers = new ParameterDriversList();
292 final double[] array = new double[6];
293 mapOrbitToArray(orbit, type, array, null);
294 final double[] scale = scale(dP, orbit);
295 final String name = type == PositionAngle.MEAN ?
296 MEAN_LAT_ARG :
297 type == PositionAngle.ECCENTRIC ? ECC_LAT_ARG : TRUE_LAT_ARG;
298 drivers.add(new ParameterDriver(A, array[0], scale[0], 0.0, Double.POSITIVE_INFINITY));
299 drivers.add(new ParameterDriver(E_X, array[1], scale[1], -1.0, 1.0));
300 drivers.add(new ParameterDriver(E_Y, array[2], scale[2], -1.0, 1.0));
301 drivers.add(new ParameterDriver(INC, array[3], scale[3], 0.0, FastMath.PI));
302 drivers.add(new ParameterDriver(RAAN, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
303 drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
304 return drivers;
305 }
306
307 },
308
309
310 EQUINOCTIAL {
311
312
313 @Override
314 public EquinoctialOrbit convertType(final Orbit orbit) {
315 return (orbit.getType() == this) ? (EquinoctialOrbit) orbit : new EquinoctialOrbit(orbit);
316 }
317
318
319 @Override
320 public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
321 final double[] stateVector, final double[] stateVectorDot) {
322
323 final EquinoctialOrbit equinoctialOrbit =
324 (EquinoctialOrbit) OrbitType.EQUINOCTIAL.convertType(orbit);
325
326 stateVector[0] = equinoctialOrbit.getA();
327 stateVector[1] = equinoctialOrbit.getEquinoctialEx();
328 stateVector[2] = equinoctialOrbit.getEquinoctialEy();
329 stateVector[3] = equinoctialOrbit.getHx();
330 stateVector[4] = equinoctialOrbit.getHy();
331 stateVector[5] = equinoctialOrbit.getL(type);
332
333 if (stateVectorDot != null) {
334 if (orbit.hasDerivatives()) {
335 stateVectorDot[0] = equinoctialOrbit.getADot();
336 stateVectorDot[1] = equinoctialOrbit.getEquinoctialExDot();
337 stateVectorDot[2] = equinoctialOrbit.getEquinoctialEyDot();
338 stateVectorDot[3] = equinoctialOrbit.getHxDot();
339 stateVectorDot[4] = equinoctialOrbit.getHyDot();
340 stateVectorDot[5] = equinoctialOrbit.getLDot(type);
341 } else {
342 Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
343 }
344 }
345
346 }
347
348
349 @Override
350 public EquinoctialOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
351 final AbsoluteDate date, final double mu, final Frame frame) {
352 if (stateVectorDot == null) {
353
354 return new EquinoctialOrbit(stateVector[0], stateVector[1], stateVector[2],
355 stateVector[3], stateVector[4], stateVector[5],
356 type, frame, date, mu);
357 } else {
358
359 return new EquinoctialOrbit(stateVector[0], stateVector[1], stateVector[2],
360 stateVector[3], stateVector[4], stateVector[5],
361 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
362 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
363 type, frame, date, mu);
364 }
365 }
366
367
368 @Override
369 public <T extends RealFieldElement<T>> FieldEquinoctialOrbit<T> convertType(final FieldOrbit<T> orbit) {
370 return (orbit.getType() == this) ? (FieldEquinoctialOrbit<T>) orbit : new FieldEquinoctialOrbit<>(orbit);
371 }
372
373
374 @Override
375 public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
376 final PositionAngle type,
377 final T[] stateVector,
378 final T[] stateVectorDot) {
379
380 final FieldEquinoctialOrbit<T> equinoctialOrbit =
381 (FieldEquinoctialOrbit<T>) OrbitType.EQUINOCTIAL.convertType(orbit);
382
383 stateVector[0] = equinoctialOrbit.getA();
384 stateVector[1] = equinoctialOrbit.getEquinoctialEx();
385 stateVector[2] = equinoctialOrbit.getEquinoctialEy();
386 stateVector[3] = equinoctialOrbit.getHx();
387 stateVector[4] = equinoctialOrbit.getHy();
388 stateVector[5] = equinoctialOrbit.getL(type);
389
390 if (stateVectorDot != null) {
391 if (orbit.hasDerivatives()) {
392 stateVectorDot[0] = equinoctialOrbit.getADot();
393 stateVectorDot[1] = equinoctialOrbit.getEquinoctialExDot();
394 stateVectorDot[2] = equinoctialOrbit.getEquinoctialEyDot();
395 stateVectorDot[3] = equinoctialOrbit.getHxDot();
396 stateVectorDot[4] = equinoctialOrbit.getHyDot();
397 stateVectorDot[5] = equinoctialOrbit.getLDot(type);
398 } else {
399 Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
400 }
401 }
402
403 }
404
405
406 @Override
407 public <T extends RealFieldElement<T>> FieldEquinoctialOrbit<T> mapArrayToOrbit(final T[] stateVector,
408 final T[] stateVectorDot,
409 final PositionAngle type,
410 final FieldAbsoluteDate<T> date,
411 final double mu, final Frame frame) {
412 if (stateVectorDot == null) {
413
414 return new FieldEquinoctialOrbit<>(stateVector[0], stateVector[1], stateVector[2],
415 stateVector[3], stateVector[4], stateVector[5],
416 type, frame, date, mu);
417 } else {
418
419 return new FieldEquinoctialOrbit<>(stateVector[0], stateVector[1], stateVector[2],
420 stateVector[3], stateVector[4], stateVector[5],
421 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
422 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
423 type, frame, date, mu);
424 }
425 }
426
427
428 @Override
429 public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type)
430 throws OrekitException {
431 final ParameterDriversList drivers = new ParameterDriversList();
432 final double[] array = new double[6];
433 mapOrbitToArray(orbit, type, array, null);
434 final double[] scale = scale(dP, orbit);
435 final String name = type == PositionAngle.MEAN ?
436 MEAN_LON_ARG :
437 type == PositionAngle.ECCENTRIC ? ECC_LON_ARG : TRUE_LON_ARG;
438 drivers.add(new ParameterDriver(A, array[0], scale[0], 0.0, Double.POSITIVE_INFINITY));
439 drivers.add(new ParameterDriver(E_X, array[1], scale[1], -1.0, 1.0));
440 drivers.add(new ParameterDriver(E_Y, array[2], scale[2], -1.0, 1.0));
441 drivers.add(new ParameterDriver(H_X, array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
442 drivers.add(new ParameterDriver(H_Y, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
443 drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
444 return drivers;
445 }
446
447
448 },
449
450
451 KEPLERIAN {
452
453
454 @Override
455 public KeplerianOrbit convertType(final Orbit orbit) {
456 return (orbit.getType() == this) ? (KeplerianOrbit) orbit : new KeplerianOrbit(orbit);
457 }
458
459
460 @Override
461 public void mapOrbitToArray(final Orbit orbit, final PositionAngle type,
462 final double[] stateVector, final double[] stateVectorDot) {
463
464 final KeplerianOrbit keplerianOrbit =
465 (KeplerianOrbit) OrbitType.KEPLERIAN.convertType(orbit);
466
467 stateVector[0] = keplerianOrbit.getA();
468 stateVector[1] = keplerianOrbit.getE();
469 stateVector[2] = keplerianOrbit.getI();
470 stateVector[3] = keplerianOrbit.getPerigeeArgument();
471 stateVector[4] = keplerianOrbit.getRightAscensionOfAscendingNode();
472 stateVector[5] = keplerianOrbit.getAnomaly(type);
473
474 if (stateVectorDot != null) {
475 if (orbit.hasDerivatives()) {
476 stateVectorDot[0] = keplerianOrbit.getADot();
477 stateVectorDot[1] = keplerianOrbit.getEDot();
478 stateVectorDot[2] = keplerianOrbit.getIDot();
479 stateVectorDot[3] = keplerianOrbit.getPerigeeArgumentDot();
480 stateVectorDot[4] = keplerianOrbit.getRightAscensionOfAscendingNodeDot();
481 stateVectorDot[5] = keplerianOrbit.getAnomalyDot(type);
482 } else {
483 Arrays.fill(stateVectorDot, 0, 6, Double.NaN);
484 }
485 }
486
487 }
488
489
490 @Override
491 public KeplerianOrbit mapArrayToOrbit(final double[] stateVector, final double[] stateVectorDot, final PositionAngle type,
492 final AbsoluteDate date, final double mu, final Frame frame) {
493 if (stateVectorDot == null) {
494
495 return new KeplerianOrbit(stateVector[0], stateVector[1], stateVector[2],
496 stateVector[3], stateVector[4], stateVector[5],
497 type, frame, date, mu);
498 } else {
499
500 return new KeplerianOrbit(stateVector[0], stateVector[1], stateVector[2],
501 stateVector[3], stateVector[4], stateVector[5],
502 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
503 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
504 type, frame, date, mu);
505 }
506 }
507
508
509 @Override
510 public <T extends RealFieldElement<T>> FieldKeplerianOrbit<T> convertType(final FieldOrbit<T> orbit) {
511 return (orbit.getType() == this) ? (FieldKeplerianOrbit<T>) orbit : new FieldKeplerianOrbit<>(orbit);
512 }
513
514
515 @Override
516 public <T extends RealFieldElement<T>> void mapOrbitToArray(final FieldOrbit<T> orbit,
517 final PositionAngle type,
518 final T[] stateVector,
519 final T[] stateVectorDot) {
520 final FieldKeplerianOrbit<T> keplerianOrbit =
521 (FieldKeplerianOrbit<T>) OrbitType.KEPLERIAN.convertType(orbit);
522
523 stateVector[0] = keplerianOrbit.getA();
524 stateVector[1] = keplerianOrbit.getE();
525 stateVector[2] = keplerianOrbit.getI();
526 stateVector[3] = keplerianOrbit.getPerigeeArgument();
527 stateVector[4] = keplerianOrbit.getRightAscensionOfAscendingNode();
528 stateVector[5] = keplerianOrbit.getAnomaly(type);
529
530 if (stateVectorDot != null) {
531 if (orbit.hasDerivatives()) {
532 stateVectorDot[0] = keplerianOrbit.getADot();
533 stateVectorDot[1] = keplerianOrbit.getEDot();
534 stateVectorDot[2] = keplerianOrbit.getIDot();
535 stateVectorDot[3] = keplerianOrbit.getPerigeeArgumentDot();
536 stateVectorDot[4] = keplerianOrbit.getRightAscensionOfAscendingNodeDot();
537 stateVectorDot[5] = keplerianOrbit.getAnomalyDot(type);
538 } else {
539 Arrays.fill(stateVectorDot, 0, 6, orbit.getDate().getField().getZero().add(Double.NaN));
540 }
541 }
542
543 }
544
545
546 @Override
547 public <T extends RealFieldElement<T>> FieldKeplerianOrbit<T> mapArrayToOrbit(final T[] stateVector,
548 final T[] stateVectorDot,
549 final PositionAngle type,
550 final FieldAbsoluteDate<T> date,
551 final double mu, final Frame frame) {
552 if (stateVectorDot == null) {
553
554 return new FieldKeplerianOrbit<>(stateVector[0], stateVector[1], stateVector[2],
555 stateVector[3], stateVector[4], stateVector[5],
556 type, frame, date, mu);
557 } else {
558
559 return new FieldKeplerianOrbit<>(stateVector[0], stateVector[1], stateVector[2],
560 stateVector[3], stateVector[4], stateVector[5],
561 stateVectorDot[0], stateVectorDot[1], stateVectorDot[2],
562 stateVectorDot[3], stateVectorDot[4], stateVectorDot[5],
563 type, frame, date, mu);
564 }
565 }
566
567
568 @Override
569 public ParameterDriversList getDrivers(final double dP, final Orbit orbit, final PositionAngle type)
570 throws OrekitException {
571 final ParameterDriversList drivers = new ParameterDriversList();
572 final double[] array = new double[6];
573 mapOrbitToArray(orbit, type, array, null);
574 final double[] scale = scale(dP, orbit);
575 final String name = type == PositionAngle.MEAN ?
576 MEAN_ANOM :
577 type == PositionAngle.ECCENTRIC ? ECC_ANOM : TRUE_ANOM;
578 drivers.add(new ParameterDriver(A, array[0], scale[0], 0.0, Double.POSITIVE_INFINITY));
579 drivers.add(new ParameterDriver(ECC, array[1], scale[1], 0.0, 1.0));
580 drivers.add(new ParameterDriver(INC, array[2], scale[2], 0.0, FastMath.PI));
581 drivers.add(new ParameterDriver(PA, array[3], scale[3], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
582 drivers.add(new ParameterDriver(RAAN, array[4], scale[4], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
583 drivers.add(new ParameterDriver(name, array[5], scale[5], Double.NEGATIVE_INFINITY, Double.POSITIVE_INFINITY));
584 return drivers;
585 }
586
587 };
588
589
590 private static final String POS_X = "Px";
591
592
593 private static final String POS_Y = "Py";
594
595
596 private static final String POS_Z = "Pz";
597
598
599 private static final String VEL_X = "Vx";
600
601
602 private static final String VEL_Y = "Vy";
603
604
605 private static final String VEL_Z = "Vz";
606
607
608 private static final String A = "a";
609
610
611 private static final String ECC = "e";
612
613
614 private static final String E_X = "ex";
615
616
617 private static final String E_Y = "ey";
618
619
620 private static final String INC = "i";
621
622
623 private static final String H_X = "hx";
624
625
626 private static final String H_Y = "hy";
627
628
629 private static final String PA = "ω";
630
631
632 private static final String RAAN = "Ω";
633
634
635 private static final String MEAN_ANOM = "M";
636
637
638 private static final String ECC_ANOM = "E";
639
640
641 private static final String TRUE_ANOM = "v";
642
643
644 private static final String MEAN_LAT_ARG = "αM";
645
646
647 private static final String ECC_LAT_ARG = "αE";
648
649
650 private static final String TRUE_LAT_ARG = "αv";
651
652
653 private static final String MEAN_LON_ARG = "λM";
654
655
656 private static final String ECC_LON_ARG = "λE";
657
658
659 private static final String TRUE_LON_ARG = "λv";
660
661
662
663
664
665
666
667
668
669 public abstract Orbit convertType(Orbit orbit);
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685 public abstract void mapOrbitToArray(Orbit orbit, PositionAngle type, double[] stateVector, double[] stateVectorDot);
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705 public abstract Orbit mapArrayToOrbit(double[] array, double arrayDot[], PositionAngle type,
706 AbsoluteDate date, double mu, Frame frame);
707
708
709
710
711
712
713
714
715
716
717 public abstract <T extends RealFieldElement<T>> FieldOrbit<T> convertType(FieldOrbit<T> orbit);
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734 public abstract <T extends RealFieldElement<T>>void mapOrbitToArray(FieldOrbit<T> orbit, PositionAngle type,
735 T[] stateVector, T[] stateVectorDot);
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756 public abstract <T extends RealFieldElement<T>> FieldOrbit<T> mapArrayToOrbit(T[] array,
757 T[] arrayDot,
758 PositionAngle type,
759 FieldAbsoluteDate<T> date,
760 double mu, Frame frame);
761
762
763
764
765
766
767
768
769 public abstract ParameterDriversList getDrivers(double dP, Orbit orbit,
770 PositionAngle type)
771 throws OrekitException;
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791 protected double[] scale(final double dP, final Orbit orbit)
792 throws OrekitException {
793
794
795 final PVCoordinates pv = orbit.getPVCoordinates();
796 final double r2 = pv.getPosition().getNormSq();
797 final double v = pv.getVelocity().getNorm();
798 final double dV = orbit.getMu() * dP / (v * r2);
799
800 final double[] scale = new double[6];
801
802
803 final double[][] jacobian = new double[6][6];
804 final Orbit converted = convertType(orbit);
805 converted.getJacobianWrtCartesian(PositionAngle.TRUE, jacobian);
806
807 for (int i = 0; i < 6; ++i) {
808 final double[] row = jacobian[i];
809 scale[i] = FastMath.abs(row[0]) * dP +
810 FastMath.abs(row[1]) * dP +
811 FastMath.abs(row[2]) * dP +
812 FastMath.abs(row[3]) * dV +
813 FastMath.abs(row[4]) * dV +
814 FastMath.abs(row[5]) * dV;
815 if (Double.isNaN(scale[i])) {
816 throw new OrekitException(OrekitMessages.SINGULAR_JACOBIAN_FOR_ORBIT_TYPE, this);
817 }
818 }
819
820 return scale;
821
822 }
823
824 }