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.analysis.polynomials.SmoothStepFactory;
20  import org.hipparchus.linear.MatrixUtils;
21  import org.hipparchus.linear.RealMatrix;
22  import org.orekit.frames.Frame;
23  import org.orekit.frames.LOFType;
24  import org.orekit.orbits.Orbit;
25  import org.orekit.orbits.OrbitType;
26  import org.orekit.orbits.PositionAngleType;
27  import org.orekit.time.AbsoluteDate;
28  import org.orekit.time.TimeInterpolator;
29  import org.orekit.time.TimeStampedPair;
30  
31  import java.util.List;
32  
33  /**
34   * State covariance blender.
35   * <p>
36   * Its purpose is to interpolate state covariance between tabulated state covariances by using the concept of blending,
37   * exposed in : "Efficient Covariance Interpolation using Blending of Approximate State Error Transitions" by Sergei
38   * Tanygin.
39   * <p>
40   * It propagates tabulated values to the interpolation date assuming a standard Keplerian model and then blend each
41   * propagated covariances using a smoothstep function.
42   * <p>
43   * It gives accurate results as explained <a
44   * href="https://orekit.org/doc/technical-notes/Implementation_of_covariance_interpolation_in_Orekit.pdf">here</a>. In the
45   * very poorly tracked test case evolving in a highly dynamical environment mentioned in the linked thread, the user can
46   * expect at worst errors of less than 0.25% in position sigmas and less than 0.4% in velocity sigmas with steps of 40mn
47   * between tabulated values.
48   *
49   * @author Vincent Cucchietti
50   * @see org.hipparchus.analysis.polynomials.SmoothStepFactory
51   * @see org.hipparchus.analysis.polynomials.SmoothStepFactory.SmoothStepFunction
52   */
53  public class StateCovarianceBlender extends AbstractStateCovarianceInterpolator {
54  
55      /** Blending function. */
56      private final SmoothStepFactory.SmoothStepFunction blendingFunction;
57  
58      /**
59       * Constructor.
60       * <p>
61       * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
62       * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
63       * case.</b>
64       *
65       * @param blendingFunction blending function
66       * @param orbitInterpolator orbit interpolator
67       * @param outLOF local orbital frame
68       *
69       * @see Frame
70       * @see OrbitType
71       * @see PositionAngleType
72       */
73      public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
74                                    final TimeInterpolator<Orbit> orbitInterpolator,
75                                    final LOFType outLOF) {
76          super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outLOF);
77          this.blendingFunction = blendingFunction;
78      }
79  
80      /**
81       * Constructor.
82       *
83       * @param blendingFunction blending function
84       * @param orbitInterpolator orbit interpolator
85       * @param outFrame desired output covariance frame
86       * @param outPositionAngleType desired output position angle
87       * @param outOrbitType desired output orbit type
88       *
89       * @see Frame
90       * @see OrbitType
91       * @see PositionAngleType
92       */
93      public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
94                                    final TimeInterpolator<Orbit> orbitInterpolator,
95                                    final Frame outFrame,
96                                    final OrbitType outOrbitType,
97                                    final PositionAngleType outPositionAngleType) {
98          super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
99          this.blendingFunction = blendingFunction;
100     }
101 
102     /** {@inheritDoc} */
103     @Override
104     protected StateCovariance computeInterpolatedCovarianceInOrbitFrame(
105             final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
106             final Orbit interpolatedOrbit) {
107 
108         // Necessarily only two sample for blending
109         final TimeStampedPair<Orbit, StateCovariance> previousUncertainState = uncertainStates.get(0);
110         final TimeStampedPair<Orbit, StateCovariance> nextUncertainState     = uncertainStates.get(1);
111 
112         // Get the interpolation date
113         final AbsoluteDate interpolationDate = interpolatedOrbit.getDate();
114 
115         // Propagate previous tabulated covariance to interpolation date
116         final StateCovariance forwardedCovariance =
117                 propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, previousUncertainState);
118 
119         // Propagate next tabulated covariance to interpolation date
120         final StateCovariance backwardedCovariance =
121                 propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, nextUncertainState);
122 
123         // Compute normalized time parameter
124         final double timeParameter =
125                 getTimeParameter(interpolationDate, previousUncertainState.getDate(), nextUncertainState.getDate());
126 
127         // Blend the covariance matrices
128         final double     blendingValue              = blendingFunction.value(timeParameter);
129         final RealMatrix forwardedCovarianceMatrix  = forwardedCovariance.getMatrix();
130         final RealMatrix backwardedCovarianceMatrix = backwardedCovariance.getMatrix();
131 
132         final RealMatrix blendedCovarianceMatrix =
133                 forwardedCovarianceMatrix.blendArithmeticallyWith(backwardedCovarianceMatrix, blendingValue);
134 
135         return new StateCovariance(blendedCovarianceMatrix, interpolationDate, interpolatedOrbit.getFrame(),
136                                    OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
137     }
138 
139     /**
140      * Propagate given state covariance to the interpolation date using keplerian motion.
141      *
142      * @param interpolationTime interpolation date at which we desire to interpolate the state covariance
143      * @param orbitAtInterpolatingTime orbit at interpolation date
144      * @param tabulatedUncertainState tabulated uncertain state
145      *
146      * @return state covariance at given interpolation date.
147      *
148      * @see StateCovariance
149      */
150     private StateCovariance propagateCovarianceAnalytically(final AbsoluteDate interpolationTime,
151                                                             final Orbit orbitAtInterpolatingTime,
152                                                             final TimeStampedPair<Orbit, StateCovariance> tabulatedUncertainState) {
153 
154         // Get orbit and covariance
155         final Orbit           tabulatedOrbit         = tabulatedUncertainState.getFirst();
156         final StateCovariance tabulatedCovariance    = tabulatedUncertainState.getSecond();
157         final Frame           interpolatedOrbitFrame = orbitAtInterpolatingTime.getFrame();
158 
159         // Express tabulated covariance in interpolated orbit frame for consistency
160         final StateCovariance tabulatedCovarianceInOrbitFrame =
161                 tabulatedCovariance.changeCovarianceFrame(tabulatedOrbit, interpolatedOrbitFrame);
162 
163         // First convert the covariance matrix to equinoctial elements to avoid singularities inherent to keplerian elements
164         final PositionAngleType positionAngleType = PositionAngleType.MEAN;
165         final RealMatrix covarianceMatrixInEquinoctial =
166                 tabulatedCovarianceInOrbitFrame.changeCovarianceType(tabulatedOrbit, OrbitType.EQUINOCTIAL, positionAngleType).getMatrix();
167 
168         // Compute state error transition matrix in equinoctial elements (identical to the one in keplerian elements)
169         final RealMatrix stateErrorTransitionMatrixInEquinoctial = MatrixUtils.createRealIdentityMatrix(6);
170         final double contribution = tabulatedOrbit.getMeanAnomalyDotWrtA() * interpolationTime.durationFrom(tabulatedOrbit.getDate());
171         stateErrorTransitionMatrixInEquinoctial.setEntry(5, 0, contribution);
172 
173         // Propagate the covariance matrix using the previously computed state error transition matrix
174         final RealMatrix propagatedCovarianceMatrixInEquinoctial =
175                 stateErrorTransitionMatrixInEquinoctial.multiply(
176                         covarianceMatrixInEquinoctial.multiplyTransposed(stateErrorTransitionMatrixInEquinoctial));
177 
178         // Recreate a StateCovariance instance
179         final StateCovariance propagatedCovarianceInEquinoctial =
180                 new StateCovariance(propagatedCovarianceMatrixInEquinoctial, interpolationTime,
181                                     interpolatedOrbitFrame, OrbitType.EQUINOCTIAL, positionAngleType);
182 
183         // Output propagated state covariance after converting back to cartesian elements
184         return propagatedCovarianceInEquinoctial.changeCovarianceType(orbitAtInterpolatingTime,
185                                                                       OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
186     }
187 }