StateCovarianceMatrixProvider.java
/* Copyright 2002-2023 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(state.getAdditionalState(additionalName));
// 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;
}
}