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