StateCovarianceBlender.java

  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. import org.hipparchus.analysis.polynomials.SmoothStepFactory;
  19. import org.hipparchus.linear.RealMatrix;
  20. import org.orekit.frames.Frame;
  21. import org.orekit.frames.LOFType;
  22. import org.orekit.orbits.Orbit;
  23. import org.orekit.orbits.OrbitType;
  24. import org.orekit.orbits.PositionAngleType;
  25. import org.orekit.time.AbsoluteDate;
  26. import org.orekit.time.TimeInterpolator;
  27. import org.orekit.time.TimeStampedPair;

  28. import java.util.List;

  29. /**
  30.  * State covariance blender.
  31.  * <p>
  32.  * Its purpose is to interpolate state covariance between tabulated state covariances by using the concept of blending,
  33.  * exposed in : "Efficient Covariance Interpolation using Blending of Approximate State Error Transitions" by Sergei
  34.  * Tanygin.
  35.  * <p>
  36.  * It propagates tabulated values to the interpolation date assuming a standard keplerian model and then blend each
  37.  * propagated covariances using a smoothstep function.
  38.  * <p>
  39.  * It gives accurate results as explained <a
  40.  * href="https://orekit.org/doc/technical-notes/Implementation_of_covariance_interpolation_in_Orekit.pdf">here</a>. In the
  41.  * very poorly tracked test case evolving in a highly dynamical environment mentioned in the linked thread, the user can
  42.  * expect at worst errors of less than 0.25% in position sigmas and less than 0.4% in velocity sigmas with steps of 40mn
  43.  * between tabulated values.
  44.  *
  45.  * @author Vincent Cucchietti
  46.  * @see org.hipparchus.analysis.polynomials.SmoothStepFactory
  47.  * @see org.hipparchus.analysis.polynomials.SmoothStepFactory.SmoothStepFunction
  48.  */
  49. public class StateCovarianceBlender extends AbstractStateCovarianceInterpolator {

  50.     /** Blending function. */
  51.     private final SmoothStepFactory.SmoothStepFunction blendingFunction;

  52.     /**
  53.      * Constructor.
  54.      * <p>
  55.      * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
  56.      * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
  57.      * case.</b>
  58.      *
  59.      * @param blendingFunction blending function
  60.      * @param orbitInterpolator orbit interpolator
  61.      * @param outLOF local orbital frame
  62.      *
  63.      * @see Frame
  64.      * @see OrbitType
  65.      * @see PositionAngleType
  66.      */
  67.     public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
  68.                                   final TimeInterpolator<Orbit> orbitInterpolator,
  69.                                   final LOFType outLOF) {
  70.         super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outLOF);
  71.         this.blendingFunction = blendingFunction;
  72.     }

  73.     /**
  74.      * Constructor.
  75.      *
  76.      * @param blendingFunction blending function
  77.      * @param orbitInterpolator orbit interpolator
  78.      * @param outFrame desired output covariance frame
  79.      * @param outPositionAngleType desired output position angle
  80.      * @param outOrbitType desired output orbit type
  81.      *
  82.      * @see Frame
  83.      * @see OrbitType
  84.      * @see PositionAngleType
  85.      */
  86.     public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
  87.                                   final TimeInterpolator<Orbit> orbitInterpolator,
  88.                                   final Frame outFrame,
  89.                                   final OrbitType outOrbitType,
  90.                                   final PositionAngleType outPositionAngleType) {
  91.         super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
  92.         this.blendingFunction = blendingFunction;
  93.     }

  94.     /** {@inheritDoc} */
  95.     @Override
  96.     protected StateCovariance computeInterpolatedCovarianceInOrbitFrame(
  97.             final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
  98.             final Orbit interpolatedOrbit) {

  99.         // Necessarily only two sample for blending
  100.         final TimeStampedPair<Orbit, StateCovariance> previousUncertainState = uncertainStates.get(0);
  101.         final TimeStampedPair<Orbit, StateCovariance> nextUncertainState     = uncertainStates.get(1);

  102.         // Get the interpolation date
  103.         final AbsoluteDate interpolationDate = interpolatedOrbit.getDate();

  104.         // Propagate previous tabulated covariance to interpolation date
  105.         final StateCovariance forwardedCovariance =
  106.                 propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, previousUncertainState);

  107.         // Propagate next tabulated covariance to interpolation date
  108.         final StateCovariance backwardedCovariance =
  109.                 propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, nextUncertainState);

  110.         // Compute normalized time parameter
  111.         final double timeParameter =
  112.                 getTimeParameter(interpolationDate, previousUncertainState.getDate(), nextUncertainState.getDate());

  113.         // Blend the covariance matrices
  114.         final double     blendingValue              = blendingFunction.value(timeParameter);
  115.         final RealMatrix forwardedCovarianceMatrix  = forwardedCovariance.getMatrix();
  116.         final RealMatrix backwardedCovarianceMatrix = backwardedCovariance.getMatrix();

  117.         final RealMatrix blendedCovarianceMatrix =
  118.                 forwardedCovarianceMatrix.blendArithmeticallyWith(backwardedCovarianceMatrix, blendingValue);

  119.         return new StateCovariance(blendedCovarianceMatrix, interpolationDate, interpolatedOrbit.getFrame(),
  120.                                    OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
  121.     }

  122.     /**
  123.      * Propagate given state covariance to the interpolation date using keplerian motion.
  124.      *
  125.      * @param interpolationTime interpolation date at which we desire to interpolate the state covariance
  126.      * @param orbitAtInterpolatingTime orbit at interpolation date
  127.      * @param tabulatedUncertainState tabulated uncertain state
  128.      *
  129.      * @return state covariance at given interpolation date.
  130.      *
  131.      * @see StateCovariance
  132.      */
  133.     private StateCovariance propagateCovarianceAnalytically(final AbsoluteDate interpolationTime,
  134.                                                             final Orbit orbitAtInterpolatingTime,
  135.                                                             final TimeStampedPair<Orbit, StateCovariance> tabulatedUncertainState) {

  136.         // Get orbit and covariance
  137.         final Orbit           tabulatedOrbit         = tabulatedUncertainState.getFirst();
  138.         final StateCovariance tabulatedCovariance    = tabulatedUncertainState.getSecond();
  139.         final Frame           interpolatedOrbitFrame = orbitAtInterpolatingTime.getFrame();

  140.         // Express tabulated covariance in interpolated orbit frame for consistency
  141.         final StateCovariance tabulatedCovarianceInOrbitFrame =
  142.                 tabulatedCovariance.changeCovarianceFrame(tabulatedOrbit, interpolatedOrbitFrame);

  143.         // First convert the covariance matrix to equinoctial elements to avoid singularities inherent to keplerian elements
  144.         final RealMatrix covarianceMatrixInEquinoctial =
  145.                 tabulatedCovarianceInOrbitFrame.changeCovarianceType(tabulatedOrbit, OrbitType.EQUINOCTIAL,
  146.                                                                      DEFAULT_POSITION_ANGLE).getMatrix();

  147.         // Compute state error transition matrix in equinoctial elements (identical to the one in keplerian elements)
  148.         final RealMatrix stateErrorTransitionMatrixInEquinoctial =
  149.                 StateCovariance.getStm(tabulatedOrbit, interpolationTime.durationFrom(tabulatedOrbit.getDate()));

  150.         // Propagate the covariance matrix using the previously computed state error transition matrix
  151.         final RealMatrix propagatedCovarianceMatrixInEquinoctial =
  152.                 stateErrorTransitionMatrixInEquinoctial.multiply(
  153.                         covarianceMatrixInEquinoctial.multiplyTransposed(stateErrorTransitionMatrixInEquinoctial));

  154.         // Recreate a StateCovariance instance
  155.         final StateCovariance propagatedCovarianceInEquinoctial =
  156.                 new StateCovariance(propagatedCovarianceMatrixInEquinoctial, interpolationTime,
  157.                                     interpolatedOrbitFrame, OrbitType.EQUINOCTIAL, DEFAULT_POSITION_ANGLE);

  158.         // Output propagated state covariance after converting back to cartesian elements
  159.         return propagatedCovarianceInEquinoctial.changeCovarianceType(orbitAtInterpolatingTime,
  160.                                                                       OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
  161.     }
  162. }