1 /* Copyright 2002-2025 CS GROUP
2 * Licensed to CS GROUP (CS) under one or more
3 * contributor license agreements. See the NOTICE file distributed with
4 * this work for additional information regarding copyright ownership.
5 * CS licenses this file to You under the Apache License, Version 2.0
6 * (the "License"); you may not use this file except in compliance with
7 * the License. You may obtain a copy of the License at
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 */
17 package org.orekit.propagation;
18
19 import org.hipparchus.CalculusFieldElement;
20 import org.hipparchus.Field;
21 import org.hipparchus.analysis.differentiation.FieldGradient;
22 import org.hipparchus.analysis.differentiation.FieldGradientField;
23 import org.hipparchus.linear.Array2DRowFieldMatrix;
24 import org.hipparchus.linear.BlockRealMatrix;
25 import org.hipparchus.linear.FieldMatrix;
26 import org.hipparchus.linear.MatrixUtils;
27 import org.hipparchus.util.MathArrays;
28 import org.orekit.errors.OrekitException;
29 import org.orekit.errors.OrekitMessages;
30 import org.orekit.frames.FieldKinematicTransform;
31 import org.orekit.frames.Frame;
32 import org.orekit.frames.LOF;
33 import org.orekit.orbits.FieldOrbit;
34 import org.orekit.orbits.OrbitType;
35 import org.orekit.orbits.PositionAngleType;
36 import org.orekit.time.AbsoluteDate;
37 import org.orekit.time.FieldAbsoluteDate;
38 import org.orekit.time.FieldTimeStamped;
39
40 /**
41 * This class is the representation of a covariance matrix at a given date.
42 * <p>
43 * Currently, the covariance only represents the orbital elements.
44 * <p>
45 * It is possible to change the covariance frame by using the {@link #changeCovarianceFrame(FieldOrbit, Frame)} or
46 * {@link #changeCovarianceFrame(FieldOrbit, LOF)} method. These methods are based on Equations (18) and (20) of
47 * <i>Covariance Transformations for Satellite Flight Dynamics Operations</i> by David A. SVallado.
48 * <p>
49 * Finally, covariance orbit type can be changed using the
50 * {@link #changeCovarianceType(FieldOrbit, OrbitType, PositionAngleType) changeCovarianceType(FieldOrbit, OrbitType,
51 * PositionAngle)} method.
52 * </p>
53 *
54 * @author Bryan Cazabonne
55 * @author Vincent Cucchietti
56 * @since 12.0
57 * @param <T> type of the field elements
58 */
59 public class FieldStateCovariance<T extends CalculusFieldElement<T>> implements FieldTimeStamped<T> {
60
61 /** State dimension. */
62 private static final int STATE_DIMENSION = 6;
63
64 /** Default position angle for covariance expressed in cartesian elements. */
65 private static final PositionAngleType DEFAULT_POSITION_ANGLE = PositionAngleType.TRUE;
66
67 /** Orbital covariance [6x6]. */
68 private final FieldMatrix<T> orbitalCovariance;
69
70 /** Covariance frame (can be null if LOF is defined). */
71 private final Frame frame;
72
73 /** Covariance LOF type (can be null if frame is defined). */
74 private final LOF lof;
75
76 /** Covariance epoch. */
77 private final FieldAbsoluteDate<T> epoch;
78
79 /** Covariance orbit type. */
80 private final OrbitType orbitType;
81
82 /** Covariance position angle type (not used if orbitType is CARTESIAN). */
83 private final PositionAngleType angleType;
84
85 /**
86 * Constructor.
87 *
88 * @param orbitalCovariance 6x6 orbital parameters covariance
89 * @param epoch epoch of the covariance
90 * @param lof covariance LOF type
91 */
92 public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
93 final LOF lof) {
94 this(orbitalCovariance, epoch, null, lof, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
95 }
96
97 /**
98 * Constructor.
99 *
100 * @param orbitalCovariance 6x6 orbital parameters covariance
101 * @param epoch epoch of the covariance
102 * @param covarianceFrame covariance frame (inertial or Earth fixed)
103 * @param orbitType orbit type of the covariance (CARTESIAN if covarianceFrame is not pseudo-inertial)
104 * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
105 */
106 public FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
107 final Frame covarianceFrame,
108 final OrbitType orbitType, final PositionAngleType angleType) {
109 this(orbitalCovariance, epoch, covarianceFrame, null, orbitType, angleType);
110 }
111
112 /**
113 * Private constructor.
114 *
115 * @param orbitalCovariance 6x6 orbital parameters covariance
116 * @param epoch epoch of the covariance
117 * @param covarianceFrame covariance frame (inertial or Earth fixed)
118 * @param lof covariance LOF type
119 * @param orbitType orbit type of the covariance
120 * @param angleType position angle type of the covariance (not used if orbitType is CARTESIAN)
121 */
122 private FieldStateCovariance(final FieldMatrix<T> orbitalCovariance, final FieldAbsoluteDate<T> epoch,
123 final Frame covarianceFrame, final LOF lof,
124 final OrbitType orbitType, final PositionAngleType angleType) {
125
126 StateCovariance.checkFrameAndOrbitTypeConsistency(covarianceFrame, orbitType);
127
128 this.orbitalCovariance = orbitalCovariance;
129 this.epoch = epoch;
130 this.frame = covarianceFrame;
131 this.lof = lof;
132 this.orbitType = orbitType;
133 this.angleType = angleType;
134
135 }
136
137 /** {@inheritDoc}. */
138 @Override
139 public FieldAbsoluteDate<T> getDate() {
140 return epoch;
141 }
142
143 /**
144 * Get the covariance matrix.
145 *
146 * @return the covariance matrix
147 */
148 public FieldMatrix<T> getMatrix() {
149 return orbitalCovariance;
150 }
151
152 /**
153 * Get the covariance orbit type.
154 *
155 * @return the covariance orbit type
156 */
157 public OrbitType getOrbitType() {
158 return orbitType;
159 }
160
161 /**
162 * Get the covariance angle type.
163 *
164 * @return the covariance angle type
165 */
166 public PositionAngleType getPositionAngleType() {
167 return angleType;
168 }
169
170 /**
171 * Get the covariance frame.
172 *
173 * @return the covariance frame (can be null)
174 *
175 * @see #getLOF()
176 */
177 public Frame getFrame() {
178 return frame;
179 }
180
181 /**
182 * Get the covariance LOF type.
183 *
184 * @return the covariance LOF type (can be null)
185 *
186 * @see #getFrame()
187 */
188 public LOF getLOF() {
189 return lof;
190 }
191
192 /**
193 * Get the covariance matrix in another orbit type.
194 * <p>
195 * The covariance orbit type <b>cannot</b> be changed if the covariance
196 * matrix is expressed in a {@link LOF local orbital frame} or a
197 * non-pseudo inertial frame.
198 * <p>
199 * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
200 * Hence, the current covariance matrix <b>will not exactly match</b> the new linearized case and the
201 * distribution will not follow a generalized Gaussian distribution anymore.
202 * <p>
203 * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
204 * dynamics operations."
205 *
206 * @param orbit orbit to which the covariance matrix should correspond
207 * @param outOrbitType target orbit type of the state covariance matrix
208 * @param outAngleType target position angle type of the state covariance matrix
209 *
210 * @return a new covariance state, expressed in the target orbit type with the target position angle
211 *
212 * @see #changeCovarianceFrame(FieldOrbit, Frame)
213 */
214 public FieldStateCovariance<T> changeCovarianceType(final FieldOrbit<T> orbit, final OrbitType outOrbitType,
215 final PositionAngleType outAngleType) {
216
217 // Handle case where the covariance is already expressed in the output type
218 if (outOrbitType == orbitType && (outOrbitType == OrbitType.CARTESIAN || outAngleType == angleType)) {
219 if (lof == null) {
220 return new FieldStateCovariance<>(orbitalCovariance, epoch, frame, orbitType, angleType);
221 }
222 else {
223 return new FieldStateCovariance<>(orbitalCovariance, epoch, lof);
224 }
225 }
226
227 // Check if the covariance is expressed in a celestial body frame
228 if (frame != null) {
229
230 // Check if the covariance is defined in an inertial frame
231 if (frame.isPseudoInertial()) {
232 return changeTypeAndCreate(orbit, epoch, frame, orbitType, angleType, outOrbitType, outAngleType,
233 orbitalCovariance);
234 }
235
236 // The covariance is not defined in an inertial frame. The orbit type cannot be changed
237 throw new OrekitException(OrekitMessages.CANNOT_CHANGE_COVARIANCE_TYPE_IF_DEFINED_IN_NON_INERTIAL_FRAME);
238
239 }
240
241 // The covariance is not expressed in a celestial body frame. The orbit type cannot be changed
242 throw new OrekitException(OrekitMessages.CANNOT_CHANGE_COVARIANCE_TYPE_IF_DEFINED_IN_LOF);
243
244 }
245
246 /**
247 * Get the covariance in a given local orbital frame.
248 * <p>
249 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
250 * in covariance orbit type is required.
251 * <p>
252 * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
253 * flight dynamics operations."
254 *
255 * @param orbit orbit to which the covariance matrix should correspond
256 * @param lofOut output local orbital frame
257 *
258 * @return a new covariance state, expressed in the output local orbital frame
259 */
260 public FieldStateCovariance<T> changeCovarianceFrame(final FieldOrbit<T> orbit, final LOF lofOut) {
261
262 // Verify current covariance frame
263 if (lof != null) {
264
265 // Check specific case where output covariance will be the same
266 if (lofOut == lof) {
267 return new FieldStateCovariance<>(orbitalCovariance, epoch, lof);
268 }
269
270 // Change the covariance local orbital frame
271 return changeFrameAndCreate(orbit, epoch, lof, lofOut, orbitalCovariance);
272
273 } else {
274
275 // Covariance is expressed in celestial body frame
276 return changeFrameAndCreate(orbit, epoch, frame, lofOut, orbitalCovariance, orbitType, angleType);
277
278 }
279
280 }
281
282 /**
283 * Get the covariance in the output frame.
284 * <p>
285 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
286 * in covariance orbit type is required.
287 * <p>
288 * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
289 * flight dynamics operations."
290 *
291 * @param orbit orbit to which the covariance matrix should correspond
292 * @param frameOut output frame
293 *
294 * @return a new covariance state, expressed in the output frame
295 */
296 public FieldStateCovariance<T> changeCovarianceFrame(final FieldOrbit<T> orbit, final Frame frameOut) {
297
298 // Verify current covariance frame
299 if (lof != null) {
300
301 // Covariance is expressed in local orbital frame
302 return changeFrameAndCreate(orbit, epoch, lof, frameOut, orbitalCovariance);
303
304 } else {
305
306 // Check specific case where output covariance will be the same
307 if (frame == frameOut) {
308 return new FieldStateCovariance<>(orbitalCovariance, epoch, frame, orbitType, angleType);
309 }
310
311 // Change covariance frame
312 return changeFrameAndCreate(orbit, epoch, frame, frameOut, orbitalCovariance, orbitType, angleType);
313
314 }
315
316 }
317
318 /**
319 * Get a time-shifted covariance matrix.
320 * <p>
321 * The shifting model is a linearized, Keplerian one. In other words, it is based on a state transition matrix that
322 * is computed assuming Keplerian motion.
323 * <p>
324 * Shifting is <em>not</em> intended as a replacement for proper covariance propagation, but should be sufficient
325 * for small time shifts or coarse accuracy.
326 *
327 * @param field to which the elements belong
328 * @param orbit orbit to which the covariance matrix should correspond
329 * @param dt time shift in seconds
330 *
331 * @return a new covariance state, shifted with respect to the instance
332 */
333 public FieldStateCovariance<T> shiftedBy(final Field<T> field, final FieldOrbit<T> orbit, final T dt) {
334
335 // Shifted orbit
336 final FieldOrbit<T> shifted = orbit.shiftedBy(dt);
337
338 // State covariance expressed in celestial body frame
339 if (frame != null) {
340
341 // State covariance expressed in a pseudo-inertial frame
342 if (frame.isPseudoInertial()) {
343
344 // Compute STM
345 final FieldMatrix<T> stm = getKeplerianStm(orbit, dt);
346
347 // Shift covariance by applying the STM
348 final FieldMatrix<T> shiftedCov = stm.multiply(getMatrix().multiplyTransposed(stm));
349 return new FieldStateCovariance<>(shiftedCov, getDate().shiftedBy(dt), frame, orbitType,
350 getPositionAngleType());
351
352 }
353
354 // State covariance expressed in a non pseudo-inertial frame
355 else {
356
357 // Convert state covariance to orbit pseudo-inertial frame
358 final FieldStateCovariance<T> inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());
359
360 // Shift the state covariance
361 final FieldStateCovariance<T> shiftedCovariance = inOrbitFrame.shiftedBy(field, orbit, dt);
362
363 // Restore the initial covariance frame
364 return shiftedCovariance.changeCovarianceFrame(shifted, frame);
365 }
366 }
367
368 // State covariance expressed in a commonly used local orbital frame (LOF)
369 else {
370
371 // Convert state covariance to orbit pseudo-inertial frame
372 final FieldStateCovariance<T> inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());
373
374 // Shift the state covariance
375 final FieldStateCovariance<T> shiftedCovariance = inOrbitFrame.shiftedBy(field, orbit, dt);
376
377 // Restore the initial covariance frame
378 return shiftedCovariance.changeCovarianceFrame(shifted, lof);
379 }
380
381 }
382
383 /** Get new state covariance instance.
384 * @return new state covariance instance.
385 */
386 public StateCovariance toStateCovariance() {
387
388 // Extract data
389 final T[][] data = orbitalCovariance.getData();
390 final int rowDim = data.length;
391 final int columnDim = data.length;
392
393 // Convert to non-field version
394 final double[][] realData = new double[rowDim][columnDim];
395 for (int i = 0; i < rowDim; i++) {
396 for (int j = 0; j < columnDim; j++) {
397 realData[i][j] = data[i][j].getReal();
398 }
399 }
400 final BlockRealMatrix realMatrix = new BlockRealMatrix(realData);
401 final AbsoluteDate realDate = epoch.toAbsoluteDate();
402
403 // Return new state covariance according to current state covariance definition
404 if (frame == null) {
405 return new StateCovariance(realMatrix, realDate, lof);
406 }
407 else {
408 return new StateCovariance(realMatrix, realDate, frame, orbitType, angleType);
409 }
410 }
411
412 /**
413 * Create a covariance matrix in another orbit type.
414 * <p>
415 * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
416 * Hence, the input covariance matrix <b>will not exactly match</b> the new linearized case and the
417 * distribution will not follow a generalized Gaussian distribution anymore.
418 * <p>
419 * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
420 * dynamics operations."
421 *
422 * @param orbit orbit to which the covariance matrix should correspond
423 * @param date covariance epoch
424 * @param covFrame covariance frame
425 * @param inOrbitType initial orbit type of the state covariance matrix
426 * @param inAngleType initial position angle type of the state covariance matrix
427 * @param outOrbitType target orbit type of the state covariance matrix
428 * @param outAngleType target position angle type of the state covariance matrix
429 * @param inputCov input covariance
430 *
431 * @return the covariance expressed in the target orbit type with the target position angle
432 */
433 private FieldStateCovariance<T> changeTypeAndCreate(final FieldOrbit<T> orbit,
434 final FieldAbsoluteDate<T> date,
435 final Frame covFrame,
436 final OrbitType inOrbitType,
437 final PositionAngleType inAngleType,
438 final OrbitType outOrbitType,
439 final PositionAngleType outAngleType,
440 final FieldMatrix<T> inputCov) {
441
442 // Check if type change is really necessary, if not then return input covariance
443 if (StateCovariance.inputAndOutputOrbitTypesAreCartesian(inOrbitType, outOrbitType) ||
444 StateCovariance.inputAndOutputAreIdentical(inOrbitType, inAngleType, outOrbitType, outAngleType)) {
445 return new FieldStateCovariance<>(inputCov, date, covFrame, inOrbitType, inAngleType);
446 }
447
448 // Notations:
449 // I: Input orbit type
450 // O: Output orbit type
451 // C: Cartesian parameters
452
453 // Extract field
454 final Field<T> field = date.getField();
455
456 // Compute dOutputdCartesian
457 final T[][] aOC = MathArrays.buildArray(field, STATE_DIMENSION, STATE_DIMENSION);
458 final FieldOrbit<T> orbitInOutputType = outOrbitType.convertType(orbit);
459 orbitInOutputType.getJacobianWrtCartesian(outAngleType, aOC);
460 final FieldMatrix<T> dOdC = new Array2DRowFieldMatrix<>(aOC, false);
461
462 // Compute dCartesiandInput
463 final T[][] aCI = MathArrays.buildArray(field, STATE_DIMENSION, STATE_DIMENSION);
464 final FieldOrbit<T> orbitInInputType = inOrbitType.convertType(orbit);
465 orbitInInputType.getJacobianWrtParameters(inAngleType, aCI);
466 final FieldMatrix<T> dCdI = new Array2DRowFieldMatrix<>(aCI, false);
467
468 // Compute dOutputdInput
469 final FieldMatrix<T> dOdI = dOdC.multiply(dCdI);
470
471 // Conversion of the state covariance matrix in target orbit type
472 final FieldMatrix<T> outputCov = dOdI.multiply(inputCov.multiplyTransposed(dOdI));
473
474 // Return the converted covariance
475 return new FieldStateCovariance<>(outputCov, date, covFrame, outOrbitType, outAngleType);
476
477 }
478
479 /**
480 * Create a covariance matrix from a {@link LOF local orbital frame} to another
481 * {@link LOF local orbital frame}.
482 * <p>
483 * Changing the covariance frame is a linear process, this method does not introduce approximation.
484 * <p>
485 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
486 * Operations" by David A. Vallado.
487 *
488 * @param orbit orbit to which the covariance matrix should correspond
489 * @param date covariance epoch
490 * @param lofIn the local orbital frame in which the input covariance matrix is expressed
491 * @param lofOut the target local orbital frame
492 * @param inputCartesianCov input covariance {@code CARTESIAN})
493 *
494 * @return the covariance matrix expressed in the target commonly used local orbital frame in cartesian elements
495 */
496 private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
497 final FieldAbsoluteDate<T> date,
498 final LOF lofIn,
499 final LOF lofOut,
500 final FieldMatrix<T> inputCartesianCov) {
501
502 // Builds the matrix to perform covariance transformation
503 final FieldMatrix<T> jacobianFromLofInToLofOut =
504 getJacobian(LOF.transformFromLOFInToLOFOut(lofIn, lofOut, date, orbit.getPVCoordinates()));
505
506 // Get the Cartesian covariance matrix converted to frameOut
507 final FieldMatrix<T> cartesianCovarianceOut =
508 jacobianFromLofInToLofOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToLofOut));
509
510 // Output converted covariance
511 return new FieldStateCovariance<>(cartesianCovarianceOut, date, lofOut);
512
513 }
514
515 /**
516 * Convert the covariance matrix from a {@link Frame frame} to a {@link LOF local orbital frame}.
517 * <p>
518 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
519 * in covariance orbit type is required.
520 * <p>
521 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
522 * Operations" by David A. Vallado.
523 * <p>
524 * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
525 * and position angle types of the input covariance must be provided.
526 * <p>
527 * <b>The output covariance matrix will provided in Cartesian orbit type and not converted back to
528 * its original expression (if input different from Cartesian elements).</b>
529 *
530 * @param orbit orbit to which the covariance matrix should correspond
531 * @param date covariance epoch
532 * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is <b>not</b>
533 * pseudo-inertial, the input covariance matrix is expected to be expressed in <b>Cartesian elements</b>.
534 * @param lofOut the target local orbital frame
535 * @param inputCov input covariance
536 * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
537 * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used
538 * if covOrbitType equals {@code CARTESIAN})
539 * @return the covariance matrix expressed in the target local orbital frame in Cartesian elements
540 * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
541 * <b>not</b> expressed in Cartesian elements.
542 */
543 private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
544 final FieldAbsoluteDate<T> date,
545 final Frame frameIn,
546 final LOF lofOut,
547 final FieldMatrix<T> inputCov,
548 final OrbitType covOrbitType,
549 final PositionAngleType covAngleType) {
550
551 // Input frame is inertial
552 if (frameIn.isPseudoInertial()) {
553
554 // Convert input matrix to Cartesian parameters in input frame
555 final FieldMatrix<T> cartesianCovarianceIn =
556 changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
557 OrbitType.CARTESIAN, PositionAngleType.MEAN,
558 inputCov).getMatrix();
559
560 // Builds the matrix to perform covariance transformation
561 final FieldMatrix<T> jacobianFromFrameInToLofOut =
562 getJacobian(lofOut.transformFromInertial(date, orbit.getPVCoordinates(frameIn)));
563
564 // Get the Cartesian covariance matrix converted to frameOut
565 final FieldMatrix<T> cartesianCovarianceOut =
566 jacobianFromFrameInToLofOut.multiply(
567 cartesianCovarianceIn.multiplyTransposed(jacobianFromFrameInToLofOut));
568
569 // Return converted covariance matrix expressed in cartesian elements
570 return new FieldStateCovariance<>(cartesianCovarianceOut, date, lofOut);
571
572 }
573
574 // Input frame is not inertial so the covariance matrix is expected to be in cartesian elements
575 else {
576
577 // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type
578 final Frame orbitInertialFrame = orbit.getFrame();
579
580 // Compute rotation matrix from frameIn to orbit inertial frame
581 final FieldMatrix<T> cartesianCovarianceInOrbitFrame =
582 changeFrameAndCreate(orbit, date, frameIn, orbitInertialFrame, inputCov,
583 OrbitType.CARTESIAN, PositionAngleType.MEAN).getMatrix();
584
585 // Convert from orbit inertial frame to lofOut
586 return changeFrameAndCreate(orbit, date, orbitInertialFrame, lofOut, cartesianCovarianceInOrbitFrame,
587 OrbitType.CARTESIAN, PositionAngleType.MEAN);
588
589 }
590
591 }
592
593 /**
594 * Convert the covariance matrix from a {@link LOF local orbital frame} to a {@link Frame frame}.
595 * <p>
596 * Changing the covariance frame is a linear process, this method does not introduce approximation.
597 * <p>
598 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
599 * Operations" by David A. Vallado.
600 * <p>
601 * The <u>input</u> covariance matrix is necessarily provided in <b>Cartesian orbit type</b>.
602 * <p>
603 * The <u>output</u> covariance matrix will be expressed in <b>Cartesian elements</b>.
604 *
605 * @param orbit orbit to which the covariance matrix should correspond
606 * @param date covariance epoch
607 * @param lofIn the local orbital frame in which the input covariance matrix is expressed
608 * @param frameOut the target frame
609 * @param inputCartesianCov input covariance ({@code CARTESIAN})
610 *
611 * @return the covariance matrix expressed in the target frame in cartesian elements
612 */
613 private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
614 final FieldAbsoluteDate<T> date,
615 final LOF lofIn,
616 final Frame frameOut,
617 final FieldMatrix<T> inputCartesianCov) {
618
619 // Output frame is pseudo-inertial
620 if (frameOut.isPseudoInertial()) {
621
622 // Builds the matrix to perform covariance transformation
623 final FieldMatrix<T> jacobianFromLofInToFrameOut =
624 getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates(frameOut)).getInverse());
625
626 // Transform covariance
627 final FieldMatrix<T> transformedCovariance =
628 jacobianFromLofInToFrameOut.multiply(
629 inputCartesianCov.multiplyTransposed(jacobianFromLofInToFrameOut));
630
631 // Get the Cartesian covariance matrix converted to frameOut
632 return new FieldStateCovariance<>(transformedCovariance, date, frameOut, OrbitType.CARTESIAN,
633 DEFAULT_POSITION_ANGLE);
634
635 }
636
637 // Output frame is not pseudo-inertial
638 else {
639
640 // Builds the matrix to perform covariance transformation
641 final FieldMatrix<T> jacobianFromLofInToOrbitFrame =
642 getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates()).getInverse());
643
644 // Get the Cartesian covariance matrix converted to orbit inertial frame
645 final FieldMatrix<T> cartesianCovarianceInOrbitFrame = jacobianFromLofInToOrbitFrame.multiply(
646 inputCartesianCov.multiplyTransposed(jacobianFromLofInToOrbitFrame));
647
648 // Get the Cartesian covariance matrix converted to frameOut
649 return changeFrameAndCreate(orbit, date, orbit.getFrame(), frameOut, cartesianCovarianceInOrbitFrame,
650 OrbitType.CARTESIAN, PositionAngleType.MEAN);
651 }
652
653 }
654
655 /**
656 * Get the covariance matrix in another frame.
657 * <p>
658 * The transformation is based on Equation (18) of "Covariance Transformations for Satellite Flight Dynamics
659 * Operations" by David A. Vallado.
660 * <p>
661 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
662 * in covariance orbit type is required.
663 * <p>
664 * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
665 * and position angle types of the input covariance must be provided.
666 * <p>
667 * In case the <u>input</u> frame is <b>not</b> pseudo-inertial, the <u>input</u> covariance matrix is necessarily
668 * expressed in <b>Cartesian elements</b>.
669 * <p>
670 * In case the <u>output</u> frame is <b>not</b> pseudo-inertial, the <u>output</u> covariance matrix will be
671 * expressed in <b>Cartesian elements</b>.
672 *
673 * @param orbit orbit to which the covariance matrix should correspond
674 * @param date covariance epoch
675 * @param frameIn the frame in which the input covariance matrix is expressed
676 * @param frameOut the target frame
677 * @param inputCov input covariance
678 * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
679 * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (<b>not</b>
680 * used if covOrbitType equals {@code CARTESIAN})
681 * @return the covariance matrix expressed in the target frame
682 *
683 * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
684 * <b>not</b> expressed in cartesian elements.
685 */
686 private FieldStateCovariance<T> changeFrameAndCreate(final FieldOrbit<T> orbit,
687 final FieldAbsoluteDate<T> date,
688 final Frame frameIn,
689 final Frame frameOut,
690 final FieldMatrix<T> inputCov,
691 final OrbitType covOrbitType,
692 final PositionAngleType covAngleType) {
693
694 // Get the transform from the covariance frame to the output frame
695 final FieldKinematicTransform<T> inToOut = frameIn.getKinematicTransformTo(frameOut, orbit.getDate());
696
697 // Matrix to perform the covariance transformation
698 final FieldMatrix<T> j = getJacobian(inToOut);
699
700 // Input frame pseudo-inertial
701 if (frameIn.isPseudoInertial()) {
702
703 // Convert input matrix to Cartesian parameters in input frame
704 final FieldMatrix<T> cartesianCovarianceIn =
705 changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
706 OrbitType.CARTESIAN, PositionAngleType.MEAN,
707 inputCov).getMatrix();
708
709 // Get the Cartesian covariance matrix converted to frameOut
710 final FieldMatrix<T> cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j));
711
712 // Output frame is pseudo-inertial
713 if (frameOut.isPseudoInertial()) {
714
715 // Convert output Cartesian matrix to initial orbit type and position angle
716 return changeTypeAndCreate(orbit, date, frameOut, OrbitType.CARTESIAN, PositionAngleType.MEAN,
717 covOrbitType, covAngleType, cartesianCovarianceOut);
718
719 }
720
721 // Output frame is not pseudo-inertial
722 else {
723
724 // Output Cartesian matrix
725 return new FieldStateCovariance<>(cartesianCovarianceOut, date, frameOut, OrbitType.CARTESIAN,
726 DEFAULT_POSITION_ANGLE);
727
728 }
729 }
730
731 // Input frame is not pseudo-inertial
732 else {
733
734 // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type
735
736 // Convert covariance matrix to frameOut
737 final FieldMatrix<T> covInFrameOut = j.multiply(inputCov.multiplyTransposed(j));
738
739 // Output the Cartesian covariance matrix converted to frameOut
740 return new FieldStateCovariance<>(covInFrameOut, date, frameOut, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
741
742 }
743
744 }
745
746 /**
747 * Builds the matrix to perform covariance frame transformation.
748 *
749 * @param transform input transformation
750 *
751 * @return the matrix to perform the covariance frame transformation
752 */
753 private FieldMatrix<T> getJacobian(final FieldKinematicTransform<T> transform) {
754 return MatrixUtils.createFieldMatrix(transform.getPVJacobian());
755 }
756
757 /**
758 * Get the state transition matrix of Keplerian motion.
759 * The coordinates used are those of the covariance itself.
760 *
761 * @param initialOrbit orbit to which the initial covariance matrix should correspond
762 * @param dt time difference between the two orbits
763 * @return the state transition matrix used to shift the covariance matrix
764 * @since 13.1
765 */
766 @SuppressWarnings("unchecked")
767 FieldMatrix<T> getKeplerianStm(final FieldOrbit<T> initialOrbit, final T dt) {
768 final Field<T> field = dt.getField();
769
770 // compute derivatives of Keplerian motion
771 final FieldGradientField<T> gradientField = FieldGradientField.getField(initialOrbit.getDate().getField(), STATE_DIMENSION);
772 final T[] stateVector = MathArrays.buildArray(field, STATE_DIMENSION);
773 orbitType.mapOrbitToArray(initialOrbit, getPositionAngleType(), stateVector, null);
774 final FieldGradient<T>[] fieldGradientStateVector = MathArrays.buildArray(gradientField, STATE_DIMENSION);
775 for (int i = 0; i < STATE_DIMENSION; i++) {
776 fieldGradientStateVector[i] = FieldGradient.variable(STATE_DIMENSION, i, stateVector[i]);
777 }
778 final AbsoluteDate absoluteDate = getDate().toAbsoluteDate();
779 final FieldAbsoluteDate<FieldGradient<T>> fieldGradientDate = new FieldAbsoluteDate<>(gradientField, absoluteDate)
780 .shiftedBy(FieldGradient.constant(STATE_DIMENSION, getDate().durationFrom(absoluteDate)));
781 final FieldOrbit<FieldGradient<T>> fieldOrbit = orbitType.mapArrayToOrbit(fieldGradientStateVector, null, getPositionAngleType(),
782 fieldGradientDate, gradientField.getOne().newInstance(initialOrbit.getMu()), initialOrbit.getFrame());
783 final FieldOrbit<FieldGradient<T>> shiftedOrbit = fieldOrbit.shiftedBy(gradientField.getZero().newInstance(dt)); // automatic differentiation
784 final FieldGradient<T>[] gradient = MathArrays.buildArray(gradientField, STATE_DIMENSION);
785 orbitType.mapOrbitToArray(shiftedOrbit, getPositionAngleType(), gradient, null);
786
787 // Return state transition matrix
788 final FieldMatrix<T> stm = MatrixUtils.createFieldIdentityMatrix(field, STATE_DIMENSION);
789 for (int i = 0; i < gradient.length; i++) {
790 stm.setRow(i, gradient[i].getGradient());
791 }
792 return stm;
793
794 }
795
796 }