LinearKeplerianCovarianceMapper.java

/* Copyright 2022-2025 Romain Serra
 * 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.differentiation.Gradient;
import org.hipparchus.analysis.differentiation.GradientField;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.util.MathArrays;
import org.orekit.frames.LOF;
import org.orekit.frames.LOFType;
import org.orekit.orbits.FieldOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngleBased;
import org.orekit.orbits.PositionAngleType;
import org.orekit.utils.DerivativeStateUtils;

/**
 * Package private class to map orbital covariance using linearized Keplerian motion
 * (with non-Keplerian correction from derivatives if available).
 * The linearization uses the same coordinates as the ones for which the covariance coefficients are defined.
 *
 * @author Romain Serra
 * @since 13.1
 */
class LinearKeplerianCovarianceMapper {

    /** Default local orbital frame to use. */
    private static final LOF DEFAULT_LOF = LOFType.QSW;

    /** Reference orbit. */
    private final Orbit orbit;

    /** Reference orbital covariance. */
    private final StateCovariance stateCovariance;

    /** Converted covariance (from reference) using the same characteristics than the reference orbit. */
    private final StateCovariance convertedCovariance;

    /**
     * Constructor.
     * @param orbit initial orbit
     * @param stateCovariance initial orbital covariance
     */
    LinearKeplerianCovarianceMapper(final Orbit orbit, final StateCovariance stateCovariance) {
        this.orbit = orbit;
        this.stateCovariance = stateCovariance;
        this.convertedCovariance = changeCovarianceBefore();
    }

    /**
     * Maps the orbital covariance to the target orbit based on Keplerian linearization.
     * The coordinates and frame are kept the same as the reference covariance.
     * @param targetOrbit target orbit
     * @return mapped covariance
     */
    StateCovariance map(final Orbit targetOrbit) {
        final PositionAngleType positionAngleType = findPositionAngleType();
        final StateCovariance shiftedCovariance = shiftCovariance(orbit, convertedCovariance, targetOrbit,
                positionAngleType);
        return changeCovarianceAfter(shiftedCovariance, targetOrbit, positionAngleType);
    }

    /**
     * Convert the initial orbital covariance in the same coordinates and frame than the initial orbit.
     * @return covariance
     */
    private StateCovariance changeCovarianceBefore() {
        final PositionAngleType positionAngleType = findPositionAngleType();
        final StateCovariance intermediateCovariance = stateCovariance.changeCovarianceFrame(orbit, orbit.getFrame());
        return intermediateCovariance.changeCovarianceType(orbit, orbit.getType(), positionAngleType);
    }

    /**
     * Extract the position angle type from the initial orbit.
     * @return position angle type
     */
    private PositionAngleType findPositionAngleType() {
        if (orbit instanceof PositionAngleBased<?>) {
            final PositionAngleBased<?> positionAngleBased = (PositionAngleBased<?>) orbit;
            return positionAngleBased.getCachedPositionAngleType();
        } else {
            // Cartesian
            return null;
        }
    }

    /**
     * Shift orbital covariance according to linearized Keplerian motion, with corrections if non-Keplerian derivatives are present.
     * @param orbit reference orbit
     * @param convertedCovariance covariance to shift
     * @param nextOrbit target orbit
     * @param positionAngleType position angle type to use
     * @return shifted covariance
     */
    private static StateCovariance shiftCovariance(final Orbit orbit, final StateCovariance convertedCovariance,
                                                   final Orbit nextOrbit, final PositionAngleType positionAngleType) {
        final RealMatrix initialCovarianceMatrix = convertedCovariance.getMatrix();
        final GradientField gradientField = GradientField.getField(6);
        final FieldOrbit<Gradient> fieldOrbit = DerivativeStateUtils.buildOrbitGradient(gradientField, orbit);
        final FieldOrbit<Gradient> shiftedFieldOrbit = fieldOrbit.shiftedBy(nextOrbit.durationFrom(orbit));
        final Gradient[] orbitalArray = MathArrays.buildArray(gradientField, 6);
        shiftedFieldOrbit.getType().mapOrbitToArray(shiftedFieldOrbit, positionAngleType, orbitalArray, null);
        final RealMatrix stateTransitionMatrix = MatrixUtils.createRealMatrix(6, 6);
        for (int i = 0; i < orbitalArray.length; i++) {
            stateTransitionMatrix.setRow(i, orbitalArray[i].getGradient());
        }
        final RealMatrix covarianceMatrix = stateTransitionMatrix.multiply(initialCovarianceMatrix
                .multiplyTransposed(stateTransitionMatrix));
        return new StateCovariance(covarianceMatrix, nextOrbit.getDate(), nextOrbit.getFrame(),
                nextOrbit.getType(), positionAngleType);
    }

    /**
     * Convert an orbital covariance to the same coordinates and frame than the initial, reference one.
     * @param covarianceToConvert covariance to convert
     * @param nextOrbit target orbit
     * @param positionAngleType position angle type to use
     * @return converted covariance
     */
    private StateCovariance changeCovarianceAfter(final StateCovariance covarianceToConvert, final Orbit nextOrbit,
                                                  final PositionAngleType positionAngleType) {
        final Orbit propagatedOrbit = orbit.shiftedBy(nextOrbit.durationFrom(orbit));
        final LOF lof = stateCovariance.getLOF() == null ? DEFAULT_LOF : stateCovariance.getLOF();
        final StateCovariance cartesianCovarianceInFrame = covarianceToConvert.changeCovarianceType(propagatedOrbit,
                OrbitType.CARTESIAN, positionAngleType);
        final StateCovariance covarianceInLof = cartesianCovarianceInFrame.changeCovarianceFrame(propagatedOrbit, lof);
        if (stateCovariance.getLOF() != null) {
            return covarianceInLof;
        } else {
            // convert back from an arbitrary LOF to reduce approximation
            final StateCovariance covariance = covarianceInLof.changeCovarianceFrame(nextOrbit, nextOrbit.getFrame());
            if (stateCovariance.getOrbitType() != OrbitType.CARTESIAN) {
                return covariance.changeCovarianceType(propagatedOrbit, stateCovariance.getOrbitType(), positionAngleType);
            } else {
                return covariance;
            }
        }
    }
}