1   /* Copyright 2022-2025 Romain Serra
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.utils;
18  
19  import org.hipparchus.geometry.euclidean.threed.Vector3D;
20  import org.hipparchus.linear.MatrixUtils;
21  import org.hipparchus.linear.RealMatrix;
22  import org.orekit.frames.Frame;
23  import org.orekit.frames.KinematicTransform;
24  import org.orekit.frames.LOFType;
25  import org.orekit.frames.Transform;
26  import org.orekit.time.AbsoluteDate;
27  
28  /**
29   * Utility class for conversions related to Cartesian covariance matrices.
30   *
31   * @author Romain Serra
32   * @since 12.2
33   */
34  public class CartesianCovarianceUtils {
35  
36      /**
37       * Private constructor.
38       */
39      private CartesianCovarianceUtils() {
40          // utility class
41      }
42  
43      /**
44       * Convert input position-velocity covariance matrix between reference frames.
45       * @param inputFrame input frame
46       * @param outputFrame output frame
47       * @param covarianceMatrix position-velocity covariance matrix in reference frame
48       * @param date epoch
49       * @return converted covariance matrix
50       */
51      public static RealMatrix changeReferenceFrame(final Frame inputFrame, final RealMatrix covarianceMatrix,
52                                                    final AbsoluteDate date, final Frame outputFrame) {
53          final KinematicTransform kinematicTransform = inputFrame.getKinematicTransformTo(outputFrame, date);
54          return changePositionVelocityFrame(covarianceMatrix, kinematicTransform);
55      }
56  
57      /**
58       * Convert input position-velocity covariance matrix from reference frame to local one.
59       * @param position position vector in reference frame
60       * @param velocity velocity vector in reference frame
61       * @param covarianceMatrix position-velocity covariance matrix in reference frame
62       * @param lofType output local orbital frame
63       * @return converted covariance matrix
64       */
65      public static RealMatrix convertToLofType(final Vector3D position, final Vector3D velocity,
66                                                final RealMatrix covarianceMatrix, final LOFType lofType) {
67          final Transform transformFromInertial = transformToLofType(lofType, position, velocity);
68          return changePositionVelocityFrame(covarianceMatrix, transformFromInertial);
69      }
70  
71      /**
72       * Convert input position-velocity covariance matrix from local frame to reference one.
73       * @param position position vector in reference frame
74       * @param velocity velocity vector in reference frame
75       * @param covarianceMatrix position-velocity covariance matrix in local frame
76       * @param lofType input local orbital frame
77       * @return converted covariance matrix
78       */
79      public static RealMatrix convertFromLofType(final LOFType lofType, final RealMatrix covarianceMatrix,
80                                                  final Vector3D position, final Vector3D velocity) {
81          final Transform transformFromInertial = transformToLofType(lofType, position, velocity);
82          return changePositionVelocityFrame(covarianceMatrix, transformFromInertial.getInverse());
83      }
84  
85      /**
86       * Get the transform from local orbital frame to reference frame.
87       * @param lofType input local frame type
88       * @param position position in reference frame
89       * @param velocity velocity in reference frame
90       * @return transform
91       */
92      private static Transform transformToLofType(final LOFType lofType, final Vector3D position,
93                                                  final Vector3D velocity) {
94          return lofType.transformFromInertial(null, new PVCoordinates(position, velocity));
95      }
96  
97      /**
98       * Convert the input position-velocity covariance matrix according to input transformation.
99       * @param covarianceMatrix original covariance matrix
100      * @param kinematicTransform kinematic frame transform
101      * @return transformed covariance matrix
102      */
103     private static RealMatrix changePositionVelocityFrame(final RealMatrix covarianceMatrix,
104                                                           final KinematicTransform kinematicTransform) {
105         final RealMatrix jacobianTransformPV = MatrixUtils.createRealMatrix(kinematicTransform.getPVJacobian());
106         return jacobianTransformPV.multiply(covarianceMatrix.multiplyTransposed(jacobianTransformPV));
107     }
108 }