Commit bab6cbbe authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Merge branch 'issue-901' into develop

parents a1571e3f 708793e8
Pipeline #2142 failed with stages
in 15 minutes and 33 seconds
/* Copyright 2002-2022 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.Array2DRowRealMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.orekit.frames.Frame;
import org.orekit.frames.Transform;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.time.AbsoluteDate;
import org.orekit.utils.CartesianDerivativesFilter;
/**
* 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)}
* <p>
* It is possible to change the covariance frame by using the
* {@link #changeCovarianceFrame(AbsoluteDate, Frame, Frame, RealMatrix)}
* method. This method is based on Equation (18) of <i>Covariance
* Transformations for Satellite Flight Dynamics Operations</i>
* by David A. Vallado. It is important to highlight that the frames
* must be inertial frames.
* <p>
* Finally, covariance orbit type can be changed using the
* {@link #changeCovarianceType(SpacecraftState, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)}
* method.
* </p>
* @author Bryan Cazabonne
* @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 PositionAngle stmAngleType;
/** Orbit type for the covariance matrix. */
private final OrbitType covOrbitType;
/** Position angle used for the covariance matrix. */
private final PositionAngle covAngleType;
/** Initial state covariance matrix. */
private RealMatrix covInit;
/**
* 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 stmOrbitType orbit type used for the State Transition Matrix computation
* @param stmAngleType position angle used for State Transition Matrix computation
* (not used if stmOrbitType equals {@code CARTESIAN})
* @param covInit initial state covariance matrix (6x6 dimension)
* @param covOrbitType orbit type for the covariance matrix
* @param covAngleType position angle used for the covariance matrix
* (not used if covOrbitType equals {@code CARTESIAN})
*/
public StateCovarianceMatrixProvider(final String additionalName, final String stmName,
final MatricesHarvester harvester,
final OrbitType stmOrbitType, final PositionAngle stmAngleType,
final RealMatrix covInit,
final OrbitType covOrbitType, final PositionAngle covAngleType) {
// Initialize fields
this.additionalName = additionalName;
this.stmName = stmName;
this.harvester = harvester;
this.covInit = covInit;
this.stmOrbitType = stmOrbitType;
this.stmAngleType = stmAngleType;
this.covOrbitType = covOrbitType;
this.covAngleType = covAngleType;
}
/** Get the orbit type in which the covariance matrix is expressed.
* @return the orbit type
*/
public OrbitType getCovarianceOrbitType() {
return covOrbitType;
}
/** {@inheritDoc} */
@Override
public String getName() {
return additionalName;
}
/** {@inheritDoc} */
@Override
public void init(final SpacecraftState initialState, final AbsoluteDate target) {
// Convert the initial covariance matrix in the same orbit type as the STM
covInit = changeCovarianceType(initialState.getOrbit(),
covOrbitType, covAngleType,
stmOrbitType, stmAngleType,
covInit);
}
/** {@inheritDoc}
* <p>
* The covariance matrix can be computed only if the State Transition Matrix state is available.
* </p>
*/
@Override
public boolean yield(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(covInit.multiplyTransposed(dYdY0));
// Update to the user defined type
propCov = changeCovarianceType(state.getOrbit(),
stmOrbitType, stmAngleType,
covOrbitType, covAngleType,
propCov);
// Return the propagated covariance matrix
return toArray(propCov);
}
/** Get the state covariance matrix (6x6 dimension).
* <p>
* The output covariance matrix is expressed in the
* same orbit type as {@link #getCovarianceOrbitType()}.
* <p>
* It is possible to change the covariance frame by using the
* {@link #changeCovarianceFrame(AbsoluteDate, Frame, Frame, RealMatrix)}
* method.
* <p>
* It is also possible to change the covariance orbit type by
* using the {@link #changeCovarianceType(SpacecraftState, OrbitType, PositionAngle,
* OrbitType, PositionAngle, RealMatrix)}
* method.
* @param state spacecraft state to which the covariance matrix should correspond
* @return the state covariance matrix
*/
public RealMatrix getStateCovariance(final SpacecraftState state) {
// Get the current propagated covariance
final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName));
// Return the converted covariance
return covariance;
}
/** Get the state covariance matrix (6x6 dimension) expressed in a given frame.
* <p>
* The output covariance matrix is expressed in the
* same orbit type as {@link #getCovarianceOrbitType()}.
* <p>
* It is also possible to change the covariance orbit type by
* using the {@link #changeCovarianceType(SpacecraftState, OrbitType, PositionAngle,
* OrbitType, PositionAngle, RealMatrix)}
* method.
* @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 matrix expressed in <code>frame</code>
*/
public RealMatrix getStateCovariance(final SpacecraftState state, final Frame frame) {
// Get the current propagated covariance
final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName));
// Return the converted covariance
return changeCovarianceFrame(state.getOrbit(), state.getFrame(), frame, covariance, covOrbitType, covAngleType);
}
/** Get the state covariance matrix (6x6 dimension) 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 matrix in <code>orbitType</code> and <code>angleType</code>
*/
public RealMatrix getStateCovariance(final SpacecraftState state, final OrbitType orbitType, final PositionAngle angleType) {
// Get the current propagated covariance
final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName));
// Return the converted covariance
return changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType, orbitType, angleType, covariance);
}
/** Get the covariance matrix in another frame.
* <p>
* The transformation is based on Equation (18) of
* "Covariance Transformations for Satellite Flight Dynamics Operations"
* by David A. Vallado".
* <p>
* As the frame transformation must be performed with the covariance
* expressed in Cartesian elements, both the orbit and position angle
* types of the input covariance must be provided.
* @param orbit orbit to which the covariance matrix should correspond
* @param frameIn the frame in which the input covariance matrix is expressed (must be inertial)
* @param frameOut the target frame (must be inertial)
* @param inputCov input covariance
* @param covOrbitType orbit type of the covariance matrix
* @param covAngleType position angle type of the covariance matrix
* (not used if covOrbitType equals {@code CARTESIAN})
* @return the covariance matrix expressed in the target frame
*/
public static RealMatrix changeCovarianceFrame(final Orbit orbit,
final Frame frameIn, final Frame frameOut,
final RealMatrix inputCov,
final OrbitType covOrbitType, final PositionAngle covAngleType) {
// Convert input matrix to Cartesian parameters in input frame
final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType,
OrbitType.CARTESIAN, PositionAngle.MEAN,
inputCov);
// Get the transform from the covariance frame to the output frame
final Transform inToOut = frameIn.getTransformTo(frameOut, orbit.getDate());
// Get the Jacobian of the transform
final double[][] jacobian = new double[STATE_DIMENSION][STATE_DIMENSION];
inToOut.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
// Matrix to perform the covariance transformation
final RealMatrix j = new Array2DRowRealMatrix(jacobian, false);
// Get the Cartesian covariance matrix converted to frameOut
final RealMatrix cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j));
// Convert orbit frame to output frame
final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameOut), frameOut, orbit.getMu());
// Convert output Cartesian matrix to initial orbit type and position angle
return changeCovarianceType(outOrbit, OrbitType.CARTESIAN, PositionAngle.MEAN,
covOrbitType, covAngleType, cartesianCovarianceOut);
}
/** Get the covariance matrix in another orbit type.
* @param orbit orbit to which the covariance matrix should correspond
* @param inOrbitType initial orbit type of the state covariance matrix
* @param inAngleType initial position angle type of the state covariance matrix
* @param outOrbitType target orbit type of the state covariance matrix
* @param outAngleType target position angle type of the state covariance matrix
* @param inputCov input covariance
* @return the covariance expressed in the target orbit type with the
* target position angle
*/
public static RealMatrix changeCovarianceType(final Orbit orbit,
final OrbitType inOrbitType, final PositionAngle inAngleType,
final OrbitType outOrbitType, final PositionAngle outAngleType,
final RealMatrix inputCov) {
// Notations:
// I: Input orbit type
// O: Output orbit type
// C: Cartesian parameters
// Compute dOutputdCartesian
final double[][] aOC = new double[STATE_DIMENSION][STATE_DIMENSION];
final Orbit orbitInOutputType = outOrbitType.convertType(orbit);
orbitInOutputType.getJacobianWrtCartesian(outAngleType, aOC);
final RealMatrix dOdC = new Array2DRowRealMatrix(aOC, false);
// Compute dCartesiandInput
final double[][] aCI = new double[STATE_DIMENSION][STATE_DIMENSION];
final Orbit orbitInInputType = inOrbitType.convertType(orbit);
orbitInInputType.getJacobianWrtParameters(inAngleType, aCI);
final RealMatrix dCdI = new Array2DRowRealMatrix(aCI, false);
// Compute dOutputdInput
final RealMatrix dOdI = dOdC.multiply(dCdI);
// Conversion of the state covariance matrix in target orbit type
final RealMatrix outputCov = dOdI.multiply(inputCov.multiplyTransposed(dOdI));
// Return the converted covariance
return outputCov;
}
/** 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;
}
/** 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;
}
}
......@@ -24,6 +24,7 @@ import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.orekit.orbits.FieldOrbit;
import org.orekit.propagation.AbstractMatricesHarvester;
import org.orekit.propagation.AdditionalStateProvider;
import org.orekit.propagation.FieldSpacecraftState;
import org.orekit.propagation.SpacecraftState;
import org.orekit.time.AbsoluteDate;
......@@ -39,7 +40,7 @@ import org.orekit.utils.ParameterDriver;
* @author Bryan Cazabonne
* @since 11.1
*/
public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatricesHarvester {
public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatricesHarvester implements AdditionalStateProvider {
/** Columns names for parameters. */
private List<String> columnsNames;
......@@ -94,11 +95,28 @@ public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatric
/** {@inheritDoc} */
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
public String getName() {
return getStmName();
}
/** {@inheritDoc} */
@Override
public double[] getAdditionalState(final SpacecraftState state) {
// Update the partial derivatives if needed
updateDerivativesIfNeeded(state);
//return the state transition matrix
return MatrixUtils.createRealMatrix(analyticalDerivativesStm);
// Return the state transition matrix in an array
return toArray(analyticalDerivativesStm);
}
/** {@inheritDoc} */
@Override
public RealMatrix getStateTransitionMatrix(final SpacecraftState state) {
// Check if additional state is defined
if (!state.hasAdditionalState(getName())) {
return null;
}
// Return the state transition matrix
return toRealMatrix(state.getAdditionalState(getName()));
}
/** {@inheritDoc} */
......@@ -214,6 +232,37 @@ public abstract class AbstractAnalyticalMatricesHarvester extends AbstractMatric
}
}
/** 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;
}
/** Set the STM data into an array.
* @param matrix STM matrix
* @return an array containing the STM data
*/
private double[] toArray(final double[][] matrix) {
final double[] array = new double[STATE_DIMENSION * STATE_DIMENSION];
int index = 0;
for (int i = 0; i < STATE_DIMENSION; ++i) {
final double[] row = matrix[i];
for (int j = 0; j < STATE_DIMENSION; ++j) {
array[index++] = row[j];
}
}
return array;
}
/**
* Get the gradient converter related to the analytical orbit propagator.
* @return the gradient converter
......
......@@ -828,7 +828,12 @@ public class BrouwerLyddanePropagator extends AbstractAnalyticalPropagator {
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
return new BrouwerLyddaneHarvester(this, stmName, initialStm, initialJacobianColumns);
// Create the harvester
final BrouwerLyddaneHarvester harvester = new BrouwerLyddaneHarvester(this, stmName, initialStm, initialJacobianColumns);
// Update the list of additional state provider
addAdditionalStateProvider(harvester);
// Return the configured harvester
return harvester;
}
/**
......
......@@ -757,7 +757,12 @@ public class EcksteinHechlerPropagator extends AbstractAnalyticalPropagator {
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
return new EcksteinHechlerHarvester(this, stmName, initialStm, initialJacobianColumns);
// Create the harvester
final EcksteinHechlerHarvester harvester = new EcksteinHechlerHarvester(this, stmName, initialStm, initialJacobianColumns);
// Update the list of additional state provider
addAdditionalStateProvider(harvester);
// Return the configured harvester
return harvester;
}
/** Local class for Eckstein-Hechler model, with fixed mean parameters. */
......
......@@ -198,7 +198,12 @@ public class KeplerianPropagator extends AbstractAnalyticalPropagator {
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
return new KeplerianHarvester(this, stmName, initialStm, initialJacobianColumns);
// Create the harvester
final KeplerianHarvester harvester = new KeplerianHarvester(this, stmName, initialStm, initialJacobianColumns);
// Update the list of additional state provider
addAdditionalStateProvider(harvester);
// Return the configured harvester
return harvester;
}
}
......@@ -594,7 +594,12 @@ public abstract class TLEPropagator extends AbstractAnalyticalPropagator {
@Override
protected AbstractMatricesHarvester createHarvester(final String stmName, final RealMatrix initialStm,
final DoubleArrayDictionary initialJacobianColumns) {
return new TLEHarvester(this, stmName, initialStm, initialJacobianColumns);
// Create the harvester
final TLEHarvester harvester = new TLEHarvester(this, stmName, initialStm, initialJacobianColumns);
// Update the list of additional state provider
addAdditionalStateProvider(harvester);
// Return the configured harvester
return harvester;
}
/**
......
......@@ -340,7 +340,7 @@ public class TLEBatchLSEstimatorTest {
// we have low correlation between the two types of measurement. We can expect a good estimate.
TLEEstimationTestUtils.checkFit(context, estimator, 4, 5,
0.0, 5.2e-6,
0.0, 3.3e-5,
0.0, 3.4e-5,
0.0, 6.1e-6,
0.0, 2.5e-9);
}
......
/* Copyright 2002-2022 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.geometry.euclidean.threed.Vector3D;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
import org.hipparchus.ode.ODEIntegrator;
import org.hipparchus.ode.nonstiff.ClassicalRungeKuttaIntegrator;
import org.hipparchus.util.FastMath;
import org.junit.Assert;
import org.junit.Before;
import org.junit.Test;
import org.orekit.Utils;
import org.orekit.forces.ForceModel;
import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel;
import org.orekit.forces.gravity.potential.GravityFieldFactory;
import org.orekit.forces.gravity.potential.ICGEMFormatReader;
import org.orekit.forces.gravity.potential.NormalizedSphericalHarmonicsProvider;
import org.orekit.frames.Frame;
import org.orekit.frames.FramesFactory;
import org.orekit.orbits.CartesianOrbit;
import org.orekit.orbits.Orbit;
import org.orekit.orbits.OrbitType;
import org.orekit.orbits.PositionAngle;
import org.orekit.propagation.analytical.EcksteinHechlerPropagator;
import org.orekit.propagation.numerical.NumericalPropagator;
import org.orekit.time.AbsoluteDate;
import org.orekit.time.TimeScalesFactory;
import org.orekit.utils.Constants;
import org.orekit.utils.IERSConventions;
import org.orekit.utils.PVCoordinates;
public class StateCovarianceMatrixProviderTest {
private SpacecraftState initialState;
private double[][] initCov;