Skip to content
GitLab
Commit 129b2d53 authored by Bryan Cazabonne's avatar Bryan Cazabonne
Browse files

Covariance transformation involving LOF shall the full Jacobian.

parent a3e618a6
Pipeline #2616 passed with stages
in 35 minutes and 11 seconds
Hide whitespace changes
Inline Side-by-side
src/main/java/org/orekit/propagation/StateCovariance.java
View file @ 129b2d53
......@@ -16,7 +16,6 @@
*/
package org.orekit.propagation;
import org.hipparchus.geometry.euclidean.threed.Rotation;
import org.hipparchus.linear.Array2DRowRealMatrix;
import org.hipparchus.linear.MatrixUtils;
import org.hipparchus.linear.RealMatrix;
......@@ -411,16 +410,13 @@ public class StateCovariance implements TimeStamped {
final LOFType lofIn, final LOFType lofOut,
final RealMatrix inputCartesianCov) {
// Compute rotation matrix from lofIn to lofOut
final Rotation rotationFromLofInToLofOut =
LOFType.rotationFromLOFInToLOFOut(lofIn, lofOut, orbit.getPVCoordinates());
// Builds the matrix to perform covariance transformation
final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromLofInToLofOut);
final RealMatrix jacobianFromLofInToLofOut =
getJacobian(LOFType.transformFromLOFInToLOFOut(lofIn, lofOut, date, orbit.getPVCoordinates()));
// Get the Cartesian covariance matrix converted to frameOut
final RealMatrix cartesianCovarianceOut =
transformationMatrix.multiply(inputCartesianCov.multiplyTransposed(transformationMatrix));
jacobianFromLofInToLofOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToLofOut));
// Output converted covariance
return new StateCovariance(cartesianCovarianceOut, date, lofOut);
......@@ -468,15 +464,13 @@ public class StateCovariance implements TimeStamped {
OrbitType.CARTESIAN, PositionAngle.MEAN,
inputCov).getMatrix();
// Compute rotation matrix from frameIn to lofOut
final Rotation rotationFromFrameInToLofOut = lofOut.rotationFromInertial(orbit.getPVCoordinates(frameIn));
// Builds the matrix to perform covariance transformation
final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromFrameInToLofOut);
final RealMatrix jacobianFromFrameInToLofOut =
getJacobian(lofOut.transformFromInertial(date, orbit.getPVCoordinates(frameIn)));
// Get the Cartesian covariance matrix converted to frameOut
final RealMatrix cartesianCovarianceOut =
transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix));
jacobianFromFrameInToLofOut.multiply(cartesianCovarianceIn.multiplyTransposed(jacobianFromFrameInToLofOut));
// Return converted covariance matrix expressed in cartesian elements
return new StateCovariance(cartesianCovarianceOut, date, lofOut);
......@@ -526,17 +520,13 @@ public class StateCovariance implements TimeStamped {
// Output frame is pseudo-inertial
if (frameOut.isPseudoInertial()) {
// Compute rotation matrix from lofIn to frameOut
final Rotation rotationFromLofInToFrameOut =
lofIn.rotationFromInertial(orbit.getPVCoordinates(frameOut)).revert();
// Builds the matrix to perform covariance transformation
final RealMatrix transformationMatrix =
buildTransformationMatrixFromRotation(rotationFromLofInToFrameOut);
final RealMatrix jacobianFromLofInToFrameOut =
getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates(frameOut)).getInverse());
// Transform covariance
final RealMatrix transformedCovariance =
transformationMatrix.multiply(inputCartesianCov.multiplyTransposed(transformationMatrix));
jacobianFromLofInToFrameOut.multiply(inputCartesianCov.multiplyTransposed(jacobianFromLofInToFrameOut));
// Get the Cartesian covariance matrix converted to frameOut
return new StateCovariance(transformedCovariance, date, frameOut, OrbitType.CARTESIAN,
......@@ -547,17 +537,13 @@ public class StateCovariance implements TimeStamped {
// Output frame is not pseudo-inertial
else {
// Compute rotation matrix from lofIn to orbit inertial frame
final Rotation rotationFromLofInToOrbitFrame =
lofIn.rotationFromInertial(orbit.getPVCoordinates()).revert();
// Builds the matrix to perform covariance transformation
final RealMatrix transformationMatrixFromLofInToOrbitFrame =
buildTransformationMatrixFromRotation(rotationFromLofInToOrbitFrame);
final RealMatrix jacobianFromLofInToOrbitFrame =
getJacobian(lofIn.transformFromInertial(date, orbit.getPVCoordinates()).getInverse());
// Get the Cartesian covariance matrix converted to orbit inertial frame
final RealMatrix cartesianCovarianceInOrbitFrame = transformationMatrixFromLofInToOrbitFrame.multiply(
inputCartesianCov.multiplyTransposed(transformationMatrixFromLofInToOrbitFrame));
final RealMatrix cartesianCovarianceInOrbitFrame = jacobianFromLofInToOrbitFrame.multiply(
inputCartesianCov.multiplyTransposed(jacobianFromLofInToOrbitFrame));
// Get the Cartesian covariance matrix converted to frameOut
return changeFrameAndCreate(orbit, date, orbit.getFrame(), frameOut, cartesianCovarianceInOrbitFrame,
......@@ -602,12 +588,8 @@ public class StateCovariance implements TimeStamped {
// 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);
final RealMatrix j = getJacobian(inToOut);
// Input frame pseudo-inertial
if (frameIn.isPseudoInertial()) {
......@@ -656,22 +638,16 @@ public class StateCovariance implements TimeStamped {
}
/**
* Builds the matrix to perform covariance transformation.
* @param rotation input rotation
* @return the matrix to perform the covariance transformation
* Builds the matrix to perform covariance frame transformation.
* @param transform input transformation
* @return the matrix to perform the covariance frame transformation
*/
private static RealMatrix buildTransformationMatrixFromRotation(final Rotation rotation) {
// Rotation
final double[][] rotationMatrixData = rotation.getMatrix();
// Fills in the upper left and lower right blocks with the rotation
final RealMatrix transformationMatrix = MatrixUtils.createRealMatrix(STATE_DIMENSION, STATE_DIMENSION);
transformationMatrix.setSubMatrix(rotationMatrixData, 0, 0);
transformationMatrix.setSubMatrix(rotationMatrixData, 3, 3);
private static RealMatrix getJacobian(final Transform transform) {
// Get the Jacobian of the transform
final double[][] jacobian = new double[STATE_DIMENSION][STATE_DIMENSION];
transform.getJacobian(CartesianDerivativesFilter.USE_PV, jacobian);
// Return
return transformationMatrix;
return new Array2DRowRealMatrix(jacobian, false);
}
......
src/test/java/org/orekit/propagation/StateCovarianceTest.java
View file @ 129b2d53
......@@ -134,10 +134,6 @@ public class StateCovarianceTest {
}
}
/**
* In that particular case, the RTN local orbital frame is perfectly aligned with the inertial frame. Hence, the
* frame conversion should return the same covariance matrix as input.
*/
@Test
@DisplayName("Test conversion from inertial frame to RTN local orbital frame")
public void should_return_same_covariance_matrix() {
......@@ -168,15 +164,15 @@ public class StateCovarianceTest {
// Then
final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] {
{ 1, 0, 0, 0, 1e-5, 1e-4 },
{ 0, 1, 0, 0, 0, 1e-5 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1e-3, 0, 0 },
{ 1e-5, 0, 0, 0, 1e-3, 0 },
{ 1e-4, 1e-5, 0, 0, 0, 1e-3 }
{ 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, -1.121400e-03, 1.000000e-04 },
{ 0.000000e+00, 1.000000e+00, 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.000000e-05 },
{ 0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
{ 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, 1.131400e-08 },
{-1.121400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001257e-03, -1.131400e-07 },
{ 1.000000e-04, 1.000000e-05, 0.000000e+00, 1.131400e-08, -1.131400e-07, 1.000000e-03 },
});
compareCovariance(covarianceMatrixInRTN, expectedMatrixInRTN, 1e-20);
compareCovariance(covarianceMatrixInRTN, expectedMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
}
......@@ -239,15 +235,15 @@ public class StateCovarianceTest {
// Then
// Expected covariance matrix obtained by rotation initial covariance matrix by 90 degrees
final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] {
{ 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 0, 0, -1e-5 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1e-3, 0, 0 },
{ 0, 0, 0, 0, 1e-3, 0 },
{ 0, -1e-5, 0, 0, 0, 1e-3 }
{ 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, -1.131400e-03, 0.000000e+00 },
{ 0.000000e+00, 1.000000e+00, 0.000000e+00, 1.131400e-03, 0.000000e+00, -1.000000e-05 },
{ 0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
{ 0.000000e+00, 1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, -1.131400e-08 },
{-1.131400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001280e-03, 0.000000e+00 },
{ 0.000000e+00, -1.000000e-05, 0.000000e+00, -1.131400e-08, 0.000000e+00, 1.000000e-03 },
});
compareCovariance(expectedMatrixInRTN, convertedCovarianceMatrixInRTN, 1e-20);
compareCovariance(expectedMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
}
@Test
......@@ -283,15 +279,15 @@ public class StateCovarianceTest {
// Expected covariance matrix obtained by rotation initial covariance matrix by -90 degrees
final RealMatrix expectedMatrixInInertialFrame = new BlockRealMatrix(new double[][] {
{ 1, 0, 0, 0, 0, 1e-5 },
{ 0, 1, 0, 0, 0, 0 },
{ 0, 0, 1, 0, 0, 0 },
{ 0, 0, 0, 1e-3, 0, 0 },
{ 0, 0, 0, 0, 1e-3, 0 },
{ 1e-5, 0, 0, 0, 0, 1e-3 }
{1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.131400e-03, 1.000000e-05 },
{0.000000e+00, 1.000000e+00, 0.000000e+00, -1.131400e-03, 0.000000e+00, 0.000000e+00 },
{0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00, 0.000000e+00, 0.000000e+00 },
{0.000000e+00, -1.131400e-03, 0.000000e+00, 1.001280e-03, 0.000000e+00, 0.000000e+00 },
{1.131400e-03, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.001280e-03, 1.131400e-08 },
{1.000000e-05, 0.000000e+00, 0.000000e+00, 0.000000e+00, 1.131400e-08, 1.000000e-03 },
});
compareCovariance(expectedMatrixInInertialFrame, convertedCovarianceMatrixInInertialFrame, 1e-20);
compareCovariance(expectedMatrixInInertialFrame, convertedCovarianceMatrixInInertialFrame, DEFAULT_VALLADO_THRESHOLD);
}
......@@ -302,6 +298,9 @@ public class StateCovarianceTest {
* More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the
* cartesian covariance in RSW from p.19.
* <p>
* Orekit applies the full frame transformation while Vallado's paper only take into account the rotation part.
* Therefore, some values are different with respect to the reference ones in the paper.
* <p>
* Note that the followings local orbital frame are equivalent RSW=RTN=QSW.
*/
@Test
......@@ -326,12 +325,12 @@ public class StateCovarianceTest {
// Then
final RealMatrix expectedCovarianceMatrixInRTN = new BlockRealMatrix(new double[][] {
{ 9.918921e-001, 6.700644e-003, -2.878187e-003, 1.892086e-005, 6.700644e-005, -2.878187e-005 },
{ 6.700644e-003, 1.013730e+000, -1.019283e-002, 6.700644e-005, 2.372970e-004, -1.019283e-004 },
{ -2.878187e-003, -1.019283e-002, 9.943782e-001, -2.878187e-005, -1.019283e-004, 4.378217e-005 },
{ 1.892086e-005, 6.700644e-005, -2.878187e-005, 1.892086e-007, 6.700644e-007, -2.878187e-007 },
{ 6.700644e-005, 2.372970e-004, -1.019283e-004, 6.700644e-007, 2.372970e-006, -1.019283e-006 },
{ -2.878187e-005, -1.019283e-004, 4.378217e-005, -2.878187e-007, -1.019283e-006, 4.378217e-007 }
{ 9.918921e-01, 6.700644e-03, -2.878187e-03, 2.637186e-05, -1.035961e-03, -2.878187e-05 },
{ 6.700644e-03, 1.013730e+00, -1.019283e-02, 1.194257e-03, 2.298460e-04, -1.019283e-04 },
{-2.878187e-03, -1.019283e-02, 9.943782e-01, -4.011613e-05, -9.872780e-05, 4.378217e-05 },
{ 2.637186e-05, 1.194257e-03, -4.011613e-05, 1.591713e-06, 9.046096e-07, -4.011613e-07 },
{-1.035961e-03, 2.298460e-04, -9.872780e-05, 9.046096e-07, 3.450431e-06, -9.872780e-07 },
{-2.878187e-05, -1.019283e-04, 4.378217e-05, -4.011613e-07, -9.872780e-07, 4.378217e-07 }
});
compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD);
......@@ -369,6 +368,10 @@ public class StateCovarianceTest {
* <p>
* More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the
* cartesian covariance in NTW from p.19.
* <p>
* Orekit applies the full frame transformation while Vallado's paper only take into account the rotation part.
* Therefore, some values are different with respect to the reference ones in the paper.
* <p>
*/
@Test
@DisplayName("Test covariance conversion Vallado test case : ECI cartesian to NTW")
......@@ -393,12 +396,12 @@ public class StateCovarianceTest {
// Then
final RealMatrix expectedCovarianceMatrixInNTW = new BlockRealMatrix(new double[][] {
{ 9.918792e-001, 6.679546e-003, -2.868345e-003, 1.879167e-005, 6.679546e-005, -2.868345e-005 },
{ 6.679546e-003, 1.013743e+000, -1.019560e-002, 6.679546e-005, 2.374262e-004, -1.019560e-004 },
{ -2.868345e-003, -1.019560e-002, 9.943782e-001, -2.868345e-005, -1.019560e-004, 4.378217e-005 },
{ 1.879167e-005, 6.679546e-005, -2.868345e-005, 1.879167e-007, 6.679546e-007, -2.868345e-007 },
{ 6.679546e-005, 2.374262e-004, -1.019560e-004, 6.679546e-007, 2.374262e-006, -1.019560e-006 },
{ -2.868345e-005, -1.019560e-004, 4.378217e-005, -2.868345e-007, -1.019560e-006, 4.378217e-007 }
{ 9.918792e-01, 6.679546e-03, -2.868345e-03, 2.621921e-05, -1.036158e-03, -2.868345e-05 },
{ 6.679546e-03, 1.013743e+00, -1.019560e-02, 1.194061e-03, 2.299986e-04, -1.019560e-04 },
{-2.868345e-03, -1.019560e-02, 9.943782e-01, -4.002079e-05, -9.876648e-05, 4.378217e-05 },
{ 2.621921e-05, 1.194061e-03, -4.002079e-05, 1.589968e-06, 9.028133e-07, -4.002079e-07 },
{-1.036158e-03, 2.299986e-04, -9.876648e-05, 9.028133e-07, 3.452177e-06, -9.876648e-07 },
{-2.868345e-05, -1.019560e-04, 4.378217e-05, -4.002079e-07, -9.876648e-07, 4.378217e-07 },
});
compareCovariance(expectedCovarianceMatrixInNTW, convertedCovarianceMatrixInNTW, DEFAULT_VALLADO_THRESHOLD);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment