StateCovarianceBlender.java
- /* Copyright 2002-2024 CS GROUP
- * Licensed to CS GROUP (CS) under one or more
- * contributor license agreements. See the NOTICE file distributed with
- * this work for additional information regarding copyright ownership.
- * CS licenses this file to You under the Apache License, Version 2.0
- * (the "License"); you may not use this file except in compliance with
- * the License. You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
- package org.orekit.propagation;
- import org.hipparchus.analysis.polynomials.SmoothStepFactory;
- import org.hipparchus.linear.RealMatrix;
- import org.orekit.frames.Frame;
- import org.orekit.frames.LOFType;
- import org.orekit.orbits.Orbit;
- import org.orekit.orbits.OrbitType;
- import org.orekit.orbits.PositionAngleType;
- import org.orekit.time.AbsoluteDate;
- import org.orekit.time.TimeInterpolator;
- import org.orekit.time.TimeStampedPair;
- import java.util.List;
- /**
- * State covariance blender.
- * <p>
- * Its purpose is to interpolate state covariance between tabulated state covariances by using the concept of blending,
- * exposed in : "Efficient Covariance Interpolation using Blending of Approximate State Error Transitions" by Sergei
- * Tanygin.
- * <p>
- * It propagates tabulated values to the interpolation date assuming a standard keplerian model and then blend each
- * propagated covariances using a smoothstep function.
- * <p>
- * It gives accurate results as explained <a
- * href="https://orekit.org/doc/technical-notes/Implementation_of_covariance_interpolation_in_Orekit.pdf">here</a>. In the
- * very poorly tracked test case evolving in a highly dynamical environment mentioned in the linked thread, the user can
- * expect at worst errors of less than 0.25% in position sigmas and less than 0.4% in velocity sigmas with steps of 40mn
- * between tabulated values.
- *
- * @author Vincent Cucchietti
- * @see org.hipparchus.analysis.polynomials.SmoothStepFactory
- * @see org.hipparchus.analysis.polynomials.SmoothStepFactory.SmoothStepFunction
- */
- public class StateCovarianceBlender extends AbstractStateCovarianceInterpolator {
- /** Blending function. */
- private final SmoothStepFactory.SmoothStepFunction blendingFunction;
- /**
- * Constructor.
- * <p>
- * <b>BEWARE:</b> If the output local orbital frame is not considered pseudo-inertial, all the covariance components
- * related to the velocity will be poorly interpolated. <b>Only the position covariance should be considered in this
- * case.</b>
- *
- * @param blendingFunction blending function
- * @param orbitInterpolator orbit interpolator
- * @param outLOF local orbital frame
- *
- * @see Frame
- * @see OrbitType
- * @see PositionAngleType
- */
- public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
- final TimeInterpolator<Orbit> orbitInterpolator,
- final LOFType outLOF) {
- super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outLOF);
- this.blendingFunction = blendingFunction;
- }
- /**
- * Constructor.
- *
- * @param blendingFunction blending function
- * @param orbitInterpolator orbit interpolator
- * @param outFrame desired output covariance frame
- * @param outPositionAngleType desired output position angle
- * @param outOrbitType desired output orbit type
- *
- * @see Frame
- * @see OrbitType
- * @see PositionAngleType
- */
- public StateCovarianceBlender(final SmoothStepFactory.SmoothStepFunction blendingFunction,
- final TimeInterpolator<Orbit> orbitInterpolator,
- final Frame outFrame,
- final OrbitType outOrbitType,
- final PositionAngleType outPositionAngleType) {
- super(DEFAULT_INTERPOLATION_POINTS, 0., orbitInterpolator, outFrame, outOrbitType, outPositionAngleType);
- this.blendingFunction = blendingFunction;
- }
- /** {@inheritDoc} */
- @Override
- protected StateCovariance computeInterpolatedCovarianceInOrbitFrame(
- final List<TimeStampedPair<Orbit, StateCovariance>> uncertainStates,
- final Orbit interpolatedOrbit) {
- // Necessarily only two sample for blending
- final TimeStampedPair<Orbit, StateCovariance> previousUncertainState = uncertainStates.get(0);
- final TimeStampedPair<Orbit, StateCovariance> nextUncertainState = uncertainStates.get(1);
- // Get the interpolation date
- final AbsoluteDate interpolationDate = interpolatedOrbit.getDate();
- // Propagate previous tabulated covariance to interpolation date
- final StateCovariance forwardedCovariance =
- propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, previousUncertainState);
- // Propagate next tabulated covariance to interpolation date
- final StateCovariance backwardedCovariance =
- propagateCovarianceAnalytically(interpolationDate, interpolatedOrbit, nextUncertainState);
- // Compute normalized time parameter
- final double timeParameter =
- getTimeParameter(interpolationDate, previousUncertainState.getDate(), nextUncertainState.getDate());
- // Blend the covariance matrices
- final double blendingValue = blendingFunction.value(timeParameter);
- final RealMatrix forwardedCovarianceMatrix = forwardedCovariance.getMatrix();
- final RealMatrix backwardedCovarianceMatrix = backwardedCovariance.getMatrix();
- final RealMatrix blendedCovarianceMatrix =
- forwardedCovarianceMatrix.blendArithmeticallyWith(backwardedCovarianceMatrix, blendingValue);
- return new StateCovariance(blendedCovarianceMatrix, interpolationDate, interpolatedOrbit.getFrame(),
- OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
- }
- /**
- * Propagate given state covariance to the interpolation date using keplerian motion.
- *
- * @param interpolationTime interpolation date at which we desire to interpolate the state covariance
- * @param orbitAtInterpolatingTime orbit at interpolation date
- * @param tabulatedUncertainState tabulated uncertain state
- *
- * @return state covariance at given interpolation date.
- *
- * @see StateCovariance
- */
- private StateCovariance propagateCovarianceAnalytically(final AbsoluteDate interpolationTime,
- final Orbit orbitAtInterpolatingTime,
- final TimeStampedPair<Orbit, StateCovariance> tabulatedUncertainState) {
- // Get orbit and covariance
- final Orbit tabulatedOrbit = tabulatedUncertainState.getFirst();
- final StateCovariance tabulatedCovariance = tabulatedUncertainState.getSecond();
- final Frame interpolatedOrbitFrame = orbitAtInterpolatingTime.getFrame();
- // Express tabulated covariance in interpolated orbit frame for consistency
- final StateCovariance tabulatedCovarianceInOrbitFrame =
- tabulatedCovariance.changeCovarianceFrame(tabulatedOrbit, interpolatedOrbitFrame);
- // First convert the covariance matrix to equinoctial elements to avoid singularities inherent to keplerian elements
- final RealMatrix covarianceMatrixInEquinoctial =
- tabulatedCovarianceInOrbitFrame.changeCovarianceType(tabulatedOrbit, OrbitType.EQUINOCTIAL,
- DEFAULT_POSITION_ANGLE).getMatrix();
- // Compute state error transition matrix in equinoctial elements (identical to the one in keplerian elements)
- final RealMatrix stateErrorTransitionMatrixInEquinoctial =
- StateCovariance.getStm(tabulatedOrbit, interpolationTime.durationFrom(tabulatedOrbit.getDate()));
- // Propagate the covariance matrix using the previously computed state error transition matrix
- final RealMatrix propagatedCovarianceMatrixInEquinoctial =
- stateErrorTransitionMatrixInEquinoctial.multiply(
- covarianceMatrixInEquinoctial.multiplyTransposed(stateErrorTransitionMatrixInEquinoctial));
- // Recreate a StateCovariance instance
- final StateCovariance propagatedCovarianceInEquinoctial =
- new StateCovariance(propagatedCovarianceMatrixInEquinoctial, interpolationTime,
- interpolatedOrbitFrame, OrbitType.EQUINOCTIAL, DEFAULT_POSITION_ANGLE);
- // Output propagated state covariance after converting back to cartesian elements
- return propagatedCovarianceInEquinoctial.changeCovarianceType(orbitAtInterpolatingTime,
- OrbitType.CARTESIAN, DEFAULT_POSITION_ANGLE);
- }
- }