diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java
new file mode 100644
index 0000000000000000000000000000000000000000..d94bf4f8f9d5e61b48e7dd0f0b75ebc39e6039e2
--- /dev/null
+++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java
@@ -0,0 +1,371 @@
+/* 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.
+ *
+ * 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}.
+ *
+ * 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.
+ *
+ * In order to add this additional state provider to an orbit
+ * propagator, user must use the
+ * {@link Propagator#addAdditionalStateProvider(AdditionalStateProvider)}
+ * method.
+ *
+ * For a given propagated spacecraft {@code state}, the propagated state
+ * covariance matrix is accessible through the method
+ * {@link #getStateCovariance(SpacecraftState)}
+ *
+ * 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 Covariance
+ * Transformations for Satellite Flight Dynamics Operations
+ * by David A. Vallado. It is important to highlight that the frames
+ * must be inertial frames.
+ *
+ * Finally, covariance orbit type can be changed using the
+ * {@link #changeCovarianceType(SpacecraftState, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)}
+ * method.
+ *
+ * @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}
+ *
+ * The covariance matrix can be computed only if the State Transition Matrix state is available.
+ *
+ */
+ @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).
+ *
+ * The output covariance matrix is expressed in the
+ * same orbit type as {@link #getCovarianceOrbitType()}.
+ *
+ * It is possible to change the covariance frame by using the
+ * {@link #changeCovarianceFrame(AbsoluteDate, Frame, Frame, RealMatrix)}
+ * method.
+ *
+ * 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.
+ *
+ * The output covariance matrix is expressed in the
+ * same orbit type as {@link #getCovarianceOrbitType()}.
+ *
+ * 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 frame
+ */
+ 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 orbitType and angleType
+ */
+ 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.
+ *
+ * The transformation is based on Equation (18) of
+ * "Covariance Transformations for Satellite Flight Dynamics Operations"
+ * by David A. Vallado".
+ *
+ * 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;
+ }
+
+}
diff --git a/src/main/java/org/orekit/propagation/analytical/AbstractAnalyticalMatricesHarvester.java b/src/main/java/org/orekit/propagation/analytical/AbstractAnalyticalMatricesHarvester.java
index 06a6adb9cde89455e67e5ac4900bcab4a56e787d..da72be72c07217536cc7b34596ad8132e98ad055 100644
--- a/src/main/java/org/orekit/propagation/analytical/AbstractAnalyticalMatricesHarvester.java
+++ b/src/main/java/org/orekit/propagation/analytical/AbstractAnalyticalMatricesHarvester.java
@@ -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 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
diff --git a/src/main/java/org/orekit/propagation/analytical/BrouwerLyddanePropagator.java b/src/main/java/org/orekit/propagation/analytical/BrouwerLyddanePropagator.java
index 84daef302b6c5efcf22b40304712063e35aef7c0..940d8e86dfd1513d91acf3bf08d735dc351c8798 100644
--- a/src/main/java/org/orekit/propagation/analytical/BrouwerLyddanePropagator.java
+++ b/src/main/java/org/orekit/propagation/analytical/BrouwerLyddanePropagator.java
@@ -648,7 +648,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;
}
/**
diff --git a/src/main/java/org/orekit/propagation/analytical/EcksteinHechlerPropagator.java b/src/main/java/org/orekit/propagation/analytical/EcksteinHechlerPropagator.java
index 4ce46c82a4ea8254042eb84c5e4db17882e59519..7287aaccc72a747a66a379032caf66aa863647be 100644
--- a/src/main/java/org/orekit/propagation/analytical/EcksteinHechlerPropagator.java
+++ b/src/main/java/org/orekit/propagation/analytical/EcksteinHechlerPropagator.java
@@ -583,7 +583,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. */
diff --git a/src/main/java/org/orekit/propagation/analytical/KeplerianPropagator.java b/src/main/java/org/orekit/propagation/analytical/KeplerianPropagator.java
index 9254260bf1667eb37d4f8453cd5f024e12fc66c5..fa07d16f1d5b8120d93fac6e14a72c58a1a6e272 100644
--- a/src/main/java/org/orekit/propagation/analytical/KeplerianPropagator.java
+++ b/src/main/java/org/orekit/propagation/analytical/KeplerianPropagator.java
@@ -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;
}
}
diff --git a/src/main/java/org/orekit/propagation/analytical/tle/TLEPropagator.java b/src/main/java/org/orekit/propagation/analytical/tle/TLEPropagator.java
index 10ae0ec31bea7ae09920fcd36395cff4259615d4..e4d76404dd65b610de28fbd63d3f56313200f79b 100644
--- a/src/main/java/org/orekit/propagation/analytical/tle/TLEPropagator.java
+++ b/src/main/java/org/orekit/propagation/analytical/tle/TLEPropagator.java
@@ -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;
}
/**
diff --git a/src/test/java/org/orekit/estimation/leastsquares/TLEBatchLSEstimatorTest.java b/src/test/java/org/orekit/estimation/leastsquares/TLEBatchLSEstimatorTest.java
index afd2924a6fbf8803b8412aa0ef2c575e68d9afed..2cb89cf7f35c0e80bb02d82691a411b7f811fa58 100644
--- a/src/test/java/org/orekit/estimation/leastsquares/TLEBatchLSEstimatorTest.java
+++ b/src/test/java/org/orekit/estimation/leastsquares/TLEBatchLSEstimatorTest.java
@@ -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);
}
diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java
new file mode 100644
index 0000000000000000000000000000000000000000..12a4336ebb2921548a8f4077a53c70d8ddc88d59
--- /dev/null
+++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java
@@ -0,0 +1,386 @@
+/* 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;
+
+ @Before
+ public void setUp() {
+ Utils.setDataRoot("orbit-determination/february-2016:potential/icgem-format");
+ GravityFieldFactory.addPotentialCoefficientsReader(new ICGEMFormatReader("eigen-6s-truncated", true));
+ Orbit initialOrbit = new CartesianOrbit(new PVCoordinates(new Vector3D(7526993.581890527, -9646310.10026971, 1464110.4928112086),
+ new Vector3D(3033.79456099698, 1715.265069098717, -4447.658745923895)),
+ FramesFactory.getEME2000(),
+ new AbsoluteDate("2016-02-13T16:00:00.000", TimeScalesFactory.getUTC()),
+ Constants.WGS84_EARTH_MU);
+ initialState = new SpacecraftState(initialOrbit);
+ initCov = new double[][] {
+ {8.651816029e+01, 5.689987127e+01, -2.763870764e+01, -2.435617201e-02, 2.058274137e-02, -5.872883051e-03},
+ {5.689987127e+01, 7.070624321e+01, 1.367120909e+01, -6.112622013e-03, 7.623626008e-03, -1.239413190e-02},
+ {-2.763870764e+01, 1.367120909e+01, 1.811858898e+02, 3.143798992e-02, -4.963106559e-02, -7.420114385e-04},
+ {-2.435617201e-02, -6.112622013e-03, 3.143798992e-02, 4.657077389e-05, 1.469943634e-05, 3.328475593e-05},
+ {2.058274137e-02, 7.623626008e-03, -4.963106559e-02, 1.469943634e-05, 3.950715934e-05, 2.516044258e-05},
+ {-5.872883051e-03, -1.239413190e-02, -7.420114385e-04, 3.328475593e-05, 2.516044258e-05, 3.547466120e-05}
+ };
+ }
+
+ /**
+ * Unit test for the covariance frame transformation.
+ */
+ @Test
+ public void testFrameConversion() {
+
+ // Reference
+ final RealMatrix referenceCov = MatrixUtils.createRealMatrix(initCov);
+
+ // Define frames
+ final Frame frameA = FramesFactory.getEME2000();
+ final Frame frameB = FramesFactory.getTEME();
+
+ // First transformation
+ RealMatrix transformedCov = StateCovarianceMatrixProvider.changeCovarianceFrame(initialState.getOrbit(), frameA, frameB, referenceCov, OrbitType.CARTESIAN, PositionAngle.MEAN);
+
+ // Second transformation
+ transformedCov = StateCovarianceMatrixProvider.changeCovarianceFrame(initialState.getOrbit(), frameB, frameA, transformedCov, OrbitType.CARTESIAN, PositionAngle.MEAN);
+
+ // Verify
+ compareCovariance(referenceCov, transformedCov, 5.2e-15);
+
+ }
+
+ /**
+ * Unit test for the covariance type transformation.
+ */
+ @Test
+ public void testTypeConversion() {
+
+ // Reference
+ final RealMatrix referenceCov = MatrixUtils.createRealMatrix(initCov);
+
+ // Define orbit types
+ final OrbitType cart = OrbitType.CARTESIAN;
+ final OrbitType kep = OrbitType.KEPLERIAN;
+
+ // First transformation
+ RealMatrix transformedCov = StateCovarianceMatrixProvider.changeCovarianceType(initialState.getOrbit(), cart, PositionAngle.MEAN,
+ kep, PositionAngle.MEAN, referenceCov);
+
+ // Second transformation
+ transformedCov = StateCovarianceMatrixProvider.changeCovarianceType(initialState.getOrbit(), kep, PositionAngle.MEAN,
+ cart, PositionAngle.MEAN, transformedCov);
+
+ // Verify
+ compareCovariance(referenceCov, transformedCov, 3.5e-12);
+
+ }
+
+ /**
+ * Unit test for covariance propagation in Cartesian elements.
+ */
+ @Test
+ public void testWithNumericalPropagatorCartesian() {
+
+ // Integrator
+ final double step = 60.0;
+ final ODEIntegrator integrator = new ClassicalRungeKuttaIntegrator(step);
+
+ // Numerical propagator
+ final String stmName = "STM";
+ final OrbitType propType = OrbitType.CARTESIAN;
+ final PositionAngle angleType = PositionAngle.MEAN;
+ final NumericalPropagator propagator = new NumericalPropagator(integrator);
+ // Add a force model
+ final NormalizedSphericalHarmonicsProvider gravity = GravityFieldFactory.getNormalizedProvider(2, 0);
+ final ForceModel holmesFeatherstone = new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), gravity);
+ propagator.addForceModel(holmesFeatherstone);
+ // Finalize setting
+ final MatricesHarvester harvester = propagator.setupMatricesComputation(stmName, null, null);
+ propagator.setOrbitType(propType);
+ propagator.setPositionAngleType(angleType);
+
+ // Create additional state
+ final String additionalName = "cartCov";
+ final RealMatrix initialCov = MatrixUtils.createRealMatrix(initCov);
+ final StateCovarianceMatrixProvider provider = new StateCovarianceMatrixProvider(additionalName, stmName, harvester,
+ propagator.getOrbitType(), propagator.getPositionAngleType(),
+ initialCov,
+ OrbitType.CARTESIAN, PositionAngle.MEAN);
+ propagator.setInitialState(initialState);
+ propagator.addAdditionalStateProvider(provider);
+
+ // Propagate
+ final SpacecraftState propagated = propagator.propagate(initialState.getDate().shiftedBy(Constants.JULIAN_DAY));
+
+ // Get the propagated covariance
+ final RealMatrix propagatedCov = provider.getStateCovariance(propagated);
+
+ // Reference (computed using a different solution)
+ final double[][] ref = new double[][] {
+ { 5.770543135e+02, 2.316979550e+02, -5.172369105e+02, -2.585893247e-01, 2.113809017e-01, -1.759509343e-01},
+ { 2.316979550e+02, 1.182942930e+02, -1.788422178e+02, -9.570305681e-02, 7.792155309e-02, -7.435822327e-02},
+ {-5.172369105e+02, -1.788422178e+02, 6.996248500e+02, 2.633605389e-01, -2.480144888e-01, 1.908427233e-01},
+ {-2.585893247e-01, -9.570305681e-02, 2.633605389e-01, 1.419148897e-04, -8.715858320e-05, 1.024944399e-04},
+ { 2.113809017e-01, 7.792155309e-02, -2.480144888e-01, -8.715858320e-05, 1.069566588e-04, -5.667563856e-05},
+ {-1.759509343e-01, -7.435822327e-02, 1.908427233e-01, 1.024944399e-04, -5.667563856e-05, 8.178356868e-05}
+ };
+ final RealMatrix referenceCov = MatrixUtils.createRealMatrix(ref);
+
+ // Verify
+ compareCovariance(referenceCov, propagatedCov, 4.0e-7);
+ Assert.assertEquals(OrbitType.CARTESIAN, provider.getCovarianceOrbitType());
+
+ ///////////
+ // Test the frame transformation
+ ///////////
+
+ // Define a new output frame
+ final Frame frameB = FramesFactory.getTEME();
+
+ // Get the covariance in TEME frame
+ RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB);
+
+ // Second transformation
+ RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN);
+
+ // Verify
+ compareCovariance(transformedCovA, transformedCovB, 1.0e-15);
+
+ ///////////
+ // Test the orbit type transformation
+ ///////////
+
+ // Define a new output frame
+ final OrbitType outOrbitType = OrbitType.KEPLERIAN;
+ final PositionAngle outAngleType = PositionAngle.MEAN;
+
+ // Transformation using getStateJacobian() method
+ RealMatrix transformedCovC = provider.getStateCovariance(propagated, outOrbitType, outAngleType);
+
+ // Second transformation
+ RealMatrix transformedCovD = StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, PositionAngle.MEAN, outOrbitType, outAngleType, propagatedCov);
+
+ // Verify
+ compareCovariance(transformedCovC, transformedCovD, 1.0e-15);
+
+ }
+
+ /**
+ * Unit test for covariance propagation in Cartesian elements.
+ * The difference here is that the propagator uses its default orbit type: EQUINOCTIAL
+ */
+ @Test
+ public void testWithNumericalPropagatorDefault() {
+
+ // Integrator
+ final double step = 60.0;
+ final ODEIntegrator integrator = new ClassicalRungeKuttaIntegrator(step);
+
+ // Numerical propagator
+ final String stmName = "STM";
+ final NumericalPropagator propagator = new NumericalPropagator(integrator);
+ // Add a force model
+ final NormalizedSphericalHarmonicsProvider gravity = GravityFieldFactory.getNormalizedProvider(2, 0);
+ final ForceModel holmesFeatherstone = new HolmesFeatherstoneAttractionModel(FramesFactory.getITRF(IERSConventions.IERS_2010, true), gravity);
+ propagator.addForceModel(holmesFeatherstone);
+ // Finalize setting
+ final MatricesHarvester harvester = propagator.setupMatricesComputation(stmName, null, null);
+
+ // Create additional state
+ final String additionalName = "cartCov";
+ final RealMatrix initialCov = MatrixUtils.createRealMatrix(initCov);
+ final StateCovarianceMatrixProvider provider = new StateCovarianceMatrixProvider(additionalName, stmName, harvester,
+ propagator.getOrbitType(), propagator.getPositionAngleType(),
+ initialCov,
+ OrbitType.CARTESIAN, PositionAngle.MEAN);
+ propagator.setInitialState(initialState);
+ propagator.addAdditionalStateProvider(provider);
+
+ // Propagate
+ final SpacecraftState propagated = propagator.propagate(initialState.getDate().shiftedBy(Constants.JULIAN_DAY));
+
+ // Get the propagated covariance
+ final RealMatrix propagatedCov = provider.getStateCovariance(propagated);
+
+ // Reference (computed using a different solution)
+ final double[][] ref = new double[][] {
+ { 5.770543135e+02, 2.316979550e+02, -5.172369105e+02, -2.585893247e-01, 2.113809017e-01, -1.759509343e-01},
+ { 2.316979550e+02, 1.182942930e+02, -1.788422178e+02, -9.570305681e-02, 7.792155309e-02, -7.435822327e-02},
+ {-5.172369105e+02, -1.788422178e+02, 6.996248500e+02, 2.633605389e-01, -2.480144888e-01, 1.908427233e-01},
+ {-2.585893247e-01, -9.570305681e-02, 2.633605389e-01, 1.419148897e-04, -8.715858320e-05, 1.024944399e-04},
+ { 2.113809017e-01, 7.792155309e-02, -2.480144888e-01, -8.715858320e-05, 1.069566588e-04, -5.667563856e-05},
+ {-1.759509343e-01, -7.435822327e-02, 1.908427233e-01, 1.024944399e-04, -5.667563856e-05, 8.178356868e-05}
+ };
+ final RealMatrix referenceCov = MatrixUtils.createRealMatrix(ref);
+
+ // Verify
+ compareCovariance(referenceCov, propagatedCov, 3.0e-5);
+ Assert.assertEquals(OrbitType.CARTESIAN, provider.getCovarianceOrbitType());
+
+ ///////////
+ // Test the frame transformation
+ ///////////
+
+ // Define a new output frame
+ final Frame frameB = FramesFactory.getTEME();
+
+ // Get the covariance in TEME frame
+ RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB);
+
+ // Second transformation
+ RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN);
+
+ // Verify
+ compareCovariance(transformedCovA, transformedCovB, 1.0e-15);
+
+ // Define a new output frame
+ final OrbitType outOrbitType = OrbitType.KEPLERIAN;
+ final PositionAngle outAngleType = PositionAngle.MEAN;
+
+ // Transformation using getStateJacobian() method
+ RealMatrix transformedCovC = provider.getStateCovariance(propagated, outOrbitType, outAngleType);
+
+ // Second transformation
+ RealMatrix transformedCovD = StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, PositionAngle.MEAN, outOrbitType, outAngleType, propagatedCov);
+
+ // Verify
+ compareCovariance(transformedCovC, transformedCovD, 1.0e-15);
+
+ }
+
+ /**
+ * Unit test for covariance propagation in Cartesian elements.
+ */
+ @Test
+ public void testWithAnalyticalPropagator() {
+
+ // Numerical propagator
+ final String stmName = "STM";
+ final OrbitType propType = OrbitType.CARTESIAN;
+ final PositionAngle angleType = PositionAngle.MEAN;
+ final EcksteinHechlerPropagator propagator = new EcksteinHechlerPropagator(initialState.getOrbit(),
+ GravityFieldFactory.getUnnormalizedProvider(6, 0));
+ // Finalize setting
+ final MatricesHarvester harvester = propagator.setupMatricesComputation(stmName, null, null);
+
+ // Create additional state
+ final String additionalName = "cartCov";
+ final RealMatrix initialCov = MatrixUtils.createRealMatrix(initCov);
+ final StateCovarianceMatrixProvider provider = new StateCovarianceMatrixProvider(additionalName, stmName, harvester,
+ propType, angleType,
+ initialCov,
+ OrbitType.CARTESIAN, PositionAngle.MEAN);
+ propagator.addAdditionalStateProvider(provider);
+
+ // Propagate
+ final SpacecraftState propagated = propagator.propagate(initialState.getDate().shiftedBy(Constants.JULIAN_DAY));
+
+ // Get the propagated covariance
+ final RealMatrix propagatedCov = provider.getStateCovariance(propagated);
+
+ // Reference (computed using a numerical solution)
+ final double[][] ref = new double[][] {
+ { 5.770543135e+02, 2.316979550e+02, -5.172369105e+02, -2.585893247e-01, 2.113809017e-01, -1.759509343e-01},
+ { 2.316979550e+02, 1.182942930e+02, -1.788422178e+02, -9.570305681e-02, 7.792155309e-02, -7.435822327e-02},
+ {-5.172369105e+02, -1.788422178e+02, 6.996248500e+02, 2.633605389e-01, -2.480144888e-01, 1.908427233e-01},
+ {-2.585893247e-01, -9.570305681e-02, 2.633605389e-01, 1.419148897e-04, -8.715858320e-05, 1.024944399e-04},
+ { 2.113809017e-01, 7.792155309e-02, -2.480144888e-01, -8.715858320e-05, 1.069566588e-04, -5.667563856e-05},
+ {-1.759509343e-01, -7.435822327e-02, 1.908427233e-01, 1.024944399e-04, -5.667563856e-05, 8.178356868e-05}
+ };
+ final RealMatrix referenceCov = MatrixUtils.createRealMatrix(ref);
+
+ // Verify
+ compareCovariance(referenceCov, propagatedCov, 5.0e-4);
+ Assert.assertEquals(OrbitType.CARTESIAN, provider.getCovarianceOrbitType());
+
+ ///////////
+ // Test the frame transformation
+ ///////////
+
+ // Define a new output frame
+ final Frame frameB = FramesFactory.getTEME();
+
+ // Get the covariance in TEME frame
+ RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB);
+
+ // Second transformation
+ RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN);
+
+ // Verify
+ compareCovariance(transformedCovA, transformedCovB, 1.0e-15);
+
+ // Define a new output frame
+ final OrbitType outOrbitType = OrbitType.KEPLERIAN;
+ final PositionAngle outAngleType = PositionAngle.MEAN;
+
+ // Transformation using getStateJacobian() method
+ RealMatrix transformedCovC = provider.getStateCovariance(propagated, outOrbitType, outAngleType);
+
+ // Second transformation
+ RealMatrix transformedCovD = StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, PositionAngle.MEAN, outOrbitType, outAngleType, propagatedCov);
+
+ // Verify
+ compareCovariance(transformedCovC, transformedCovD, 1.0e-15);
+
+ }
+
+ /**
+ * Compare two covariance matrices
+ * @param reference reference covariance
+ * @param computed computed covariance
+ * @param threshold threshold for comparison
+ */
+ private void compareCovariance(final RealMatrix reference, final RealMatrix computed, final double threshold) {
+ for (int row = 0; row < reference.getRowDimension(); row++) {
+ for (int column = 0; column < reference.getColumnDimension(); column++) {
+ Assert.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column), FastMath.abs(threshold * reference.getEntry(row, column)));
+ }
+ }
+ }
+
+}
diff --git a/src/test/java/org/orekit/propagation/analytical/tle/TLEStateTransitionMatrixTest.java b/src/test/java/org/orekit/propagation/analytical/tle/TLEStateTransitionMatrixTest.java
index b37976b89a156688432f7dba7c41c3e7c3096495..7cd3487b9bf0f9f1d204b4c582330eef7f01bf6d 100644
--- a/src/test/java/org/orekit/propagation/analytical/tle/TLEStateTransitionMatrixTest.java
+++ b/src/test/java/org/orekit/propagation/analytical/tle/TLEStateTransitionMatrixTest.java
@@ -64,15 +64,7 @@ public class TLEStateTransitionMatrixTest {
final AbsoluteDate target = initialState.getDate().shiftedBy(initialState.getKeplerianPeriod());
MatricesHarvester harvester = propagator.setupMatricesComputation("stm", null, null);
RealMatrix dYdY0 = harvester.getStateTransitionMatrix(initialState);
- for (int i = 0; i < 6; ++i) {
- for (int j = 0; j < 6; ++j) {
- if (i == j) {
- Assert.assertEquals(1.0, dYdY0.getEntry(i, j), tolerance);
- } else {
- Assert.assertEquals(0.0, dYdY0.getEntry(i, j), tolerance);
- }
- }
- }
+ Assert.assertNull(dYdY0);
final SpacecraftState finalState = propagator.propagate(target);
dYdY0 = harvester.getStateTransitionMatrix(finalState);