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