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