1 /* Copyright 2002-2024 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.LOF;
26 import org.orekit.frames.Transform;
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.CartesianDerivativesFilter;
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 /** {@inheritDoc}. */
147 @Override
148 public AbsoluteDate getDate() {
149 return epoch;
150 }
151
152 /**
153 * Get the covariance matrix.
154 * @return the covariance matrix
155 */
156 public RealMatrix getMatrix() {
157 return orbitalCovariance;
158 }
159
160 /**
161 * Get the covariance orbit type.
162 * @return the covariance orbit type
163 */
164 public OrbitType getOrbitType() {
165 return orbitType;
166 }
167
168 /**
169 * Get the covariance angle type.
170 * @return the covariance angle type
171 */
172 public PositionAngleType getPositionAngleType() {
173 return angleType;
174 }
175
176 /**
177 * Get the covariance frame.
178 * @return the covariance frame (can be null)
179 * @see #getLOF()
180 */
181 public Frame getFrame() {
182 return frame;
183 }
184
185 /**
186 * Get the covariance LOF type.
187 * @return the covariance LOF type (can be null)
188 * @see #getFrame()
189 */
190 public LOF getLOF() {
191 return lof;
192 }
193
194 /**
195 * Get the covariance matrix in another orbit type.
196 * <p>
197 * The covariance orbit type <b>cannot</b> be changed if the covariance
198 * matrix is expressed in a {@link LOF local orbital frame} or a
199 * non-pseudo inertial frame.
200 * <p>
201 * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
202 * Hence, the current covariance matrix <b>will not exactly match</b> the new linearized case and the
203 * distribution will not follow a generalized Gaussian distribution anymore.
204 * <p>
205 * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
206 * dynamics operations."
207 *
208 * @param orbit orbit to which the covariance matrix should correspond
209 * @param outOrbitType target orbit type of the state covariance matrix
210 * @param outAngleType target position angle type of the state covariance matrix
211 * @return a new covariance state, expressed in the target orbit type with the target position angle
212 * @see #changeCovarianceFrame(Orbit, Frame)
213 */
214 public StateCovariance changeCovarianceType(final Orbit 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 && (outAngleType == angleType || outOrbitType == OrbitType.CARTESIAN)) {
219 if (lof == null) {
220 return new StateCovariance(orbitalCovariance, epoch, frame, orbitType, angleType);
221 }
222 else {
223 return new StateCovariance(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 * @return a new covariance state, expressed in the output local orbital frame
258 */
259 public StateCovariance changeCovarianceFrame(final Orbit orbit, final LOF lofOut) {
260
261 // Verify current covariance frame
262 if (lof != null) {
263
264 // Change the covariance local orbital frame
265 return changeFrameAndCreate(orbit, epoch, lof, lofOut, orbitalCovariance);
266
267 } else {
268
269 // Covariance is expressed in celestial body frame
270 return changeFrameAndCreate(orbit, epoch, frame, lofOut, orbitalCovariance, orbitType, angleType);
271
272 }
273
274 }
275
276 /**
277 * Get the covariance in the output frame.
278 * <p>
279 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
280 * in covariance orbit type is required.
281 * <p>
282 * This is based on equation (18) to (20) "from Vallado, D. A. (2004). Covariance transformations for satellite
283 * flight dynamics operations."
284 *
285 * @param orbit orbit to which the covariance matrix should correspond
286 * @param frameOut output frame
287 * @return a new covariance state, expressed in the output frame
288 */
289 public StateCovariance changeCovarianceFrame(final Orbit orbit, final Frame frameOut) {
290
291 // Verify current covariance frame
292 if (lof != null) {
293
294 // Covariance is expressed in local orbital frame
295 return changeFrameAndCreate(orbit, epoch, lof, frameOut, orbitalCovariance);
296
297 } else {
298
299 // Change covariance frame
300 return changeFrameAndCreate(orbit, epoch, frame, frameOut, orbitalCovariance, orbitType, angleType);
301
302 }
303
304 }
305
306 /**
307 * Get a time-shifted covariance matrix.
308 * <p>
309 * The shifting model is a linearized, Keplerian one. In other words, it is based on a state transition matrix that
310 * is computed assuming Keplerian motion.
311 * <p>
312 * Shifting is <em>not</em> intended as a replacement for proper covariance propagation, but should be sufficient
313 * for small time shifts or coarse accuracy.
314 *
315 * @param orbit orbit to which the covariance matrix should correspond
316 * @param dt time shift in seconds
317 * @return a new covariance state, shifted with respect to the instance
318 */
319 public StateCovariance shiftedBy(final Orbit orbit, final double dt) {
320
321 // Shifted orbit
322 final Orbit shifted = orbit.shiftedBy(dt);
323
324 // State covariance expressed in celestial body frame
325 if (frame != null) {
326
327 // State covariance expressed in a pseudo-inertial frame
328 if (frame.isPseudoInertial()) {
329
330 // Compute STM
331 final RealMatrix stm = getStm(orbit, dt);
332
333 // Convert covariance in STM type (i.e., Equinoctial elements)
334 final StateCovariance inStmType = changeTypeAndCreate(orbit, epoch, frame, orbitType, angleType,
335 OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
336 orbitalCovariance);
337
338 // Shift covariance by applying the STM
339 final RealMatrix shiftedCov = stm.multiply(inStmType.getMatrix().multiplyTransposed(stm));
340
341 // Restore the initial covariance type
342 return changeTypeAndCreate(shifted, shifted.getDate(), frame,
343 OrbitType.EQUINOCTIAL, PositionAngleType.MEAN,
344 orbitType, angleType, shiftedCov);
345 }
346
347 // State covariance expressed in a non pseudo-inertial frame
348 else {
349
350 // Convert state covariance to orbit pseudo-inertial frame
351 final StateCovariance inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());
352
353 // Shift the state covariance
354 final StateCovariance shiftedCovariance = inOrbitFrame.shiftedBy(orbit, dt);
355
356 // Restore the initial covariance frame
357 return shiftedCovariance.changeCovarianceFrame(shifted, frame);
358 }
359 }
360
361 // State covariance expressed in a commonly used local orbital frame (LOF)
362 else {
363
364 // Convert state covariance to orbit pseudo-inertial frame
365 final StateCovariance inOrbitFrame = changeCovarianceFrame(orbit, orbit.getFrame());
366
367 // Shift the state covariance
368 final StateCovariance shiftedCovariance = inOrbitFrame.shiftedBy(orbit, dt);
369
370 // Restore the initial covariance frame
371 return shiftedCovariance.changeCovarianceFrame(shifted, lof);
372 }
373
374 }
375
376 /**
377 * Create a covariance matrix in another orbit type.
378 * <p>
379 * As this type change uses the jacobian matrix of the transformation, it introduces a linear approximation.
380 * Hence, the input covariance matrix <b>will not exactly match</b> the new linearized case and the
381 * distribution will not follow a generalized Gaussian distribution anymore.
382 * <p>
383 * This is based on equation (1) to (6) from "Vallado, D. A. (2004). Covariance transformations for satellite flight
384 * dynamics operations."
385 *
386 * @param orbit orbit to which the covariance matrix should correspond
387 * @param date covariance epoch
388 * @param covFrame covariance frame
389 * @param inOrbitType initial orbit type of the state covariance matrix
390 * @param inAngleType initial position angle type of the state covariance matrix
391 * @param outOrbitType target orbit type of the state covariance matrix
392 * @param outAngleType target position angle type of the state covariance matrix
393 * @param inputCov input covariance
394 * @return the covariance expressed in the target orbit type with the target position angle
395 */
396 private static StateCovariance changeTypeAndCreate(final Orbit orbit, final AbsoluteDate date,
397 final Frame covFrame,
398 final OrbitType inOrbitType, final PositionAngleType inAngleType,
399 final OrbitType outOrbitType, final PositionAngleType outAngleType,
400 final RealMatrix inputCov) {
401
402 // Notations:
403 // I: Input orbit type
404 // O: Output orbit type
405 // C: Cartesian parameters
406
407 // Compute dOutputdCartesian
408 final double[][] aOC = new double[STATE_DIMENSION][STATE_DIMENSION];
409 final Orbit orbitInOutputType = outOrbitType.convertType(orbit);
410 orbitInOutputType.getJacobianWrtCartesian(outAngleType, aOC);
411 final RealMatrix dOdC = new Array2DRowRealMatrix(aOC, false);
412
413 // Compute dCartesiandInput
414 final double[][] aCI = new double[STATE_DIMENSION][STATE_DIMENSION];
415 final Orbit orbitInInputType = inOrbitType.convertType(orbit);
416 orbitInInputType.getJacobianWrtParameters(inAngleType, aCI);
417 final RealMatrix dCdI = new Array2DRowRealMatrix(aCI, false);
418
419 // Compute dOutputdInput
420 final RealMatrix dOdI = dOdC.multiply(dCdI);
421
422 // Conversion of the state covariance matrix in target orbit type
423 final RealMatrix outputCov = dOdI.multiply(inputCov.multiplyTransposed(dOdI));
424
425 // Return the converted covariance
426 return new StateCovariance(outputCov, date, covFrame, outOrbitType, outAngleType);
427
428 }
429
430 /**
431 * Create a covariance matrix from a {@link LOF local orbital frame} to another
432 * {@link LOF local orbital frame}.
433 * <p>
434 * Changing the covariance frame is a linear process, this method does not introduce approximation.
435 * <p>
436 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
437 * Operations" by David A. Vallado.
438 *
439 * @param orbit orbit to which the covariance matrix should correspond
440 * @param date covariance epoch
441 * @param lofIn the local orbital frame in which the input covariance matrix is expressed
442 * @param lofOut the target local orbital frame
443 * @param inputCartesianCov input covariance {@code CARTESIAN})
444 * @return the covariance matrix expressed in the target commonly used local orbital frame in Cartesian elements
445 */
446 private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
447 final LOF lofIn, final LOF lofOut,
448 final RealMatrix inputCartesianCov) {
449
450 // Builds the matrix to perform covariance transformation
451 final RealMatrix jacobianFromLofInToLofOut =
452 getJacobian(LOF.transformFromLOFInToLOFOut(lofIn, lofOut, date, orbit.getPVCoordinates()));
453
454 // Get the Cartesian covariance matrix converted to frameOut
455 final RealMatrix cartesianCovarianceOut =
456 jacobianFromLofInToLofOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToLofOut));
457
458 // Output converted covariance
459 return new StateCovariance(cartesianCovarianceOut, date, lofOut);
460
461 }
462
463 /**
464 * Convert the covariance matrix from a {@link Frame frame} to a {@link LOF local orbital frame}.
465 * <p>
466 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
467 * in covariance orbit type is required.
468 * <p>
469 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
470 * Operations" by David A. Vallado.
471 * <p>
472 * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
473 * and position angle types of the input covariance must be provided.
474 * <p>
475 * <b>The output covariance matrix will provided in Cartesian orbit type and not converted back to
476 * its original expression (if input different from Cartesian elements).</b>
477 *
478 * @param orbit orbit to which the covariance matrix should correspond
479 * @param date covariance epoch
480 * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is <b>not</b>
481 * pseudo-inertial, the input covariance matrix is expected to be expressed in <b>Cartesian elements</b>.
482 * @param lofOut the target local orbital frame
483 * @param inputCov input covariance
484 * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
485 * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used
486 * if covOrbitType equals {@code CARTESIAN})
487 * @return the covariance matrix expressed in the target local orbital frame in Cartesian elements
488 * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
489 * <b>not</b> expressed in Cartesian elements.
490 */
491 private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
492 final Frame frameIn, final LOF lofOut,
493 final RealMatrix inputCov,
494 final OrbitType covOrbitType,
495 final PositionAngleType covAngleType) {
496
497 // Input frame is inertial
498 if (frameIn.isPseudoInertial()) {
499
500 // Convert input matrix to Cartesian parameters in input frame
501 final RealMatrix cartesianCovarianceIn =
502 changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
503 OrbitType.CARTESIAN, PositionAngleType.MEAN,
504 inputCov).getMatrix();
505
506 // Builds the matrix to perform covariance transformation
507 final RealMatrix jacobianFromFrameInToLofOut =
508 getJacobian(lofOut.transformFromInertial(date, orbit.getPVCoordinates(frameIn)));
509
510 // Get the Cartesian covariance matrix converted to frameOut
511 final RealMatrix cartesianCovarianceOut =
512 jacobianFromFrameInToLofOut.multiply(cartesianCovarianceIn.multiplyTransposed(jacobianFromFrameInToLofOut));
513
514 // Return converted covariance matrix expressed in Cartesian elements
515 return new StateCovariance(cartesianCovarianceOut, date, lofOut);
516
517 }
518
519 // Input frame is not inertial so the covariance matrix is expected to be in Cartesian elements
520 else {
521
522 // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type
523 final Frame orbitInertialFrame = orbit.getFrame();
524
525 // Compute rotation matrix from frameIn to orbit inertial frame
526 final RealMatrix cartesianCovarianceInOrbitFrame =
527 changeFrameAndCreate(orbit, date, frameIn, orbitInertialFrame, inputCov,
528 OrbitType.CARTESIAN, PositionAngleType.MEAN).getMatrix();
529
530 // Convert from orbit inertial frame to lofOut
531 return changeFrameAndCreate(orbit, date, orbitInertialFrame, lofOut, cartesianCovarianceInOrbitFrame,
532 OrbitType.CARTESIAN, PositionAngleType.MEAN);
533
534 }
535
536 }
537
538 /**
539 * Convert the covariance matrix from a {@link LOF local orbital frame} to a {@link Frame frame}.
540 * <p>
541 * Changing the covariance frame is a linear process, this method does not introduce approximation.
542 * <p>
543 * The transformation is based on Equation (18) to (20) of "Covariance Transformations for Satellite Flight Dynamics
544 * Operations" by David A. Vallado.
545 * <p>
546 * The <u>input</u> covariance matrix is necessarily provided in <b>Cartesian orbit type</b>.
547 * <p>
548 * The <u>output</u> covariance matrix will be expressed in <b>Cartesian elements</b>.
549 *
550 * @param orbit orbit to which the covariance matrix should correspond
551 * @param date covariance epoch
552 * @param lofIn the local orbital frame in which the input covariance matrix is expressed
553 * @param frameOut the target frame
554 * @param inputCartesianCov input covariance ({@code CARTESIAN})
555 * @return the covariance matrix expressed in the target frame in Cartesian elements
556 */
557 private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
558 final LOF lofIn, final Frame frameOut,
559 final RealMatrix inputCartesianCov) {
560
561 // Output frame is pseudo-inertial
562 if (frameOut.isPseudoInertial()) {
563
564 // Builds the matrix to perform covariance transformation
565 final RealMatrix jacobianFromLofInToFrameOut =
566 getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates(frameOut)).getInverse());
567
568 // Transform covariance
569 final RealMatrix transformedCovariance =
570 jacobianFromLofInToFrameOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToFrameOut));
571
572 // Get the Cartesian covariance matrix converted to frameOut
573 return new StateCovariance(transformedCovariance, date, frameOut, OrbitType.CARTESIAN,
574 DEFAULT_POSITION_ANGLE);
575
576 }
577
578 // Output frame is not pseudo-inertial
579 else {
580
581 // Builds the matrix to perform covariance transformation
582 final RealMatrix jacobianFromLofInToOrbitFrame =
583 getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates()).getInverse());
584
585 // Get the Cartesian covariance matrix converted to orbit inertial frame
586 final RealMatrix cartesianCovarianceInOrbitFrame = jacobianFromLofInToOrbitFrame.multiply(
587 inputCartesianCov.multiplyTransposed(jacobianFromLofInToOrbitFrame));
588
589 // Get the Cartesian covariance matrix converted to frameOut
590 return changeFrameAndCreate(orbit, date, orbit.getFrame(), frameOut, cartesianCovarianceInOrbitFrame,
591 OrbitType.CARTESIAN, PositionAngleType.MEAN);
592 }
593
594 }
595
596 /**
597 * Get the covariance matrix in another frame.
598 * <p>
599 * The transformation is based on Equation (18) of "Covariance Transformations for Satellite Flight Dynamics
600 * Operations" by David A. Vallado.
601 * <p>
602 * Changing the covariance frame is a linear process, this method does not introduce approximation unless a change
603 * in covariance orbit type is required.
604 * <p>
605 * As the frame transformation must be performed with the covariance expressed in Cartesian elements, both the orbit
606 * and position angle types of the input covariance must be provided.
607 * <p>
608 * In case the <u>input</u> frame is <b>not</b> pseudo-inertial, the <u>input</u> covariance matrix is necessarily
609 * expressed in <b>Cartesian elements</b>.
610 * <p>
611 * In case the <u>output</u> frame is <b>not</b> pseudo-inertial, the <u>output</u> covariance matrix will be
612 * expressed in <b>Cartesian elements</b>.
613 *
614 * @param orbit orbit to which the covariance matrix should correspond
615 * @param date covariance epoch
616 * @param frameIn the frame in which the input covariance matrix is expressed
617 * @param frameOut the target frame
618 * @param inputCov input covariance
619 * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial)
620 * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (<b>not</b>
621 * used if covOrbitType equals {@code CARTESIAN})
622 * @return the covariance matrix expressed in the target frame
623 * @throws OrekitException if input frame is <b>not</b> pseudo-inertial <b>and</b> the input covariance is
624 * <b>not</b> expressed in Cartesian elements.
625 */
626 private static StateCovariance changeFrameAndCreate(final Orbit orbit, final AbsoluteDate date,
627 final Frame frameIn, final Frame frameOut,
628 final RealMatrix inputCov,
629 final OrbitType covOrbitType,
630 final PositionAngleType covAngleType) {
631
632 // Get the transform from the covariance frame to the output frame
633 final Transform inToOut = frameIn.getTransformTo(frameOut, orbit.getDate());
634
635 // Matrix to perform the covariance transformation
636 final RealMatrix j = getJacobian(inToOut);
637
638 // Input frame pseudo-inertial
639 if (frameIn.isPseudoInertial()) {
640
641 // Convert input matrix to Cartesian parameters in input frame
642 final RealMatrix cartesianCovarianceIn =
643 changeTypeAndCreate(orbit, date, frameIn, covOrbitType, covAngleType,
644 OrbitType.CARTESIAN, PositionAngleType.MEAN,
645 inputCov).getMatrix();
646
647 // Get the Cartesian covariance matrix converted to frameOut
648 final RealMatrix cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j));
649
650 // Output frame is pseudo-inertial
651 if (frameOut.isPseudoInertial()) {
652
653 // Convert output Cartesian matrix to initial orbit type and position angle
654 return changeTypeAndCreate(orbit, date, frameOut, OrbitType.CARTESIAN, PositionAngleType.MEAN,
655 covOrbitType, covAngleType, cartesianCovarianceOut);
656
657 }
658
659 // Output frame is not pseudo-inertial
660 else {
661
662 // Output Cartesian matrix
663 return new StateCovariance(cartesianCovarianceOut, date, frameOut, OrbitType.CARTESIAN,
664 DEFAULT_POSITION_ANGLE);
665
666 }
667 }
668
669 // Input frame is not pseudo-inertial
670 else {
671
672 // Method checkInputConsistency already verify that input covariance is defined in CARTESIAN type
673
674 // Convert covariance matrix to frameOut
675 final RealMatrix covInFrameOut = j.multiply(inputCov.multiplyTransposed(j));
676
677 // Output the Cartesian covariance matrix converted to frameOut
678 return new StateCovariance(covInFrameOut, date, frameOut, OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
679
680 }
681
682 }
683
684 /**
685 * Builds the matrix to perform covariance frame transformation.
686 * @param transform input transformation
687 * @return the matrix to perform the covariance frame transformation
688 */
689 private static RealMatrix getJacobian(final Transform transform) {
690 // Get the Jacobian of the transform
691 final double[][] jacobian = new double[STATE_DIMENSION][STATE_DIMENSION];
692 transform.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
693 // Return
694 return new Array2DRowRealMatrix(jacobian, false);
695
696 }
697
698 /**
699 * Get the state transition matrix considering Keplerian contribution only.
700 *
701 * @param initialOrbit orbit to which the initial covariance matrix should correspond
702 * @param dt time difference between the two orbits
703 * @return the state transition matrix used to shift the covariance matrix
704 */
705 public static RealMatrix getStm(final Orbit initialOrbit, final double dt) {
706
707 // initialize the STM
708 final RealMatrix stm = MatrixUtils.createRealIdentityMatrix(STATE_DIMENSION);
709
710 // State transition matrix using Keplerian contribution only
711 final double contribution = initialOrbit.getMeanAnomalyDotWrtA() * dt;
712 stm.setEntry(5, 0, contribution);
713
714 // Return
715 return stm;
716
717 }
718
719 }