StateCovarianceMatrixProvider.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.linear.MatrixUtils;
- import org.hipparchus.linear.RealMatrix;
- import org.orekit.frames.Frame;
- import org.orekit.orbits.Orbit;
- import org.orekit.orbits.OrbitType;
- import org.orekit.orbits.PositionAngleType;
- import org.orekit.time.AbsoluteDate;
- /**
- * Additional state provider for state covariance matrix.
- * <p>
- * This additional state provider allows computing a propagated covariance matrix based on a user defined input state
- * covariance matrix. The computation of the propagated covariance matrix uses the State Transition Matrix between the
- * propagated spacecraft state and the initial state. As a result, the user must define the name
- * {@link #stmName of the provider for the State Transition Matrix}.
- * <p>
- * As the State Transition Matrix and the input state covariance matrix can be expressed in different orbit types, the
- * user must specify both orbit types when building the covariance provider. In addition, the position angle used in
- * both matrices must also be specified.
- * <p>
- * In order to add this additional state provider to an orbit propagator, user must use the
- * {@link Propagator#addAdditionalStateProvider(AdditionalStateProvider)} method.
- * <p>
- * For a given propagated spacecraft {@code state}, the propagated state covariance matrix is accessible through the
- * method {@link #getStateCovariance(SpacecraftState)}
- *
- * @author Bryan Cazabonne
- * @author Vincent Cucchietti
- * @since 11.3
- */
- public class StateCovarianceMatrixProvider implements AdditionalStateProvider {
- /** Dimension of the state. */
- private static final int STATE_DIMENSION = 6;
- /** Name of the state for State Transition Matrix. */
- private final String stmName;
- /** Matrix harvester to access the State Transition Matrix. */
- private final MatricesHarvester harvester;
- /** Name of the additional state. */
- private final String additionalName;
- /** Orbit type used for the State Transition Matrix. */
- private final OrbitType stmOrbitType;
- /** Position angle used for State Transition Matrix. */
- private final PositionAngleType stmAngleType;
- /** Orbit type for the covariance matrix. */
- private final OrbitType covOrbitType;
- /** Position angle used for the covariance matrix. */
- private final PositionAngleType covAngleType;
- /** Initial state covariance. */
- private StateCovariance covInit;
- /** Initial state covariance matrix. */
- private RealMatrix covMatrixInit;
- /**
- * Constructor.
- *
- * @param additionalName name of the additional state
- * @param stmName name of the state for State Transition Matrix
- * @param harvester matrix harvester as returned by
- * {@code propagator.setupMatricesComputation(stmName, null, null)}
- * @param covInit initial state covariance
- */
- public StateCovarianceMatrixProvider(final String additionalName, final String stmName,
- final MatricesHarvester harvester, final StateCovariance covInit) {
- // Initialize fields
- this.additionalName = additionalName;
- this.stmName = stmName;
- this.harvester = harvester;
- this.covInit = covInit;
- this.covOrbitType = covInit.getOrbitType();
- this.covAngleType = covInit.getPositionAngleType();
- this.stmOrbitType = harvester.getOrbitType();
- this.stmAngleType = harvester.getPositionAngleType();
- }
- /** {@inheritDoc} */
- @Override
- public String getName() {
- return additionalName;
- }
- /** {@inheritDoc} */
- @Override
- public void init(final SpacecraftState initialState, final AbsoluteDate target) {
- // Convert the initial state covariance in the same orbit type as the STM
- covInit = covInit.changeCovarianceType(initialState.getOrbit(), stmOrbitType, stmAngleType);
- // Express covariance matrix in the same frame as the STM
- final Orbit initialOrbit = initialState.getOrbit();
- final StateCovariance covInitInSTMFrame = covInit.changeCovarianceFrame(initialOrbit, initialOrbit.getFrame());
- covMatrixInit = covInitInSTMFrame.getMatrix();
- }
- /**
- * {@inheritDoc}
- * <p>
- * The covariance matrix can be computed only if the State Transition Matrix state is available.
- * </p>
- */
- @Override
- public boolean yields(final SpacecraftState state) {
- return !state.hasAdditionalState(stmName);
- }
- /** {@inheritDoc} */
- @Override
- public double[] getAdditionalState(final SpacecraftState state) {
- // State transition matrix for the input state
- final RealMatrix dYdY0 = harvester.getStateTransitionMatrix(state);
- // Compute the propagated covariance matrix
- RealMatrix propCov = dYdY0.multiply(covMatrixInit.multiplyTransposed(dYdY0));
- final StateCovariance propagated = new StateCovariance(propCov, state.getDate(), state.getFrame(), stmOrbitType, stmAngleType);
- // Update to the user defined type
- propCov = propagated.changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType).getMatrix();
- // Return the propagated covariance matrix
- return toArray(propCov);
- }
- /**
- * Get the orbit type in which the covariance matrix is expressed.
- *
- * @return the orbit type
- */
- public OrbitType getCovarianceOrbitType() {
- return covOrbitType;
- }
- /**
- * Get the state covariance in the same frame/local orbital frame, orbit type and position angle as the initial
- * covariance.
- *
- * @param state spacecraft state to which the covariance matrix should correspond
- * @return the state covariance
- * @see #getStateCovariance(SpacecraftState, Frame)
- * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
- */
- public StateCovariance getStateCovariance(final SpacecraftState state) {
- // Get the current propagated covariance
- final RealMatrix covarianceMatrix = toRealMatrix(getAdditionalState(state));
- // Create associated state covariance
- final StateCovariance covariance =
- new StateCovariance(covarianceMatrix, state.getDate(), state.getFrame(), covOrbitType, covAngleType);
- // Return the state covariance in same frame/lof as initial covariance
- if (covInit.getLOF() == null) {
- return covariance;
- }
- else {
- return covariance.changeCovarianceFrame(state.getOrbit(), covInit.getLOF());
- }
- }
- /**
- * Get the state covariance expressed in a given frame.
- * <p>
- * The output covariance matrix is expressed in the same orbit type as {@link #getCovarianceOrbitType()}.
- *
- * @param state spacecraft state to which the covariance matrix should correspond
- * @param frame output frame for which the output covariance matrix must be expressed (must be inertial)
- * @return the state covariance expressed in <code>frame</code>
- * @see #getStateCovariance(SpacecraftState)
- * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngleType)
- */
- public StateCovariance getStateCovariance(final SpacecraftState state, final Frame frame) {
- // Return the converted covariance
- return getStateCovariance(state).changeCovarianceFrame(state.getOrbit(), frame);
- }
- /**
- * Get the state covariance expressed in a given orbit type.
- *
- * @param state spacecraft state to which the covariance matrix should correspond
- * @param orbitType output orbit type
- * @param angleType output position angle (not used if orbitType equals {@code CARTESIAN})
- * @return the state covariance in <code>orbitType</code> and <code>angleType</code>
- * @see #getStateCovariance(SpacecraftState)
- * @see #getStateCovariance(SpacecraftState, Frame)
- */
- public StateCovariance getStateCovariance(final SpacecraftState state, final OrbitType orbitType,
- final PositionAngleType angleType) {
- // Return the converted covariance
- return getStateCovariance(state).changeCovarianceType(state.getOrbit(), orbitType, angleType);
- }
- /**
- * Set the covariance data into an array.
- *
- * @param covariance covariance matrix
- * @return an array containing the covariance data
- */
- private double[] toArray(final RealMatrix covariance) {
- final double[] array = new double[STATE_DIMENSION * STATE_DIMENSION];
- int index = 0;
- for (int i = 0; i < STATE_DIMENSION; ++i) {
- for (int j = 0; j < STATE_DIMENSION; ++j) {
- array[index++] = covariance.getEntry(i, j);
- }
- }
- return array;
- }
- /**
- * Convert an array to a matrix (6x6 dimension).
- *
- * @param array input array
- * @return the corresponding matrix
- */
- private RealMatrix toRealMatrix(final double[] array) {
- final RealMatrix matrix = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
- int index = 0;
- for (int i = 0; i < STATE_DIMENSION; ++i) {
- for (int j = 0; j < STATE_DIMENSION; ++j) {
- matrix.setEntry(i, j, array[index++]);
- }
- }
- return matrix;
- }
- }