From f9c723456828871212f2e326e84ccd31df4f1f39 Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Mon, 26 Sep 2022 17:44:30 +0200 Subject: [PATCH 01/10] WIP Implementing covariance conversion from/to LOFType --- .../StateCovarianceMatrixProvider.java | 234 ++++++++++++++---- 1 file changed, 185 insertions(+), 49 deletions(-) diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index c342d7630..e115f4a09 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -16,10 +16,14 @@ */ 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; +import org.orekit.errors.OrekitException; +import org.orekit.errors.OrekitMessages; import org.orekit.frames.Frame; +import org.orekit.frames.LOFType; import org.orekit.frames.Transform; import org.orekit.orbits.CartesianOrbit; import org.orekit.orbits.Orbit; @@ -230,25 +234,177 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } - /** 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 - * @see #getStateCovariance(SpacecraftState) - * @see #getStateCovariance(SpacecraftState, Frame) + /** + * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. + *

+ * The transformation is based on Equation (20) 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 lofOut the target local orbital frame + * @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 RealMatrix getStateCovariance(final SpacecraftState state, final OrbitType orbitType, final PositionAngle angleType) { + public static RealMatrix changeCovarianceFrame(final Orbit orbit, + final Frame frameIn, final LOFType lofOut, + final RealMatrix inputCov, + final OrbitType covOrbitType, final PositionAngle covAngleType) { - // Get the current propagated covariance - final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); + // Input frame is inertial + if (frameIn.isPseudoInertial()) { + + // Convert input matrix to Cartesian parameters in input frame + final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, + OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); + + // 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); + + // Get the Cartesian covariance matrix converted to frameOut + final RealMatrix cartesianCovarianceOut = + transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); + + // Convert orbit frame to output frame + final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameIn), frameIn, orbit.getMu()); + + // Convert output Cartesian matrix to initial orbit type and position angle + return changeCovarianceType(outOrbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); + + } + + // Output frame is not inertial + else { + throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameIn.getName()); + } + } + + /** + * 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 changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType, orbitType, angleType, covariance); + return outputCov; } + private static RealMatrix buildTransformationMatrixFromRotation(final Rotation rotation) { + + final double[][] rotationMatrixData = rotation.getMatrix(); + + final RealMatrix transformationMatrix = MatrixUtils.createRealMatrix(6, 6); + + // Fills in the upper left and lower right blocks with the rotation + transformationMatrix.setSubMatrix(rotationMatrixData, 0, 0); + transformationMatrix.setSubMatrix(rotationMatrixData, 3, 3); + + return transformationMatrix; + } + + /** + * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. + *

+ * The transformation is based on Equation (20) 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 lofIn the local orbital 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 LOFType lofIn, final Frame frameOut, + final RealMatrix inputCov, + final OrbitType covOrbitType, final PositionAngle covAngleType) { + + // Input frame is inertial + if (frameOut.isPseudoInertial()) { + + // Convert input matrix to Cartesian parameters in input frame + final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, + OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); + + // 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); + + // Get the Cartesian covariance matrix converted to frameOut + final RealMatrix cartesianCovarianceOut = + transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); + + // 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); + + } + + // Output frame is not inertial + else { + throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameOut.getName()); + } + } + /** Get the covariance matrix in another frame. *

* The transformation is based on Equation (18) of @@ -295,50 +451,30 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { // Convert output Cartesian matrix to initial orbit type and position angle return changeCovarianceType(outOrbit, OrbitType.CARTESIAN, PositionAngle.MEAN, - covOrbitType, covAngleType, cartesianCovarianceOut); + 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 + /** + * 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 + * + * @see #getStateCovariance(SpacecraftState) + * @see #getStateCovariance(SpacecraftState, Frame) */ - 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); + public RealMatrix getStateCovariance(final SpacecraftState state, final OrbitType orbitType, + final PositionAngle angleType) { - // 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)); + // Get the current propagated covariance + final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); // Return the converted covariance - return outputCov; + return changeCovarianceType(state.getOrbit(), covOrbitType, covAngleType, orbitType, angleType, covariance); } -- GitLab From 3045442022a8568cc4c6b970e7eb61f14a67373c Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Tue, 27 Sep 2022 17:34:55 +0200 Subject: [PATCH 02/10] WIP Implementing covariance conversion from/to LOFType Added tests based on Vallado's paper :"Covariance Transformations for Satellite Flight Dynamics Operations" to validate covariance conversion from/to (commonly used) local orbital frame). Added a static method in LOFType to directly rotate from one LOFType to another (to debug). Reformatted LOFType. --- src/main/java/org/orekit/frames/LOFType.java | 155 +++-- .../StateCovarianceMatrixProvider.java | 168 ++++- .../StateCovarianceMatrixProviderTest.java | 573 +++++++++++++++--- 3 files changed, 736 insertions(+), 160 deletions(-) diff --git a/src/main/java/org/orekit/frames/LOFType.java b/src/main/java/org/orekit/frames/LOFType.java index 54e4f8c6c..127a867d0 100644 --- a/src/main/java/org/orekit/frames/LOFType.java +++ b/src/main/java/org/orekit/frames/LOFType.java @@ -16,11 +16,12 @@ */ package org.orekit.frames; -import org.hipparchus.Field; import org.hipparchus.CalculusFieldElement; +import org.hipparchus.Field; import org.hipparchus.geometry.euclidean.threed.FieldRotation; import org.hipparchus.geometry.euclidean.threed.FieldVector3D; import org.hipparchus.geometry.euclidean.threed.Rotation; +import org.hipparchus.geometry.euclidean.threed.RotationConvention; import org.hipparchus.geometry.euclidean.threed.Vector3D; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; @@ -47,16 +48,19 @@ public enum LOFType { * @see #NTW */ TNW { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, TNW, pv); + } + /** {@inheritDoc} */ public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getVelocity(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.PLUS_I), new FieldVector3D<>(field, Vector3D.PLUS_K)); @@ -81,16 +85,48 @@ public enum LOFType { * @see #VVLH */ QSW { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + + final Vector3D position = pv.getPosition(); + final Vector3D velocity = pv.getVelocity(); + final Vector3D momentum = Vector3D.crossProduct(position, velocity); + + switch (fromLOF) { + case EQW: + final Vector3D ascendingNode = new Vector3D(-momentum.getY(), momentum.getX(), 0); + return new Rotation(ascendingNode, Vector3D.crossProduct(Vector3D.PLUS_K, ascendingNode), + position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + case NTW: + return new Rotation(Vector3D.crossProduct(velocity, Vector3D.PLUS_K), velocity, + position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + case VNC: + return new Rotation(velocity, Vector3D.PLUS_J, + position, Vector3D.PLUS_K); + case LVLH: + return Rotation.IDENTITY; + case VVLH: + return new Rotation(Vector3D.MINUS_I, Vector3D.MINUS_K, + Vector3D.PLUS_I, Vector3D.PLUS_K); + case LVLH_CCSDS: + return new Rotation(Vector3D.PLUS_K, Vector3D.MINUS_J, + Vector3D.PLUS_I, Vector3D.PLUS_K); + case TNW: + return new Rotation(velocity, Vector3D.crossProduct(Vector3D.PLUS_K, velocity), + position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + default: + return Rotation.IDENTITY; + } + } + /** {@inheritDoc} */ public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.PLUS_I), new FieldVector3D<>(field, Vector3D.PLUS_K)); @@ -121,16 +157,19 @@ public enum LOFType { * @see #VVLH */ LVLH { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, LVLH, pv); + } + /** {@inheritDoc} */ public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.PLUS_I), new FieldVector3D<>(field, Vector3D.PLUS_K)); @@ -159,16 +198,19 @@ public enum LOFType { * @since 11.0 */ LVLH_CCSDS { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.MINUS_K, Vector3D.MINUS_J); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, LVLH_CCSDS, pv); + } + /** {@inheritDoc} */ public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.MINUS_K), new FieldVector3D<>(field, Vector3D.MINUS_J)); @@ -196,15 +238,18 @@ public enum LOFType { * @see #LVLH_CCSDS */ VVLH { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return LVLH_CCSDS.rotationFromInertial(pv); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, VVLH, pv); + } + /** {@inheritDoc} */ public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return LVLH_CCSDS.rotationFromInertial(field, pv); } @@ -225,16 +270,19 @@ public enum LOFType { * @see #NTW */ VNC { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_J); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, VNC, pv); + } + @Override public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getVelocity(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.PLUS_I), new FieldVector3D<>(field, Vector3D.PLUS_J)); @@ -247,7 +295,6 @@ public enum LOFType { * @since 11.0 */ EQW { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { final Vector3D m = pv.getMomentum(); @@ -255,9 +302,13 @@ public enum LOFType { Vector3D.PLUS_I, Vector3D.PLUS_J); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, EQW, pv); + } + @Override public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { final FieldVector3D m = pv.getMomentum(); return new FieldRotation<>(new FieldVector3D<>(m.getY().negate(), m.getX(), field.getZero()), m, @@ -282,16 +333,19 @@ public enum LOFType { * @since 11.0 */ NTW { - /** {@inheritDoc} */ public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_J, Vector3D.PLUS_K); } + @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + return rotationFromLOFInToLOFOut(fromLOF, NTW, pv); + } + @Override public > FieldRotation rotationFromInertial(final Field field, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getVelocity(), pv.getMomentum(), new FieldVector3D<>(field, Vector3D.PLUS_J), new FieldVector3D<>(field, Vector3D.PLUS_K)); @@ -299,9 +353,28 @@ public enum LOFType { }; - /** Get the transform from an inertial frame defining position-velocity and the local orbital frame. + /** + * Get the rotation from (common) input to output local orbital frame. + * + * @param in input (common) local orbital frame + * @param out output (common) local orbital frame + * @param pv position-velocity of the spacecraft in some inertial frame + * @return rotation from (common) input to output local orbital frame. + */ + public static Rotation rotationFromLOFInToLOFOut(final LOFType in, final LOFType out, final PVCoordinates pv) { + + final Rotation inToQSW = LOFType.QSW.rotationFromLOFType(in, pv); + + final Rotation QSWToOut = LOFType.QSW.rotationFromLOFType(out, pv).revert(); + + return QSWToOut.compose(inToQSW, RotationConvention.VECTOR_OPERATOR); + } + + /** + * Get the transform from an inertial frame defining position-velocity and the local orbital frame. + * * @param date current date - * @param pv position-velocity of the spacecraft in some inertial frame + * @param pv position-velocity of the spacecraft in some inertial frame * @return transform from the frame where position-velocity are defined to local orbital frame */ public Transform transformFromInertial(final AbsoluteDate date, final PVCoordinates pv) { @@ -310,8 +383,8 @@ public enum LOFType { final Transform translation = new Transform(date, pv.negate()); // compute the rotation part of the transform - final Rotation r = rotationFromInertial(pv); - final Vector3D p = pv.getPosition(); + final Rotation r = rotationFromInertial(pv); + final Vector3D p = pv.getPosition(); final Vector3D momentum = pv.getMomentum(); final Transform rotation = new Transform(date, r, new Vector3D(1.0 / p.getNormSq(), r.applyTo(momentum))); @@ -320,22 +393,37 @@ public enum LOFType { } - /** Get the transform from an inertial frame defining position-velocity and the local orbital frame. - * @param date current date + /** + * Get the rotation from inertial frame to local orbital frame. + *

+ * This rotation does not include any time derivatives. If first time derivatives (i.e. rotation rate) is needed as + * well, the full {@link #transformFromInertial(AbsoluteDate, PVCoordinates) transformFromInertial} method must be + * called and the complete rotation transform must be extracted from it. + *

+ * * @param pv position-velocity of the spacecraft in some inertial frame - * @param type of the fiels elements + * @return rotation from inertial frame to local orbital frame + */ + public abstract Rotation rotationFromInertial(PVCoordinates pv); + + /** + * Get the transform from an inertial frame defining position-velocity and the local orbital frame. + * + * @param date current date + * @param pv position-velocity of the spacecraft in some inertial frame + * @param type of the fields elements * @return transform from the frame where position-velocity are defined to local orbital frame * @since 9.0 */ public > FieldTransform transformFromInertial(final FieldAbsoluteDate date, - final FieldPVCoordinates pv) { + final FieldPVCoordinates pv) { // compute the translation part of the transform final FieldTransform translation = new FieldTransform<>(date, pv.negate()); // compute the rotation part of the transform - final FieldRotation r = rotationFromInertial(date.getField(), pv); - final FieldVector3D p = pv.getPosition(); + final FieldRotation r = rotationFromInertial(date.getField(), pv); + final FieldVector3D p = pv.getPosition(); final FieldVector3D momentum = pv.getMomentum(); final FieldTransform rotation = new FieldTransform(date, r, new FieldVector3D<>(p.getNormSq().reciprocal(), r.applyTo(momentum))); @@ -344,18 +432,7 @@ public enum LOFType { } - /** Get the rotation from inertial frame to local orbital frame. - *

- * This rotation does not include any time derivatives. If first - * time derivatives (i.e. rotation rate) is needed as well, the full - * {@link #transformFromInertial(AbsoluteDate, PVCoordinates) transformFromInertial} - * method must be called and the complete rotation transform must be extracted - * from it. - *

- * @param pv position-velocity of the spacecraft in some inertial frame - * @return rotation from inertial frame to local orbital frame - */ - public abstract Rotation rotationFromInertial(PVCoordinates pv); + public abstract Rotation rotationFromLOFType(LOFType fromLOF, PVCoordinates pv); /** Get the rotation from inertial frame to local orbital frame. *

diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index e115f4a09..9a625d5c4 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -25,13 +25,14 @@ import org.orekit.errors.OrekitMessages; import org.orekit.frames.Frame; import org.orekit.frames.LOFType; 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; +import java.util.Locale; + /** * Additional state provider for state covariance matrix. *

@@ -235,7 +236,8 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } /** - * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. + * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to another + * {@link LOFType commonly used local orbital frame} *

* The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics * Operations" by David A. Vallado". @@ -244,52 +246,99 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { * 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 lofOut the target local orbital frame + * @param lofIn the local orbital frame in which the input covariance matrix is expressed. + * @param lofOut the target local orbital frame. + * @param pivotFrame the pivot 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 LOFType lofOut, - final RealMatrix inputCov, - final OrbitType covOrbitType, final PositionAngle covAngleType) { + final LOFType lofIn, final LOFType lofOut, final Frame pivotFrame, + final RealMatrix inputCov, + final OrbitType covOrbitType, final PositionAngle covAngleType) { - // Input frame is inertial - if (frameIn.isPseudoInertial()) { + // In case the input and output local orbital frame type are the same + if (lofIn.equals(lofOut)) { + return inputCov; + } + // Pivot frame is inertial + if (pivotFrame.isPseudoInertial()) { // Convert input matrix to Cartesian parameters in input frame final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, - OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); + OrbitType.CARTESIAN, PositionAngle.MEAN, + inputCov); - // Compute rotation matrix from frameIn to lofOut - final Rotation rotationFromFrameInToLofOut = lofOut.rotationFromInertial(orbit.getPVCoordinates(frameIn)); + // Compute rotation matrix from lofIn to frameOut + + /* + final Rotation rotationFromLofInToFrameOut = + lofIn.rotationFromInertial(orbit.getPVCoordinates()).revert(); + + System.out.println("rotationFromLofInToFrameOut.revert"); + printMatrix(new BlockRealMatrix(rotationFromLofInToFrameOut.revert().getMatrix())); + + final Rotation rotationFromPivotFrameToLofOut = + lofIn.rotationFromInertial(orbit.getPVCoordinates()); + + System.out.println("rotationFromPivotFrameToLofOut"); + printMatrix(new BlockRealMatrix(rotationFromPivotFrameToLofOut.getMatrix())); + + final Rotation rotationFromLofInToLofOut = + rotationFromPivotFrameToLofOut.compose(rotationFromLofInToFrameOut, + RotationConvention.VECTOR_OPERATOR); + + */ + + final Rotation rotationFromLofInToLofOut = LOFType.rotationFromLOFInToLOFOut(lofIn, lofOut, + orbit.getPVCoordinates( + )); // Builds the matrix to perform covariance transformation - final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromFrameInToLofOut); + final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromLofInToLofOut); + + System.out.println("transformationMatrix"); + printMatrix(transformationMatrix); // Get the Cartesian covariance matrix converted to frameOut final RealMatrix cartesianCovarianceOut = transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); // Convert orbit frame to output frame - final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameIn), frameIn, orbit.getMu()); + // 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); + return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); } - // Output frame is not inertial + // Pivot frame is not inertial else { - throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameIn.getName()); + throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, pivotFrame.getName()); } } + public static void printMatrix(final RealMatrix covariance) { + + // Create a string builder + final StringBuilder covToPrint = new StringBuilder(); + for (int row = 0; row < covariance.getRowDimension(); row++) { + for (int column = 0; column < covariance.getColumnDimension(); column++) { + covToPrint.append(String.format(Locale.US, "%16.16e", covariance.getEntry(row, column))); + covToPrint.append(" "); + } + covToPrint.append("\n"); + } + + // Print + System.out.println(covToPrint); + + } + /** * Get the covariance matrix in another orbit type. * @@ -349,7 +398,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } /** - * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. + * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to a {@link Frame frame}. *

* The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics * Operations" by David A. Vallado". @@ -364,20 +413,20 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { * @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 LOFType lofIn, final Frame frameOut, - final RealMatrix inputCov, - final OrbitType covOrbitType, final PositionAngle covAngleType) { + final LOFType lofIn, final Frame frameOut, + final RealMatrix inputCov, + final OrbitType covOrbitType, final PositionAngle covAngleType) { // Input frame is inertial if (frameOut.isPseudoInertial()) { // Convert input matrix to Cartesian parameters in input frame final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, - OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); + OrbitType.CARTESIAN, PositionAngle.MEAN, + inputCov); // Compute rotation matrix from lofIn to frameOut final Rotation rotationFromLofInToFrameOut = @@ -391,11 +440,11 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); // Convert orbit frame to output frame - final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameOut), frameOut, orbit.getMu()); + // 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); + return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); } @@ -405,6 +454,62 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } } + /** + * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. + *

+ * The transformation is based on Equation (20) 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 lofOut the target local orbital frame + * @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 LOFType lofOut, + final RealMatrix inputCov, + final OrbitType covOrbitType, final PositionAngle covAngleType) { + + // Input frame is inertial + if (frameIn.isPseudoInertial()) { + + // Convert input matrix to Cartesian parameters in input frame + final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, + OrbitType.CARTESIAN, PositionAngle.MEAN, + inputCov); + + // 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); + + // Get the Cartesian covariance matrix converted to frameOut + final RealMatrix cartesianCovarianceOut = + transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); + + // Convert orbit frame to output frame + // final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameIn), frameIn, orbit.getMu()); + + // Convert output Cartesian matrix to initial orbit type and position angle + return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); + + } + + // Output frame is not inertial + else { + throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameIn.getName()); + } + } + /** Get the covariance matrix in another frame. *

* The transformation is based on Equation (18) of @@ -447,11 +552,12 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { 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()); + // FIXME The line below can be safely removed without changing the tests results + // 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); + return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); } diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java index 73a37b091..2a74bc8f5 100644 --- a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java +++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java @@ -17,13 +17,14 @@ package org.orekit.propagation; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.linear.BlockRealMatrix; 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.jupiter.api.Assertions; -import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.DisplayName; import org.junit.jupiter.api.Test; import org.orekit.Utils; import org.orekit.forces.ForceModel; @@ -33,6 +34,7 @@ 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.frames.LOFType; import org.orekit.orbits.CartesianOrbit; import org.orekit.orbits.Orbit; import org.orekit.orbits.OrbitType; @@ -45,30 +47,14 @@ import org.orekit.utils.Constants; import org.orekit.utils.IERSConventions; import org.orekit.utils.PVCoordinates; +import java.util.Locale; + public class StateCovarianceMatrixProviderTest { private SpacecraftState initialState; private double[][] initCov; - @BeforeEach - 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} - }; - } + final private double DEFAULT_VALLADO_THRESHOLD = 1e-6; /** * Unit test for the covariance frame transformation. @@ -76,6 +62,9 @@ public class StateCovarianceMatrixProviderTest { @Test public void testFrameConversion() { + // Initialization + setUp(); + // Reference final RealMatrix referenceCov = MatrixUtils.createRealMatrix(initCov); @@ -84,22 +73,124 @@ public class StateCovarianceMatrixProviderTest { final Frame frameB = FramesFactory.getTEME(); // First transformation - RealMatrix transformedCov = StateCovarianceMatrixProvider.changeCovarianceFrame(initialState.getOrbit(), frameA, frameB, referenceCov, OrbitType.CARTESIAN, PositionAngle.MEAN); + 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); + transformedCov = StateCovarianceMatrixProvider.changeCovarianceFrame(initialState.getOrbit(), frameB, frameA, + transformedCov, OrbitType.CARTESIAN, + PositionAngle.MEAN); // Verify compareCovariance(referenceCov, transformedCov, 5.2e-15); } + 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 } + }; + } + + /** + * 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++) { + if (reference.getEntry(row, column) == 0) { + Assertions.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column), + threshold); + } + else { + Assertions.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column), + FastMath.abs(threshold * reference.getEntry(row, column))); + } + } + } + } + + @Test + @DisplayName("Test conversion from inertial frame to RTN local orbital frame") + void should_return_same_covariance_matrix() { + + // Given + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; + + final PVCoordinates initialPV = new PVCoordinates( + new Vector3D(6778000, 0, 0), + new Vector3D(0, 7668.63, 0)); + + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + + final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] { + { 1, 0, 0, 0, 0, 0 }, + { 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 }, + { 0, 0, 0, 0, 0, 1e-3 } + }); + + // When + final RealMatrix covarianceMatrixInRTN = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, + LOFType.QSW, + initialCovarianceInInertialFrame, + OrbitType.CARTESIAN, + PositionAngle.MEAN); + + // Then + final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] { + { 1, 0, 0, 0, 0, 0 }, + { 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 }, + { 0, 0, 0, 0, 0, 1e-3 } + }); + + compareCovariance(covarianceMatrixInRTN, expectedMatrixInRTN, 1e-20); + + } + /** * Unit test for the covariance type transformation. */ @Test public void testTypeConversion() { + // Initialization + setUp(); + // Reference final RealMatrix referenceCov = MatrixUtils.createRealMatrix(initCov); @@ -108,12 +199,14 @@ public class StateCovarianceMatrixProviderTest { final OrbitType kep = OrbitType.KEPLERIAN; // First transformation - RealMatrix transformedCov = StateCovarianceMatrixProvider.changeCovarianceType(initialState.getOrbit(), cart, PositionAngle.MEAN, - kep, PositionAngle.MEAN, referenceCov); + 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); + transformedCov = + StateCovarianceMatrixProvider.changeCovarianceType(initialState.getOrbit(), kep, PositionAngle.MEAN, + cart, PositionAngle.MEAN, transformedCov); // Verify compareCovariance(referenceCov, transformedCov, 3.5e-12); @@ -126,18 +219,22 @@ public class StateCovarianceMatrixProviderTest { @Test public void testWithNumericalPropagatorCartesian() { + // Initialization + setUp(); + // 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 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); + 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); @@ -145,12 +242,13 @@ public class StateCovarianceMatrixProviderTest { 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); + 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); @@ -162,12 +260,18 @@ public class StateCovarianceMatrixProviderTest { // 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} + { 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); @@ -186,7 +290,10 @@ public class StateCovarianceMatrixProviderTest { RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB); // Second transformation - RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN); + RealMatrix transformedCovB = + StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), + frameB, propagatedCov, OrbitType.CARTESIAN, + PositionAngle.MEAN); // Verify compareCovariance(transformedCovA, transformedCovB, 1.0e-15); @@ -196,14 +303,17 @@ public class StateCovarianceMatrixProviderTest { /////////// // Define a new output frame - final OrbitType outOrbitType = OrbitType.KEPLERIAN; + 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); + RealMatrix transformedCovD = + StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, + PositionAngle.MEAN, outOrbitType, outAngleType, + propagatedCov); // Verify compareCovariance(transformedCovC, transformedCovD, 1.0e-15); @@ -211,33 +321,38 @@ public class StateCovarianceMatrixProviderTest { } /** - * Unit test for covariance propagation in Cartesian elements. - * The difference here is that the propagator uses its default orbit type: EQUINOCTIAL + * 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() { + // Initialization + setUp(); + // Integrator final double step = 60.0; final ODEIntegrator integrator = new ClassicalRungeKuttaIntegrator(step); // Numerical propagator - final String stmName = "STM"; + 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); + 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); + 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); @@ -249,12 +364,18 @@ public class StateCovarianceMatrixProviderTest { // 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} + { 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); @@ -273,20 +394,26 @@ public class StateCovarianceMatrixProviderTest { RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB); // Second transformation - RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN); + 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 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); + RealMatrix transformedCovD = + StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, + PositionAngle.MEAN, outOrbitType, outAngleType, + propagatedCov); // Verify compareCovariance(transformedCovC, transformedCovD, 1.0e-15); @@ -299,22 +426,28 @@ public class StateCovarianceMatrixProviderTest { @Test public void testWithAnalyticalPropagator() { + // Initialization + setUp(); + // Numerical propagator - final String stmName = "STM"; - final OrbitType propType = OrbitType.CARTESIAN; + 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)); + 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); + 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 @@ -325,12 +458,18 @@ public class StateCovarianceMatrixProviderTest { // 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} + { 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); @@ -349,38 +488,292 @@ public class StateCovarianceMatrixProviderTest { RealMatrix transformedCovA = provider.getStateCovariance(propagated, frameB); // Second transformation - RealMatrix transformedCovB = StateCovarianceMatrixProvider.changeCovarianceFrame(propagated.getOrbit(), propagated.getFrame(), frameB, propagatedCov, OrbitType.CARTESIAN, PositionAngle.MEAN); + 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 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); + RealMatrix transformedCovD = + StateCovarianceMatrixProvider.changeCovarianceType(propagated.getOrbit(), OrbitType.CARTESIAN, + PositionAngle.MEAN, outOrbitType, outAngleType, + propagatedCov); // Verify compareCovariance(transformedCovC, transformedCovD, 1.0e-15); } + @Test + @DisplayName("Test conversion from inertial frame to RTN local orbital frame") + void should_rotate_covariance_matrix_by_ninety_degrees() { + + // Given + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; + + final PVCoordinates initialPV = new PVCoordinates( + new Vector3D(0, 6778000, 0), + new Vector3D(-7668.63, 0, 0)); + + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + + final RealMatrix initialCovarianceInInertialFrame = 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 } + }); + + // When + final RealMatrix convertedCovarianceMatrixInRTN = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, LOFType.QSW, + initialCovarianceInInertialFrame, + OrbitType.CARTESIAN, + PositionAngle.MEAN); + + // 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 } + }); + + compareCovariance(expectedMatrixInRTN, convertedCovarianceMatrixInRTN, 1e-20); + } + + @Test + @DisplayName("Test conversion from RTN local orbital frame to inertial frame") + void should_rotate_covariance_matrix_by_minus_ninety_degrees() { + + // Given + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; + + final PVCoordinates initialPV = new PVCoordinates( + new Vector3D(0, 6778000, 0), + new Vector3D(-7668.63, 0, 0)); + + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + + final RealMatrix initialCovarianceInRTN = 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 } + }); + + // When + final RealMatrix convertedCovarianceMatrixInInertialFrame = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + LOFType.QSW, inertialFrame, initialCovarianceInRTN, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // Then + + // 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 } + }); + + compareCovariance(expectedMatrixInInertialFrame, convertedCovarianceMatrixInInertialFrame, 1e-20); + + } + /** - * Compare two covariance matrices - * @param reference reference covariance - * @param computed computed covariance - * @param threshold threshold for comparison + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * 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. */ - 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++) { - Assertions.assertEquals(reference.getEntry(row, column), computed.getEntry(row, column), FastMath.abs(threshold * reference.getEntry(row, column))); + @Test + @DisplayName("Test Vallado test case : ECI cartesian to RTN") + void should_Vallado_RSW_covariance_matrix() { + + // Initialize Orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); + + // When + final RealMatrix convertedCovarianceMatrixInRTN = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, LOFType.QSW, initialCovarianceMatrix, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // 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 } + }); + + compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD); + + } + + private AbsoluteDate getValladoInitialDate() { + return new AbsoluteDate(2000, 12, 15, 16, 58, 50.208, TimeScalesFactory.getUTC()); + } + + private PVCoordinates getValladoInitialPV() { + return new PVCoordinates( + new Vector3D(-605792.21660, -5870229.51108, 3493053.19896), + new Vector3D(-1568.25429, -3702.34891, -6479.48395)); + } + + private double getValladoMu() { + return Constants.IERS2010_EARTH_MU; + } + + private RealMatrix getValladoInitialCovarianceMatrix() { + return new BlockRealMatrix(new double[][] { + { 1, 1e-2, 1e-2, 1e-4, 1e-4, 1e-4 }, + { 1e-2, 1, 1e-2, 1e-4, 1e-4, 1e-4 }, + { 1e-2, 1e-2, 1, 1e-4, 1e-4, 1e-4 }, + { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 }, + { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 }, + { 1e-4, 1e-4, 1e-4, 1e-6, 1e-6, 1e-6 } + }); + } + + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * 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. + */ + @Test + @DisplayName("Test Vallado test case : ECI cartesian to NTW") + void should_Vallado_NTW_covariance_matrix() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); + + // When + final RealMatrix convertedCovarianceMatrixInNTW = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, LOFType.NTW, initialCovarianceMatrix, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // 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 } + }); + + compareCovariance(expectedCovarianceMatrixInNTW, convertedCovarianceMatrixInNTW, DEFAULT_VALLADO_THRESHOLD); + + } + + @Test + @DisplayName("Test conversion from Vallado test case NTW to RSW") + void should_convert_Vallado_NTW_to_RSW() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrixInNTW = 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 } + }); + + // When + final RealMatrix convertedCovarianceMatrixInRTN = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, LOFType.NTW, LOFType.QSW, + inertialFrame, initialCovarianceMatrixInNTW, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // 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 } + }); + System.out.println("convertedCovarianceMatrixInRTN"); + printMatrix(convertedCovarianceMatrixInRTN); + compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, 1e-3); + + } + + public static void printMatrix(final RealMatrix covariance) { + + // Create a string builder + final StringBuilder covToPrint = new StringBuilder(); + for (int row = 0; row < covariance.getRowDimension(); row++) { + for (int column = 0; column < covariance.getColumnDimension(); column++) { + covToPrint.append(String.format(Locale.US, "%16.16e", covariance.getEntry(row, column))); + covToPrint.append(" "); } + covToPrint.append("\n"); } + + // Print + System.out.println(covToPrint); + } } -- GitLab From ff1c610da8bbca9dc406286425035c5cff1160fb Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Wed, 28 Sep 2022 18:04:42 +0200 Subject: [PATCH 03/10] Fixed rotationFromLOFType and rotationFromLOFInToLOFOut methods --- src/main/java/org/orekit/frames/LOFType.java | 44 +++++++++++++------- 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/orekit/frames/LOFType.java b/src/main/java/org/orekit/frames/LOFType.java index 127a867d0..bb99b19c8 100644 --- a/src/main/java/org/orekit/frames/LOFType.java +++ b/src/main/java/org/orekit/frames/LOFType.java @@ -23,12 +23,15 @@ import org.hipparchus.geometry.euclidean.threed.FieldVector3D; import org.hipparchus.geometry.euclidean.threed.Rotation; import org.hipparchus.geometry.euclidean.threed.RotationConvention; import org.hipparchus.geometry.euclidean.threed.Vector3D; +import org.hipparchus.util.FastMath; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; import org.orekit.utils.FieldPVCoordinates; import org.orekit.utils.PVCoordinates; -/** Enumerate for different types of Local Orbital Frames. +/** + * Enumerate for different types of Local Orbital Frames. + * * @author Luc Maisonobe */ public enum LOFType { @@ -95,30 +98,43 @@ public enum LOFType { final Vector3D position = pv.getPosition(); final Vector3D velocity = pv.getVelocity(); - final Vector3D momentum = Vector3D.crossProduct(position, velocity); + final Vector3D momentum = pv.getMomentum(); switch (fromLOF) { case EQW: final Vector3D ascendingNode = new Vector3D(-momentum.getY(), momentum.getX(), 0); - return new Rotation(ascendingNode, Vector3D.crossProduct(Vector3D.PLUS_K, ascendingNode), - position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + return new Rotation(ascendingNode, Vector3D.crossProduct(momentum, ascendingNode), + position, Vector3D.crossProduct(momentum, position)); case NTW: - return new Rotation(Vector3D.crossProduct(velocity, Vector3D.PLUS_K), velocity, - position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + final Vector3D transverse = velocity.crossProduct(momentum); + + final double cosAngle = + transverse.dotProduct(position) / (transverse.getNorm() * position.getNorm()); + + final double angle = FastMath.acos(cosAngle); + + final double[][] rotationMatrixData = new double[][] { + { FastMath.cos(angle), FastMath.sin(angle), 0 }, + { -FastMath.sin(angle), FastMath.cos(angle), 0 }, + { 0, 0, 1 }, + }; + + return new Rotation(rotationMatrixData, 1e-15); + case VNC: - return new Rotation(velocity, Vector3D.PLUS_J, - position, Vector3D.PLUS_K); + return new Rotation(velocity, velocity.crossProduct(momentum), + position, momentum); case LVLH: return Rotation.IDENTITY; case VVLH: - return new Rotation(Vector3D.MINUS_I, Vector3D.MINUS_K, - Vector3D.PLUS_I, Vector3D.PLUS_K); + return new Rotation(position.negate(), position.negate().crossProduct(momentum.negate()), + position, momentum); case LVLH_CCSDS: - return new Rotation(Vector3D.PLUS_K, Vector3D.MINUS_J, - Vector3D.PLUS_I, Vector3D.PLUS_K); + return new Rotation(momentum.negate().crossProduct(position), position, + position, momentum); case TNW: - return new Rotation(velocity, Vector3D.crossProduct(Vector3D.PLUS_K, velocity), - position, Vector3D.crossProduct(Vector3D.PLUS_K, position)); + return new Rotation(velocity, momentum.crossProduct(velocity), + position, momentum.crossProduct(position)); default: return Rotation.IDENTITY; } -- GitLab From 603c955a775e85151d6ad9be2e786bcec8c5b79b Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Wed, 28 Sep 2022 18:06:27 +0200 Subject: [PATCH 04/10] Fixed StateCovarianceMatrixProvider. It can now use non-inertial frame of reference in the changeCovarianceFrame(frame to frame) method. Added tests to StateCovarianceMatrixProviderTest to check this fix. --- .../StateCovarianceMatrixProvider.java | 59 +----- .../StateCovarianceMatrixProviderTest.java | 179 +++++++++++++++++- 2 files changed, 179 insertions(+), 59 deletions(-) diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index 9a625d5c4..85e2973c6 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -31,8 +31,6 @@ import org.orekit.orbits.PositionAngle; import org.orekit.time.AbsoluteDate; import org.orekit.utils.CartesianDerivativesFilter; -import java.util.Locale; - /** * Additional state provider for state covariance matrix. *

@@ -260,56 +258,25 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { final RealMatrix inputCov, final OrbitType covOrbitType, final PositionAngle covAngleType) { - // In case the input and output local orbital frame type are the same - if (lofIn.equals(lofOut)) { - return inputCov; - } - // Pivot frame is inertial if (pivotFrame.isPseudoInertial()) { + // Convert input matrix to Cartesian parameters in input frame final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); - // Compute rotation matrix from lofIn to frameOut - - /* - final Rotation rotationFromLofInToFrameOut = - lofIn.rotationFromInertial(orbit.getPVCoordinates()).revert(); - - System.out.println("rotationFromLofInToFrameOut.revert"); - printMatrix(new BlockRealMatrix(rotationFromLofInToFrameOut.revert().getMatrix())); - - final Rotation rotationFromPivotFrameToLofOut = - lofIn.rotationFromInertial(orbit.getPVCoordinates()); - - System.out.println("rotationFromPivotFrameToLofOut"); - printMatrix(new BlockRealMatrix(rotationFromPivotFrameToLofOut.getMatrix())); - - final Rotation rotationFromLofInToLofOut = - rotationFromPivotFrameToLofOut.compose(rotationFromLofInToFrameOut, - RotationConvention.VECTOR_OPERATOR); - - */ - + // Compute rotation matrix from lofIn to lofOut final Rotation rotationFromLofInToLofOut = LOFType.rotationFromLOFInToLOFOut(lofIn, lofOut, - orbit.getPVCoordinates( - )); + orbit.getPVCoordinates()); // Builds the matrix to perform covariance transformation final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromLofInToLofOut); - System.out.println("transformationMatrix"); - printMatrix(transformationMatrix); - // Get the Cartesian covariance matrix converted to frameOut final RealMatrix cartesianCovarianceOut = transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); - // 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(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, covOrbitType, covAngleType, cartesianCovarianceOut); @@ -320,22 +287,6 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { else { throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, pivotFrame.getName()); } - } - - public static void printMatrix(final RealMatrix covariance) { - - // Create a string builder - final StringBuilder covToPrint = new StringBuilder(); - for (int row = 0; row < covariance.getRowDimension(); row++) { - for (int column = 0; column < covariance.getColumnDimension(); column++) { - covToPrint.append(String.format(Locale.US, "%16.16e", covariance.getEntry(row, column))); - covToPrint.append(" "); - } - covToPrint.append("\n"); - } - - // Print - System.out.println(covToPrint); } @@ -395,6 +346,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { transformationMatrix.setSubMatrix(rotationMatrixData, 3, 3); return transformationMatrix; + } /** @@ -452,6 +404,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { else { throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameOut.getName()); } + } /** @@ -508,6 +461,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { else { throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameIn.getName()); } + } /** Get the covariance matrix in another frame. @@ -597,6 +551,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } } return matrix; + } /** Set the covariance data into an array. diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java index 2a74bc8f5..ed2bb63ba 100644 --- a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java +++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java @@ -26,6 +26,7 @@ import org.hipparchus.util.FastMath; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.DisplayName; import org.junit.jupiter.api.Test; +import org.mockito.Mockito; import org.orekit.Utils; import org.orekit.forces.ForceModel; import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel; @@ -53,7 +54,6 @@ public class StateCovarianceMatrixProviderTest { private SpacecraftState initialState; private double[][] initCov; - final private double DEFAULT_VALLADO_THRESHOLD = 1e-6; /** @@ -547,7 +547,6 @@ public class StateCovarianceMatrixProviderTest { PositionAngle.MEAN); // 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 }, @@ -616,7 +615,7 @@ public class StateCovarianceMatrixProviderTest { */ @Test @DisplayName("Test Vallado test case : ECI cartesian to RTN") - void should_Vallado_RSW_covariance_matrix() { + void should_return_Vallado_RSW_covariance_matrix_from_ECI() { // Initialize Orekit Utils.setDataRoot("regular-data"); @@ -683,7 +682,7 @@ public class StateCovarianceMatrixProviderTest { */ @Test @DisplayName("Test Vallado test case : ECI cartesian to NTW") - void should_Vallado_NTW_covariance_matrix() { + void should_return_Vallado_NTW_covariance_matrix_from_ECI() { // Initialize orekit Utils.setDataRoot("regular-data"); @@ -716,6 +715,171 @@ public class StateCovarianceMatrixProviderTest { } + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the + * cartesian covariance in ECEF from p.18. + *

+ *

+ * BEWARE: It has been found that the earth rotation in this Vallado's case is given 1 million times slower than + * the expected value. This has been corrected and the expected covariance matrix is now the covariance matrix + * computed by Orekit given the similarities with Vallado's results. In addition, the small differences potentially + * come from the custom EOP that Vallado uses. Hence, this test can be considered as a non regression + * test. + *

+ */ + @Test + @DisplayName("Test Vallado test case : ECI cartesian to PEF") + void should_return_Vallado_PEF_covariance_matrix_from_ECI() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + // Initialize orbit + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + // Initialize input covariance matrix + final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); + + // Initialize output frame + final Frame outputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); + + // When + final RealMatrix convertedCovarianceMatrixInITRF = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, outputFrame, initialCovarianceMatrix, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // Then + final RealMatrix expectedCovarianceMatrixInITRF = new BlockRealMatrix(new double[][] { + { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, + 3.4548396261054936e-05, 2.6851237046859200e-06, 5.8312677693153940e-05 }, + { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, + 1.4852736004690684e-04, 1.6544247282904867e-04, 1.2884310644320954e-04 }, + { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, + 5.9252211072590390e-05, 1.2841787487219444e-04, 1.0000913090989617e-04 }, + { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, + 3.5631474857130520e-07, 7.6083489184819870e-07, 5.9252213790760030e-07 }, + { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, + 7.6083489184819880e-07, 1.6542289254142709e-06, 1.2841787929229964e-06 }, + { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, + 5.9252213790760020e-07, 1.2841787929229960e-06, 1.0000913098203875e-06 } + }); + + compareCovariance(expectedCovarianceMatrixInITRF, convertedCovarianceMatrixInITRF, 1e-20); + + } + + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the + * cartesian covariance in ECEF from p.18. + */ + @Test + //TODO + @DisplayName("Test Vallado test case : PEF cartesian to ECI") + void should_return_Vallado_ECI_covariance_matrix_from_PEF() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrix = new BlockRealMatrix(new double[][] { + { 9.934002e-001, 7.512598e-003, 5.831364e-003, 3.400170e-005, 7.512591e-005, 5.831364e-005 }, + { 7.512598e-003, 1.006599e+000, 1.288427e-002, 7.512605e-005, 1.659892e-004, 1.288427e-004 }, + { 5.831364e-003, 1.288427e-002, 1.000001e+000, 5.831364e-005, 1.288427e-004, 1.000092e-004 }, + { 3.400170e-005, 7.512605e-005, 5.831364e-005, 3.400170e-007, 7.512598e-007, 5.831364e-007 }, + { 7.512591e-005, 1.659892e-004, 1.288427e-004, 7.512598e-007, 1.659891e-006, 1.288427e-006 }, + { 5.831364e-005, 1.288427e-004, 1.000092e-004, 5.831364e-007, 1.288427e-006, 1.000092e-006 } + }); + + final Frame inputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, false); + + // When + final RealMatrix convertedCovarianceMatrixInECI = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inputFrame, inertialFrame, initialCovarianceMatrix, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // Then + final RealMatrix expectedCovarianceMatrixInECI = getValladoInitialCovarianceMatrix(); + + final RealMatrix differenceMatrix = expectedCovarianceMatrixInECI.subtract(convertedCovarianceMatrixInECI); + System.out.println("convertedCovarianceMatrixInECI"); + printMatrix(convertedCovarianceMatrixInECI); + + System.out.println("differenceMatrix"); + printMatrix(differenceMatrix); + + compareCovariance(expectedCovarianceMatrixInECI, convertedCovarianceMatrixInECI, DEFAULT_VALLADO_THRESHOLD); + + } + + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the + * cartesian covariance in MOD from p.17. + */ + @Test + @DisplayName("Test Vallado test case : ECI cartesian to MOD") + void should_return_Vallado_MOD_covariance_matrix_from_ECI() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame inertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); + + final Frame outputFrame = FramesFactory.getMOD(IERSConventions.IERS_2010); + + // When + final RealMatrix convertedCovarianceMatrixInMOD = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + inertialFrame, outputFrame, initialCovarianceMatrix, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // Then + final RealMatrix expectedCovarianceMatrixInMOD = new BlockRealMatrix(new double[][] { + { 9.999939e-001, 9.999070e-003, 9.997861e-003, 9.993866e-005, 9.999070e-005, 9.997861e-005 }, + { 9.999070e-003, 1.000004e+000, 1.000307e-002, 9.999070e-005, 1.000428e-004, 1.000307e-004 }, + { 9.997861e-003, 1.000307e-002, 1.000002e+000, 9.997861e-005, 1.000307e-004, 1.000186e-004 }, + { 9.993866e-005, 9.999070e-005, 9.997861e-005, 9.993866e-007, 9.999070e-007, 9.997861e-007 }, + { 9.999070e-005, 1.000428e-004, 1.000307e-004, 9.999070e-007, 1.000428e-006, 1.000307e-006 }, + { 9.997861e-005, 1.000307e-004, 1.000186e-004, 9.997861e-007, 1.000307e-006, 1.000186e-006 }, + }); + + final RealMatrix differenceMatrix = expectedCovarianceMatrixInMOD.subtract(convertedCovarianceMatrixInMOD); + System.out.println("convertedCovarianceMatrixInMOD"); + printMatrix(convertedCovarianceMatrixInMOD); + + System.out.println("differenceMatrix"); + printMatrix(differenceMatrix); + + compareCovariance(expectedCovarianceMatrixInMOD, convertedCovarianceMatrixInMOD, DEFAULT_VALLADO_THRESHOLD); + + } + @Test @DisplayName("Test conversion from Vallado test case NTW to RSW") void should_convert_Vallado_NTW_to_RSW() { @@ -723,6 +887,8 @@ public class StateCovarianceMatrixProviderTest { // Initialize orekit Utils.setDataRoot("regular-data"); + final Orbit orbitMock = Mockito.mock(Orbit.class); + // Given final AbsoluteDate initialDate = getValladoInitialDate(); final PVCoordinates initialPV = getValladoInitialPV(); @@ -753,9 +919,8 @@ public class StateCovarianceMatrixProviderTest { { 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 } }); - System.out.println("convertedCovarianceMatrixInRTN"); - printMatrix(convertedCovarianceMatrixInRTN); - compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, 1e-3); + + compareCovariance(expectedCovarianceMatrixInRTN, convertedCovarianceMatrixInRTN, DEFAULT_VALLADO_THRESHOLD); } -- GitLab From ba51e09b75a7dab0805fcbf210786d335a38a2cb Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Thu, 29 Sep 2022 16:31:20 +0200 Subject: [PATCH 05/10] Added new error message for wrong orbital parameters type. Updated associated test. --- src/main/java/org/orekit/errors/OrekitMessages.java | 13 ++++++++----- .../org/orekit/localization/OrekitMessages_da.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_de.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_el.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_en.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_es.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_fr.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_gl.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_it.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_no.utf8 | 3 +++ .../org/orekit/localization/OrekitMessages_ro.utf8 | 3 +++ .../java/org/orekit/errors/OrekitMessagesTest.java | 2 +- 12 files changed, 39 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/orekit/errors/OrekitMessages.java b/src/main/java/org/orekit/errors/OrekitMessages.java index 943f252ae..ba1fedc50 100644 --- a/src/main/java/org/orekit/errors/OrekitMessages.java +++ b/src/main/java/org/orekit/errors/OrekitMessages.java @@ -16,6 +16,8 @@ */ package org.orekit.errors; +import org.hipparchus.exception.Localizable; + import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; @@ -27,8 +29,6 @@ import java.util.MissingResourceException; import java.util.PropertyResourceBundle; import java.util.ResourceBundle; -import org.hipparchus.exception.Localizable; - /** * Enumeration for localized messages formats. *

@@ -330,11 +330,14 @@ public enum OrekitMessages implements Localizable { INCOMPATIBLE_UNITS("units {0} and {1} are not compatible"), MISSING_VELOCITY("missing velocity data"), ATTEMPT_TO_GENERATE_MALFORMED_FILE("attempt to generate file {0} with a formatting error"), - FIND_ROOT("{0} failed to find root between {1} (g={2,number,0.0##############E0}) and {3} (g={4,number,0.0##############E0})\nLast iteration at {5} (g={6,number,0.0##############E0})"), + FIND_ROOT( + "{0} failed to find root between {1} (g={2,number,0.0##############E0}) and {3} (g={4,number,0.0##############E0})\nLast iteration at {5} (g={6,number,0.0##############E0})"), BACKWARD_PROPAGATION_NOT_ALLOWED("backward propagation not allowed here"), - NO_STATION_ECCENTRICITY_FOR_EPOCH("no station eccentricity values for the given epoch {0}, validity interval is between {1} and {2}"), + NO_STATION_ECCENTRICITY_FOR_EPOCH( + "no station eccentricity values for the given epoch {0}, validity interval is between {1} and {2}"), INCONSISTENT_SELECTION("inconsistent parameters selection between pairs {0}/{1} and {2}/{3}"), - NOT_STRICTLY_POSITIVE("value is not strictly positive: {0}"); + NOT_STRICTLY_POSITIVE("value is not strictly positive: {0}"), + WRONG_ORBIT_PARAMETERS_TYPE("orbital parameters type: {0} is different from expected orbital type : {1}"); // CHECKSTYLE: resume JavadocVariable check diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_da.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_da.utf8 index 0a29a76cf..e793e692f 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_da.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_da.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_de.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_de.utf8 index 319a268f1..820563c16 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_de.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_de.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_el.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_el.utf8 index 6781b77df..c9955093f 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_el.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_el.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_en.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_en.utf8 index 4cebfd8ac..61d2a8bd3 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_en.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_en.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = inconsistent parameters selection between pairs {0}/{1} # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = value is not strictly positive: {0} + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = orbital parameters type: {0} is different from expected orbital type : {1} diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_es.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_es.utf8 index 1933515dd..30ccf0616 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_es.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_es.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = selección de parámetros inconsistente entre los pares # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_fr.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_fr.utf8 index 0cca99688..0fe8bd514 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_fr.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_fr.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = sélection de paramètres incohérents entre les paires # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = la valeur {0} n''est pas strictement positive + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = Le type de paramètres orbitaux: {0} est différent du type de paramètres orbitaux attendus: {1} diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_gl.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_gl.utf8 index 85a59898d..68c36841e 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_gl.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_gl.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_it.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_it.utf8 index 4ebae1d16..584ba6d11 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_it.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_it.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = scelta di parametri discordante per le coppie {0}/{1} e # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_no.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_no.utf8 index a8ec9b3b4..66e7bb617 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_no.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_no.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/main/resources/assets/org/orekit/localization/OrekitMessages_ro.utf8 b/src/main/resources/assets/org/orekit/localization/OrekitMessages_ro.utf8 index 2cff1356f..8bd8a782f 100644 --- a/src/main/resources/assets/org/orekit/localization/OrekitMessages_ro.utf8 +++ b/src/main/resources/assets/org/orekit/localization/OrekitMessages_ro.utf8 @@ -758,3 +758,6 @@ INCONSISTENT_SELECTION = # value is not strictly positive: {0} NOT_STRICTLY_POSITIVE = + +# orbital parameters type: {0} is different from expected orbital type : {1} +WRONG_ORBIT_PARAMETERS_TYPE = diff --git a/src/test/java/org/orekit/errors/OrekitMessagesTest.java b/src/test/java/org/orekit/errors/OrekitMessagesTest.java index a4b5b2b34..4e98dd352 100644 --- a/src/test/java/org/orekit/errors/OrekitMessagesTest.java +++ b/src/test/java/org/orekit/errors/OrekitMessagesTest.java @@ -30,7 +30,7 @@ public class OrekitMessagesTest { @Test public void testMessageNumber() { - Assertions.assertEquals(253, OrekitMessages.values().length); + Assertions.assertEquals(254, OrekitMessages.values().length); } @Test -- GitLab From 433cf29cea600fb2fca01dfa417fa1df72cdf603 Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Thu, 29 Sep 2022 16:57:07 +0200 Subject: [PATCH 06/10] Fixed issue-964 Added methods to convert covariance reference frame : from LOFType to Frame from Frame to LOFType from LOFType to LOFType. Fixed existing method to convert from/to inertial/non-inertial frame. Added tests to verify these methods. TODO : Fix rotation from LOFType to LOFType as the computed rotation is different from the manually computed rotation. See https://forum.orekit.org/t/expected-rotation-different-from-manually-computed-rotation/1987/3 for more information. --- .../StateCovarianceMatrixProvider.java | 574 ++++++++++-------- .../StateCovarianceMatrixProviderTest.java | 338 ++++++++--- 2 files changed, 551 insertions(+), 361 deletions(-) diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index 85e2973c6..7343edfd9 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -34,40 +34,32 @@ 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 + * 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. + * 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. + * 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)} + * 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(Orbit, Frame, Frame, RealMatrix, OrbitType, PositionAngle)} - * 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. + * {@link #changeCovarianceFrame(Orbit, Frame, Frame, RealMatrix, OrbitType, PositionAngle)} 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(Orbit, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)} - * method. + * {@link #changeCovarianceType(Orbit, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)} method. *

+ * * @author Bryan Cazabonne + * @author Vincent Cucchietti * @since 11.3 */ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { @@ -101,16 +93,18 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { /** * 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 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 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}) + * @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, @@ -119,20 +113,51 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { 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; + 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 + /** + * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to another + * {@link LOFType commonly used local orbital frame}. + *

+ * The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics + * Operations" by David A. Vallado". + *

+ * As this method transforms from and to a {@link LOFType commonly used local orbital frame}, it necessarily takes + * in a covariance matrix expressed in cartesian elements and output a covariance matrix also expressed in + * the + * cartesian elements. + * + * @param orbit orbit to which the covariance matrix should correspond + * @param lofIn the local orbital frame in which the input covariance matrix is expressed + * @param lofOut the target local orbital frame + * @param inputCartesianCov input covariance {@code CARTESIAN}) + * @return the covariance matrix expressed in the target commonly used local orbital frame in cartesian elements */ - public OrbitType getCovarianceOrbitType() { - return covOrbitType; + public static RealMatrix changeCovarianceFrame(final Orbit orbit, + 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); + + // Get the Cartesian covariance matrix converted to frameOut + final RealMatrix cartesianCovarianceOut = + transformationMatrix.multiply(inputCartesianCov.multiplyTransposed(transformationMatrix)); + + // Output converted covariance + return cartesianCovarianceOut; + } /** {@inheritDoc} */ @@ -151,14 +176,18 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { 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); + private static RealMatrix buildTransformationMatrixFromRotation(final Rotation rotation) { + + final double[][] rotationMatrixData = rotation.getMatrix(); + + final RealMatrix transformationMatrix = MatrixUtils.createRealMatrix(6, 6); + + // Fills in the upper left and lower right blocks with the rotation + transformationMatrix.setSubMatrix(rotationMatrixData, 0, 0); + transformationMatrix.setSubMatrix(rotationMatrixData, 3, 3); + + return transformationMatrix; + } /** {@inheritDoc} */ @@ -181,111 +210,153 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } - /** Get the state covariance matrix (6x6 dimension). + /** + * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to a {@link Frame frame}. *

- * The output covariance matrix is expressed in the - * same orbit type as {@link #getCovarianceOrbitType()}. + * The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics + * Operations" by David A. Vallado". *

- * It is possible to change the covariance frame by using the - * {@link #changeCovarianceFrame(Orbit, Frame, Frame, RealMatrix, OrbitType, PositionAngle)} - * method. + * The input covariance matrix is expected to be expressed in cartesian elements. *

- * It is also possible to change the covariance orbit type by - * using the {@link #changeCovarianceType(Orbit, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)} - * method. - * @param state spacecraft state to which the covariance matrix should correspond - * @return the state covariance matrix - * @see #getStateCovariance(SpacecraftState, Frame) - * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngle) + * The output covariance matrix will be expressed in cartesian elements. + * + * @param orbit orbit to which the covariance matrix should correspond + * @param lofIn the local orbital frame in which the input covariance matrix is expressed + * @param frameOut the target frame + * @param inputCartesianCov input covariance ({@code CARTESIAN}) + * @return the covariance matrix expressed in the target frame in cartesian elements */ - public RealMatrix getStateCovariance(final SpacecraftState state) { + public static RealMatrix changeCovarianceFrame(final Orbit orbit, + final LOFType lofIn, final Frame frameOut, + final RealMatrix inputCartesianCov) { - // Get the current propagated covariance - final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); + // Initialize output variable + final RealMatrix cartesianCovarianceOut; - // Return the converted covariance - return covariance; + // Get the orbit inertial frame + final Frame orbitInertialFrame = orbit.getFrame(); - } + if (frameOut.isPseudoInertial()) { + // Compute rotation matrix from lofIn to frameOut + final Rotation rotationFromLofInToFrameOut = + lofIn.rotationFromInertial(orbit.getPVCoordinates(frameOut)).revert(); - /** 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(Orbit, 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 - * @see #getStateCovariance(SpacecraftState) - * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngle) - */ - public RealMatrix getStateCovariance(final SpacecraftState state, final Frame frame) { + // Builds the matrix to perform covariance transformation + final RealMatrix transformationMatrix = + buildTransformationMatrixFromRotation(rotationFromLofInToFrameOut); - // Get the current propagated covariance - final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); + // Get the Cartesian covariance matrix converted to frameOut + cartesianCovarianceOut = + transformationMatrix.multiply(inputCartesianCov.multiplyTransposed(transformationMatrix)); - // Return the converted covariance - return changeCovarianceFrame(state.getOrbit(), state.getFrame(), frame, covariance, covOrbitType, covAngleType); + } + 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); + + // Get the Cartesian covariance matrix converted to orbit inertial frame + final RealMatrix cartesianCovarianceInOrbitFrame = transformationMatrixFromLofInToOrbitFrame.multiply( + inputCartesianCov.multiplyTransposed(transformationMatrixFromLofInToOrbitFrame)); + + // Get the Cartesian covariance matrix converted to frameOut + cartesianCovarianceOut = + changeCovarianceFrame(orbit, orbitInertialFrame, frameOut, cartesianCovarianceInOrbitFrame, + OrbitType.CARTESIAN, PositionAngle.MEAN); + } + + // Output converted covariance + return cartesianCovarianceOut; } /** - * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to another - * {@link LOFType commonly used local orbital frame} + * Get the covariance matrix in another frame. *

- * The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics + * 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. + *

+ * In case the input frame is not pseudo-inertial, the input covariance matrix is expected to + * be expressed in cartesian elements. + *

+ * In case the output frame is not pseudo-inertial, the output covariance matrix will be + * expressed in cartesian elements. * - * @param orbit orbit to which the covariance matrix should correspond - * @param lofIn the local orbital frame in which the input covariance matrix is expressed. - * @param lofOut the target local orbital frame. - * @param pivotFrame the pivot frame (must be inertial). - * @param inputCov input covariance + * @param orbit orbit to which the covariance matrix should correspond + * @param frameIn the frame in which the input covariance matrix is expressed + * @param frameOut the target frame + * @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}) + * {@code CARTESIAN}) * @return the covariance matrix expressed in the target frame + * @throws OrekitException if input frame is not pseudo-inertial and the input covariance is + * not expressed in cartesian elements. */ public static RealMatrix changeCovarianceFrame(final Orbit orbit, - final LOFType lofIn, final LOFType lofOut, final Frame pivotFrame, + final Frame frameIn, final Frame frameOut, final RealMatrix inputCov, final OrbitType covOrbitType, final PositionAngle covAngleType) { - // Pivot frame is inertial - if (pivotFrame.isPseudoInertial()) { + // 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); + + // Input frame pseudo-inertial + if (frameIn.isPseudoInertial()) { // Convert input matrix to Cartesian parameters in input frame final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); - // Compute rotation matrix from lofIn to lofOut - final Rotation rotationFromLofInToLofOut = LOFType.rotationFromLOFInToLOFOut(lofIn, lofOut, - orbit.getPVCoordinates()); + // Get the Cartesian covariance matrix converted to frameOut + final RealMatrix cartesianCovarianceOut = j.multiply(cartesianCovarianceIn.multiplyTransposed(j)); - // Builds the matrix to perform covariance transformation - final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromLofInToLofOut); + // Output frame is pseudo-inertial + if (frameOut.isPseudoInertial()) { - // Get the Cartesian covariance matrix converted to frameOut - final RealMatrix cartesianCovarianceOut = - transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); + // Convert output Cartesian matrix to initial orbit type and position angle + return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, + covOrbitType, covAngleType, cartesianCovarianceOut); + } + // Output frame is not pseudo-inertial + else { - // Convert output Cartesian matrix to initial orbit type and position angle - return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, - covOrbitType, covAngleType, cartesianCovarianceOut); + // Output cartesian matrix + return cartesianCovarianceOut; + } } - // Pivot frame is not inertial + // Input frame is not pseudo-inertial else { - throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, pivotFrame.getName()); + // Covariance is expressed in cartesian elements + if (covOrbitType.equals(OrbitType.CARTESIAN)) { + + // Output the Cartesian covariance matrix converted to frameOut + return j.multiply(inputCov.multiplyTransposed(j)); + + } + // Covariance is not expressed in cartesian elements + else { + throw new OrekitException(OrekitMessages.WRONG_ORBIT_PARAMETERS_TYPE, covOrbitType, + OrbitType.CARTESIAN); + } + } } @@ -293,19 +364,18 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { /** * 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 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 - * + * @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) { + final OrbitType inOrbitType, final PositionAngle inAngleType, + final OrbitType outOrbitType, final PositionAngle outAngleType, + final RealMatrix inputCov) { // Notations: // I: Input orbit type @@ -335,200 +405,186 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } - private static RealMatrix buildTransformationMatrixFromRotation(final Rotation rotation) { - - final double[][] rotationMatrixData = rotation.getMatrix(); - - final RealMatrix transformationMatrix = MatrixUtils.createRealMatrix(6, 6); - - // Fills in the upper left and lower right blocks with the rotation - transformationMatrix.setSubMatrix(rotationMatrixData, 0, 0); - transformationMatrix.setSubMatrix(rotationMatrixData, 3, 3); - - return transformationMatrix; - - } - /** - * Convert the covariance matrix from a {@link LOFType commonly used local orbital frame} to a {@link Frame frame}. + * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. *

* The transformation is based on Equation (20) 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. + *

+ * The output covariance matrix will necessarily be expressed in cartesian elements and not converted back to + * its original expression (if input different from cartesian elements). + *

* - * @param orbit orbit to which the covariance matrix should correspond - * @param lofIn the local orbital 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 + * @param orbit orbit to which the covariance matrix should correspond + * @param frameIn the frame in which the input covariance matrix is expressed. In case the frame is not + * pseudo-inertial, the input covariance matrix is expected to be expressed in cartesian elements. + * @param lofOut the target local orbital frame + * @param inputCov input covariance + * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial) + * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not used + * if covOrbitType equals {@code CARTESIAN}) + * @return the covariance matrix expressed in the target local orbital frame in cartesian elements + * @throws OrekitException if input frame is not pseudo-inertial and the input covariance is + * not expressed in cartesian elements. */ public static RealMatrix changeCovarianceFrame(final Orbit orbit, - final LOFType lofIn, final Frame frameOut, + final Frame frameIn, final LOFType lofOut, final RealMatrix inputCov, final OrbitType covOrbitType, final PositionAngle covAngleType) { // Input frame is inertial - if (frameOut.isPseudoInertial()) { + if (frameIn.isPseudoInertial()) { // Convert input matrix to Cartesian parameters in input frame final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, OrbitType.CARTESIAN, PositionAngle.MEAN, inputCov); - // Compute rotation matrix from lofIn to frameOut - final Rotation rotationFromLofInToFrameOut = - lofIn.rotationFromInertial(orbit.getPVCoordinates(frameOut)).revert(); + // 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(rotationFromLofInToFrameOut); + final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromFrameInToLofOut); // Get the Cartesian covariance matrix converted to frameOut final RealMatrix cartesianCovarianceOut = transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); - // 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(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, - covOrbitType, covAngleType, cartesianCovarianceOut); + // Return converted covariance matrix expressed in cartesian elements + return cartesianCovarianceOut; } - // Output frame is not inertial + // Input frame is not inertial so the covariance matrix is expected to be in cartesian elements else { - throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameOut.getName()); + if (covOrbitType.equals(OrbitType.CARTESIAN)) { + final Frame orbitInertialFrame = orbit.getFrame(); + + // Compute rotation matrix from frameIn to orbit inertial frame + final RealMatrix cartesianCovarianceInOrbitFrame = + changeCovarianceFrame(orbit, frameIn, orbitInertialFrame, inputCov, + OrbitType.CARTESIAN, + PositionAngle.MEAN); + + // Convert from orbit inertial frame to lofOut + return changeCovarianceFrame(orbit, orbitInertialFrame, lofOut, cartesianCovarianceInOrbitFrame, + OrbitType.CARTESIAN, + PositionAngle.MEAN); + + } + else { + throw new OrekitException(OrekitMessages.WRONG_ORBIT_PARAMETERS_TYPE, covOrbitType, + OrbitType.CARTESIAN); + + } + } } /** - * Convert the covariance matrix from a {@link Frame frame} to a {@link LOFType commonly used local orbital frame}. - *

- * The transformation is based on Equation (20) 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. + * Get the orbit type in which the covariance matrix is expressed. * - * @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 lofOut the target local orbital frame - * @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 + * @return the orbit type */ - public static RealMatrix changeCovarianceFrame(final Orbit orbit, - final Frame frameIn, final LOFType lofOut, - final RealMatrix inputCov, - final OrbitType covOrbitType, final PositionAngle covAngleType) { - - // Input frame is inertial - if (frameIn.isPseudoInertial()) { - - // Convert input matrix to Cartesian parameters in input frame - final RealMatrix cartesianCovarianceIn = changeCovarianceType(orbit, covOrbitType, covAngleType, - OrbitType.CARTESIAN, PositionAngle.MEAN, - inputCov); - - // Compute rotation matrix from frameIn to lofOut - final Rotation rotationFromFrameInToLofOut = lofOut.rotationFromInertial(orbit.getPVCoordinates(frameIn)); + public OrbitType getCovarianceOrbitType() { + return covOrbitType; + } - // Builds the matrix to perform covariance transformation - final RealMatrix transformationMatrix = buildTransformationMatrixFromRotation(rotationFromFrameInToLofOut); + /** + * {@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); + } - // Get the Cartesian covariance matrix converted to frameOut - final RealMatrix cartesianCovarianceOut = - transformationMatrix.multiply(cartesianCovarianceIn.multiplyTransposed(transformationMatrix)); + /** + * 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(Orbit, Frame, Frame, RealMatrix, OrbitType, PositionAngle)} method. + *

+ * It is also possible to change the covariance orbit type by using the + * {@link #changeCovarianceType(Orbit, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)} method. + * + * @param state spacecraft state to which the covariance matrix should correspond + * @return the state covariance matrix + * @see #getStateCovariance(SpacecraftState, Frame) + * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngle) + */ + public RealMatrix getStateCovariance(final SpacecraftState state) { - // Convert orbit frame to output frame - // final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameIn), frameIn, orbit.getMu()); + // Get the current propagated covariance + final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); - // Convert output Cartesian matrix to initial orbit type and position angle - return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, - covOrbitType, covAngleType, cartesianCovarianceOut); + // Return the converted covariance + return covariance; - } + } - // Output frame is not inertial - else { - throw new OrekitException(OrekitMessages.NON_PSEUDO_INERTIAL_FRAME, frameIn.getName()); + /** + * 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; } - /** Get the covariance matrix in another frame. + /** + * Get the state covariance matrix (6x6 dimension) expressed in a given frame. *

- * The transformation is based on Equation (18) of - * "Covariance Transformations for Satellite Flight Dynamics Operations" - * by David A. Vallado". + * The output covariance matrix is expressed in the same orbit type as {@link #getCovarianceOrbitType()}. *

- * 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 + * It is also possible to change the covariance orbit type by using the + * {@link #changeCovarianceType(Orbit, 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 + * @see #getStateCovariance(SpacecraftState) + * @see #getStateCovariance(SpacecraftState, OrbitType, PositionAngle) */ - 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)); + public RealMatrix getStateCovariance(final SpacecraftState state, final Frame frame) { - // Convert orbit frame to output frame - // FIXME The line below can be safely removed without changing the tests results - // final Orbit outOrbit = new CartesianOrbit(orbit.getPVCoordinates(frameOut), frameOut, orbit.getMu()); + // Get the current propagated covariance + final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); - // Convert output Cartesian matrix to initial orbit type and position angle - return changeCovarianceType(orbit, OrbitType.CARTESIAN, PositionAngle.MEAN, - covOrbitType, covAngleType, cartesianCovarianceOut); + // 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 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 - * * @see #getStateCovariance(SpacecraftState) * @see #getStateCovariance(SpacecraftState, Frame) */ public RealMatrix getStateCovariance(final SpacecraftState state, final OrbitType orbitType, - final PositionAngle angleType) { + final PositionAngle angleType) { // Get the current propagated covariance final RealMatrix covariance = toRealMatrix(state.getAdditionalState(additionalName)); @@ -538,29 +594,15 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { } - /** 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. + /** + * 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; + 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); diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java index ed2bb63ba..b64a549bc 100644 --- a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java +++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java @@ -26,8 +26,8 @@ import org.hipparchus.util.FastMath; import org.junit.jupiter.api.Assertions; import org.junit.jupiter.api.DisplayName; import org.junit.jupiter.api.Test; -import org.mockito.Mockito; import org.orekit.Utils; +import org.orekit.errors.OrekitException; import org.orekit.forces.ForceModel; import org.orekit.forces.gravity.HolmesFeatherstoneAttractionModel; import org.orekit.forces.gravity.potential.GravityFieldFactory; @@ -141,15 +141,15 @@ public class StateCovarianceMatrixProviderTest { void should_return_same_covariance_matrix() { // Given - final AbsoluteDate initialDate = new AbsoluteDate(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final double mu = 398600e9; + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; final PVCoordinates initialPV = new PVCoordinates( new Vector3D(6778000, 0, 0), new Vector3D(0, 7668.63, 0)); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu); final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] { { 1, 0, 0, 0, 0, 0 }, @@ -162,7 +162,7 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix covarianceMatrixInRTN = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, + initialInertialFrame, LOFType.QSW, initialCovarianceInInertialFrame, OrbitType.CARTESIAN, @@ -515,19 +515,19 @@ public class StateCovarianceMatrixProviderTest { } @Test - @DisplayName("Test conversion from inertial frame to RTN local orbital frame") + @DisplayName("Test covariance conversion from inertial frame to RTN local orbital frame") void should_rotate_covariance_matrix_by_ninety_degrees() { // Given - final AbsoluteDate initialDate = new AbsoluteDate(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final double mu = 398600e9; + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; final PVCoordinates initialPV = new PVCoordinates( new Vector3D(0, 6778000, 0), new Vector3D(-7668.63, 0, 0)); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu); final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] { { 1, 0, 0, 0, 0, 1e-5 }, @@ -541,7 +541,7 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInRTN = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, LOFType.QSW, + initialInertialFrame, LOFType.QSW, initialCovarianceInInertialFrame, OrbitType.CARTESIAN, PositionAngle.MEAN); @@ -561,19 +561,19 @@ public class StateCovarianceMatrixProviderTest { } @Test - @DisplayName("Test conversion from RTN local orbital frame to inertial frame") + @DisplayName("Test covariance conversion from RTN local orbital frame to inertial frame") void should_rotate_covariance_matrix_by_minus_ninety_degrees() { // Given - final AbsoluteDate initialDate = new AbsoluteDate(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final double mu = 398600e9; + final AbsoluteDate initialDate = new AbsoluteDate(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final double mu = 398600e9; final PVCoordinates initialPV = new PVCoordinates( new Vector3D(0, 6778000, 0), new Vector3D(-7668.63, 0, 0)); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, mu); + final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu); final RealMatrix initialCovarianceInRTN = new BlockRealMatrix(new double[][] { { 1, 0, 0, 0, 0, 0 }, @@ -587,8 +587,8 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInInertialFrame = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - LOFType.QSW, inertialFrame, initialCovarianceInRTN, - OrbitType.CARTESIAN, PositionAngle.MEAN); + LOFType.QSW, initialInertialFrame, + initialCovarianceInRTN); // Then @@ -612,26 +612,30 @@ public class StateCovarianceMatrixProviderTest { *

* 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. + *

+ * Note that the followings local orbital frame are equivalent RSW=RTN=QSW. */ @Test - @DisplayName("Test Vallado test case : ECI cartesian to RTN") + @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to RTN") void should_return_Vallado_RSW_covariance_matrix_from_ECI() { // Initialize Orekit Utils.setDataRoot("regular-data"); // Given - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); // When final RealMatrix convertedCovarianceMatrixInRTN = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, LOFType.QSW, initialCovarianceMatrix, + initialInertialFrame, LOFType.QSW, + initialCovarianceMatrix, OrbitType.CARTESIAN, PositionAngle.MEAN); // Then @@ -681,24 +685,26 @@ public class StateCovarianceMatrixProviderTest { * cartesian covariance in NTW from p.19. */ @Test - @DisplayName("Test Vallado test case : ECI cartesian to NTW") + @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to NTW") void should_return_Vallado_NTW_covariance_matrix_from_ECI() { // Initialize orekit Utils.setDataRoot("regular-data"); // Given - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); // When final RealMatrix convertedCovarianceMatrixInNTW = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, LOFType.NTW, initialCovarianceMatrix, + initialInertialFrame, LOFType.NTW, + initialCovarianceMatrix, OrbitType.CARTESIAN, PositionAngle.MEAN); // Then @@ -715,6 +721,91 @@ public class StateCovarianceMatrixProviderTest { } + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial covariance matrix from p.14 as a reference to test multiple + * conversions. + *

+ * This test aims to verify the numerical precision after various conversions and serves as a non regression test + * for future updates. + *

+ * Also, note that the conversion from the RTN to TEME tests the fact that the orbit is initially expressed in GCRF + * while we want the covariance expressed in TEME. Hence, it tests that the rotation from RTN to TEME needs to be + * obtained by expressing the orbit PVCoordinates in the TEME frame (see relevent changeCovarianceFrame method). + */ + @Test + @DisplayName("Test custom covariance conversion Vallado test case : GCRF -> TEME -> IRTF -> NTW -> RTN -> ITRF -> GCRF") + void should_return_initial_covariance_after_multiple_conversion() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrixInGCRF = getValladoInitialCovarianceMatrix(); + + final Frame teme = FramesFactory.getTEME(); + + final Frame itrf = FramesFactory.getITRF(IERSConventions.IERS_2010, false); + + // When + // GCRF -> TEME + final RealMatrix convertedCovarianceMatrixInTEME = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + initialInertialFrame, teme, + initialCovarianceMatrixInGCRF, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // TEME -> ITRF + final RealMatrix convertedCovarianceMatrixInITRF = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + teme, itrf, + convertedCovarianceMatrixInTEME, + OrbitType.CARTESIAN, PositionAngle.MEAN); + // ITRF -> NTW + final RealMatrix convertedCovarianceMatrixInNTW = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + itrf, LOFType.NTW, + convertedCovarianceMatrixInITRF, + OrbitType.CARTESIAN, PositionAngle.MEAN); + // NTW -> RTN + final RealMatrix convertedCovarianceMatrixInRTN = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + LOFType.NTW, LOFType.QSW, + convertedCovarianceMatrixInNTW); + // RTN -> ITRF + final RealMatrix convertedCovarianceMatrixBackInITRF = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + LOFType.QSW, itrf, + convertedCovarianceMatrixInRTN); + + // ITRF -> TEME + final RealMatrix convertedCovarianceMatrixBackInTEME = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + itrf, teme, + convertedCovarianceMatrixBackInITRF, + OrbitType.CARTESIAN, PositionAngle.MEAN); + // TEME -> GCRF + final RealMatrix convertedCovarianceMatrixInGCRF = + StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, + teme, initialInertialFrame, + convertedCovarianceMatrixBackInTEME, + OrbitType.CARTESIAN, PositionAngle.MEAN); + + // Then + final RealMatrix expectedCovarianceMatrixInGCRF = getValladoInitialCovarianceMatrix(); + + compareCovariance(expectedCovarianceMatrixInGCRF, convertedCovarianceMatrixInGCRF, 1e-12); + + } + /** * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations * from David A. Vallado. @@ -731,7 +822,7 @@ public class StateCovarianceMatrixProviderTest { *

*/ @Test - @DisplayName("Test Vallado test case : ECI cartesian to PEF") + @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to PEF") void should_return_Vallado_PEF_covariance_matrix_from_ECI() { // Initialize orekit @@ -739,11 +830,11 @@ public class StateCovarianceMatrixProviderTest { // Given // Initialize orbit - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); // Initialize input covariance matrix final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); @@ -754,23 +845,24 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInITRF = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, outputFrame, initialCovarianceMatrix, + initialInertialFrame, outputFrame, + initialCovarianceMatrix, OrbitType.CARTESIAN, PositionAngle.MEAN); // Then final RealMatrix expectedCovarianceMatrixInITRF = new BlockRealMatrix(new double[][] { - { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, - 3.4548396261054936e-05, 2.6851237046859200e-06, 5.8312677693153940e-05 }, - { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, - 1.4852736004690684e-04, 1.6544247282904867e-04, 1.2884310644320954e-04 }, - { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, - 5.9252211072590390e-05, 1.2841787487219444e-04, 1.0000913090989617e-04 }, - { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, - 3.5631474857130520e-07, 7.6083489184819870e-07, 5.9252213790760030e-07 }, - { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, - 7.6083489184819880e-07, 1.6542289254142709e-06, 1.2841787929229964e-06 }, - { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, - 5.9252213790760020e-07, 1.2841787929229960e-06, 1.0000913098203875e-06 } + { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, 3.4548396261054936e-05, + 2.6851237046859200e-06, 5.8312677693153940e-05 }, + { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, 1.4852736004690684e-04, + 1.6544247282904867e-04, 1.2884310644320954e-04 }, + { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, 5.9252211072590390e-05, + 1.2841787487219444e-04, 1.0000913090989617e-04 }, + { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, 3.5631474857130520e-07, + 7.6083489184819870e-07, 5.9252213790760030e-07 }, + { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, 7.6083489184819880e-07, + 1.6542289254142709e-06, 1.2841787929229964e-06 }, + { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, 5.9252213790760020e-07, + 1.2841787929229960e-06, 1.0000913098203875e-06 } }); compareCovariance(expectedCovarianceMatrixInITRF, convertedCovarianceMatrixInITRF, 1e-20); @@ -782,29 +874,35 @@ public class StateCovarianceMatrixProviderTest { * from David A. Vallado. *

* More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the - * cartesian covariance in ECEF from p.18. + * cartesian covariance in PEF from p.18. */ @Test - //TODO - @DisplayName("Test Vallado test case : PEF cartesian to ECI") + @DisplayName("Test covariance conversion Vallado test case : PEF cartesian to ECI") void should_return_Vallado_ECI_covariance_matrix_from_PEF() { // Initialize orekit Utils.setDataRoot("regular-data"); // Given - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); - - final RealMatrix initialCovarianceMatrix = new BlockRealMatrix(new double[][] { - { 9.934002e-001, 7.512598e-003, 5.831364e-003, 3.400170e-005, 7.512591e-005, 5.831364e-005 }, - { 7.512598e-003, 1.006599e+000, 1.288427e-002, 7.512605e-005, 1.659892e-004, 1.288427e-004 }, - { 5.831364e-003, 1.288427e-002, 1.000001e+000, 5.831364e-005, 1.288427e-004, 1.000092e-004 }, - { 3.400170e-005, 7.512605e-005, 5.831364e-005, 3.400170e-007, 7.512598e-007, 5.831364e-007 }, - { 7.512591e-005, 1.659892e-004, 1.288427e-004, 7.512598e-007, 1.659891e-006, 1.288427e-006 }, - { 5.831364e-005, 1.288427e-004, 1.000092e-004, 5.831364e-007, 1.288427e-006, 1.000092e-006 } + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); + + final RealMatrix initialCovarianceMatrixInPEF = new BlockRealMatrix(new double[][] { + { 9.9340005761276870e-01, 7.5124999798868530e-03, 5.8312675007359050e-03, 3.4548396261054936e-05, + 2.6851237046859200e-06, 5.8312677693153940e-05 }, + { 7.5124999798868025e-03, 1.0065990293034541e+00, 1.2884310200351924e-02, 1.4852736004690684e-04, + 1.6544247282904867e-04, 1.2884310644320954e-04 }, + { 5.8312675007359040e-03, 1.2884310200351924e-02, 1.0000009130837746e+00, 5.9252211072590390e-05, + 1.2841787487219444e-04, 1.0000913090989617e-04 }, + { 3.4548396261054936e-05, 1.4852736004690686e-04, 5.9252211072590403e-05, 3.5631474857130520e-07, + 7.6083489184819870e-07, 5.9252213790760030e-07 }, + { 2.6851237046859150e-06, 1.6544247282904864e-04, 1.2841787487219447e-04, 7.6083489184819880e-07, + 1.6542289254142709e-06, 1.2841787929229964e-06 }, + { 5.8312677693153934e-05, 1.2884310644320950e-04, 1.0000913090989616e-04, 5.9252213790760020e-07, + 1.2841787929229960e-06, 1.0000913098203875e-06 } }); final Frame inputFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, false); @@ -812,20 +910,14 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInECI = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inputFrame, inertialFrame, initialCovarianceMatrix, + inputFrame, initialInertialFrame, + initialCovarianceMatrixInPEF, OrbitType.CARTESIAN, PositionAngle.MEAN); // Then final RealMatrix expectedCovarianceMatrixInECI = getValladoInitialCovarianceMatrix(); - final RealMatrix differenceMatrix = expectedCovarianceMatrixInECI.subtract(convertedCovarianceMatrixInECI); - System.out.println("convertedCovarianceMatrixInECI"); - printMatrix(convertedCovarianceMatrixInECI); - - System.out.println("differenceMatrix"); - printMatrix(differenceMatrix); - - compareCovariance(expectedCovarianceMatrixInECI, convertedCovarianceMatrixInECI, DEFAULT_VALLADO_THRESHOLD); + compareCovariance(expectedCovarianceMatrixInECI, convertedCovarianceMatrixInECI, 1e-7); } @@ -835,19 +927,21 @@ public class StateCovarianceMatrixProviderTest { *

* More specifically, we're using the initial covariance matrix from p.14 and compare the computed result with the * cartesian covariance in MOD from p.17. + *

*/ @Test - @DisplayName("Test Vallado test case : ECI cartesian to MOD") + @DisplayName("Test covariance conversion Vallado test case : ECI cartesian to MOD") void should_return_Vallado_MOD_covariance_matrix_from_ECI() { // Initialize orekit Utils.setDataRoot("regular-data"); // Given - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); final RealMatrix initialCovarianceMatrix = getValladoInitialCovarianceMatrix(); @@ -856,7 +950,8 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInMOD = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, - inertialFrame, outputFrame, initialCovarianceMatrix, + initialInertialFrame, outputFrame, + initialCovarianceMatrix, OrbitType.CARTESIAN, PositionAngle.MEAN); // Then @@ -869,31 +964,31 @@ public class StateCovarianceMatrixProviderTest { { 9.997861e-005, 1.000307e-004, 1.000186e-004, 9.997861e-007, 1.000307e-006, 1.000186e-006 }, }); - final RealMatrix differenceMatrix = expectedCovarianceMatrixInMOD.subtract(convertedCovarianceMatrixInMOD); - System.out.println("convertedCovarianceMatrixInMOD"); - printMatrix(convertedCovarianceMatrixInMOD); - - System.out.println("differenceMatrix"); - printMatrix(differenceMatrix); - compareCovariance(expectedCovarianceMatrixInMOD, convertedCovarianceMatrixInMOD, DEFAULT_VALLADO_THRESHOLD); } + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial NTW covariance matrix from p.19 and compare the computed result with + * the cartesian covariance in RSW from the same page. + *

+ */ @Test - @DisplayName("Test conversion from Vallado test case NTW to RSW") + @DisplayName("Test covariance conversion from Vallado test case NTW to RSW") void should_convert_Vallado_NTW_to_RSW() { // Initialize orekit Utils.setDataRoot("regular-data"); - final Orbit orbitMock = Mockito.mock(Orbit.class); - // Given - final AbsoluteDate initialDate = getValladoInitialDate(); - final PVCoordinates initialPV = getValladoInitialPV(); - final Frame inertialFrame = FramesFactory.getGCRF(); - final Orbit initialOrbit = new CartesianOrbit(initialPV, inertialFrame, initialDate, getValladoMu()); + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); final RealMatrix initialCovarianceMatrixInNTW = new BlockRealMatrix(new double[][] { { 9.918792e-001, 6.679546e-003, -2.868345e-003, 1.879167e-005, 6.679546e-005, -2.868345e-005 }, @@ -907,8 +1002,7 @@ public class StateCovarianceMatrixProviderTest { // When final RealMatrix convertedCovarianceMatrixInRTN = StateCovarianceMatrixProvider.changeCovarianceFrame(initialOrbit, LOFType.NTW, LOFType.QSW, - inertialFrame, initialCovarianceMatrixInNTW, - OrbitType.CARTESIAN, PositionAngle.MEAN); + initialCovarianceMatrixInNTW); // Then final RealMatrix expectedCovarianceMatrixInRTN = new BlockRealMatrix(new double[][] { @@ -924,6 +1018,60 @@ public class StateCovarianceMatrixProviderTest { } + /** + * This test is based on the following paper : Covariance Transformations for Satellite Flight Dynamics Operations + * from David A. Vallado. + *

+ * More specifically, we're using the initial NTW covariance matrix from p.19 and compare the computed result with + * the cartesian covariance in RSW from the same page. + *

+ */ + @Test + @DisplayName("Test thrown error if input frame is not pseudo-inertial and the covariance matrix is not expressed in cartesian elements") + void should_return_orekit_exception() { + + // Initialize orekit + Utils.setDataRoot("regular-data"); + + // Given + final AbsoluteDate initialDate = getValladoInitialDate(); + final PVCoordinates initialPV = getValladoInitialPV(); + final Frame initialInertialFrame = FramesFactory.getGCRF(); + final Orbit initialOrbit = + new CartesianOrbit(initialPV, initialInertialFrame, initialDate, getValladoMu()); + + final RealMatrix randomCovarianceMatrix = 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 } + }); + + final Frame nonInertialFrame = FramesFactory.getITRF(IERSConventions.IERS_2010, true); + + final Frame inertialFrame = FramesFactory.getGCRF(); + + // When & Then + Assertions.assertThrows(OrekitException.class, + () -> { + StateCovarianceMatrixProvider.changeCovarianceFrame( + initialOrbit, nonInertialFrame, inertialFrame, + randomCovarianceMatrix, OrbitType.CIRCULAR, + PositionAngle.MEAN); + }); + + Assertions.assertThrows(OrekitException.class, + () -> { + StateCovarianceMatrixProvider.changeCovarianceFrame( + initialOrbit, nonInertialFrame, LOFType.QSW, + randomCovarianceMatrix, OrbitType.EQUINOCTIAL, + PositionAngle.MEAN); + }); + + } + public static void printMatrix(final RealMatrix covariance) { // Create a string builder -- GitLab From 48fb5ccb40bd809ca5cb74e6892247ed43dc01e3 Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Fri, 30 Sep 2022 09:42:34 +0200 Subject: [PATCH 07/10] Fixed issue-964 Fixed rotation from LOFType to LOFType using a composition of rotation to the ECI frame where the given PVCoordinates are defined. --- src/main/java/org/orekit/frames/LOFType.java | 130 +++++++++++++------ 1 file changed, 90 insertions(+), 40 deletions(-) diff --git a/src/main/java/org/orekit/frames/LOFType.java b/src/main/java/org/orekit/frames/LOFType.java index bb99b19c8..988611338 100644 --- a/src/main/java/org/orekit/frames/LOFType.java +++ b/src/main/java/org/orekit/frames/LOFType.java @@ -23,7 +23,6 @@ import org.hipparchus.geometry.euclidean.threed.FieldVector3D; import org.hipparchus.geometry.euclidean.threed.Rotation; import org.hipparchus.geometry.euclidean.threed.RotationConvention; import org.hipparchus.geometry.euclidean.threed.Vector3D; -import org.hipparchus.util.FastMath; import org.orekit.time.AbsoluteDate; import org.orekit.time.FieldAbsoluteDate; import org.orekit.utils.FieldPVCoordinates; @@ -52,16 +51,20 @@ public enum LOFType { */ TNW { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, TNW, pv); } /** {@inheritDoc} */ + @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getVelocity(), pv.getMomentum(), @@ -89,58 +92,78 @@ public enum LOFType { */ QSW { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { - final Vector3D position = pv.getPosition(); - final Vector3D velocity = pv.getVelocity(); - final Vector3D momentum = pv.getMomentum(); + // Compute the common rotation to all cases + final Rotation rotationFromECIToQSW = LOFType.QSW.rotationFromInertial(pv); switch (fromLOF) { case EQW: - final Vector3D ascendingNode = new Vector3D(-momentum.getY(), momentum.getX(), 0); - return new Rotation(ascendingNode, Vector3D.crossProduct(momentum, ascendingNode), - position, Vector3D.crossProduct(momentum, position)); - case NTW: - final Vector3D transverse = velocity.crossProduct(momentum); + final Rotation rotationFromEQWToECI = LOFType.EQW.rotationFromInertial(pv); - final double cosAngle = - transverse.dotProduct(position) / (transverse.getNorm() * position.getNorm()); + final Rotation rotationFromEQWToQSW = + rotationFromEQWToECI.compose(rotationFromECIToQSW, RotationConvention.FRAME_TRANSFORM); - final double angle = FastMath.acos(cosAngle); + return rotationFromEQWToQSW; + case NTW: + final Rotation rotationFromNTWToECI = LOFType.NTW.rotationFromInertial(pv).revert(); - final double[][] rotationMatrixData = new double[][] { - { FastMath.cos(angle), FastMath.sin(angle), 0 }, - { -FastMath.sin(angle), FastMath.cos(angle), 0 }, - { 0, 0, 1 }, - }; + final Rotation rotationFromNTWToQSW = + rotationFromNTWToECI.compose(rotationFromECIToQSW, RotationConvention.FRAME_TRANSFORM); - return new Rotation(rotationMatrixData, 1e-15); + return rotationFromNTWToQSW; case VNC: - return new Rotation(velocity, velocity.crossProduct(momentum), - position, momentum); + final Rotation rotationFromVNCToECI = LOFType.VNC.rotationFromInertial(pv); + + final Rotation rotationFromVNCToQSW = + rotationFromVNCToECI.compose(rotationFromECIToQSW, RotationConvention.FRAME_TRANSFORM); + + return rotationFromVNCToQSW; + case LVLH: return Rotation.IDENTITY; + case VVLH: - return new Rotation(position.negate(), position.negate().crossProduct(momentum.negate()), - position, momentum); + final Rotation rotationFromVVLHToECI = LOFType.VVLH.rotationFromInertial(pv); + + final Rotation rotationFromVVLHToQSW = + rotationFromVVLHToECI.compose(rotationFromECIToQSW, RotationConvention.FRAME_TRANSFORM); + + return rotationFromVVLHToQSW; + case LVLH_CCSDS: - return new Rotation(momentum.negate().crossProduct(position), position, - position, momentum); + final Rotation rotationFromLVLH_CCSDSToECI = LOFType.LVLH_CCSDS.rotationFromInertial(pv); + + final Rotation rotationFromLVLH_CCSDSToQSW = + rotationFromLVLH_CCSDSToECI.compose(rotationFromECIToQSW, + RotationConvention.FRAME_TRANSFORM); + + return rotationFromLVLH_CCSDSToQSW; + case TNW: - return new Rotation(velocity, momentum.crossProduct(velocity), - position, momentum.crossProduct(position)); + final Rotation rotationFromTNWToECI = LOFType.TNW.rotationFromInertial(pv); + + final Rotation rotationFromTNWToQSW = + rotationFromTNWToECI.compose(rotationFromECIToQSW, RotationConvention.FRAME_TRANSFORM); + + return rotationFromTNWToQSW; + default: return Rotation.IDENTITY; } } /** {@inheritDoc} */ + @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), @@ -174,16 +197,20 @@ public enum LOFType { */ LVLH { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_K); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, LVLH, pv); } /** {@inheritDoc} */ + @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), @@ -215,16 +242,20 @@ public enum LOFType { */ LVLH_CCSDS { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getPosition(), pv.getMomentum(), Vector3D.MINUS_K, Vector3D.MINUS_J); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, LVLH_CCSDS, pv); } /** {@inheritDoc} */ + @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { return new FieldRotation<>(pv.getPosition(), pv.getMomentum(), @@ -255,15 +286,19 @@ public enum LOFType { */ VVLH { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return LVLH_CCSDS.rotationFromInertial(pv); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, VVLH, pv); } /** {@inheritDoc} */ + @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { return LVLH_CCSDS.rotationFromInertial(field, pv); @@ -287,15 +322,19 @@ public enum LOFType { */ VNC { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_I, Vector3D.PLUS_J); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, VNC, pv); } + /** {@inheritDoc} */ @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { @@ -312,16 +351,20 @@ public enum LOFType { */ EQW { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { final Vector3D m = pv.getMomentum(); return new Rotation(new Vector3D(-m.getY(), m.getX(), 0), m, Vector3D.PLUS_I, Vector3D.PLUS_J); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, EQW, pv); } + /** {@inheritDoc} */ @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { @@ -350,15 +393,19 @@ public enum LOFType { */ NTW { /** {@inheritDoc} */ + @Override public Rotation rotationFromInertial(final PVCoordinates pv) { return new Rotation(pv.getVelocity(), pv.getMomentum(), Vector3D.PLUS_J, Vector3D.PLUS_K); } - @Override public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { + /** {@inheritDoc} */ + @Override + public Rotation rotationFromLOFType(final LOFType fromLOF, final PVCoordinates pv) { return rotationFromLOFInToLOFOut(fromLOF, NTW, pv); } + /** {@inheritDoc} */ @Override public > FieldRotation rotationFromInertial(final Field field, final FieldPVCoordinates pv) { @@ -370,20 +417,23 @@ public enum LOFType { }; /** - * Get the rotation from (common) input to output local orbital frame. + * Get the rotation from input to output {@link LOFType commonly used local orbital frame}. * - * @param in input (common) local orbital frame - * @param out output (common) local orbital frame - * @param pv position-velocity of the spacecraft in some inertial frame - * @return rotation from (common) input to output local orbital frame. + * @param in input commonly used local orbital frame + * @param out output commonly used local orbital frame + * @param pv position-velocity of the spacecraft in some inertial frame + * @return rotation from input to output {@link LOFType commonly used local orbital frame}. */ public static Rotation rotationFromLOFInToLOFOut(final LOFType in, final LOFType out, final PVCoordinates pv) { + // First compute the rotation from the input LOF to the pivot LOF QSW final Rotation inToQSW = LOFType.QSW.rotationFromLOFType(in, pv); + // Then compute the rotation from the pivot LOF QSW to the output LOF final Rotation QSWToOut = LOFType.QSW.rotationFromLOFType(out, pv).revert(); - return QSWToOut.compose(inToQSW, RotationConvention.VECTOR_OPERATOR); + // Output composed rotation + return inToQSW.compose(QSWToOut, RotationConvention.FRAME_TRANSFORM); } /** -- GitLab From 71cc0811ead582a7be3296f13061f9e1bfcdd9f7 Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Fri, 30 Sep 2022 09:56:38 +0200 Subject: [PATCH 08/10] Improved & Fixed Javadoc Improved & Fixed Javadoc of StateCovarianceMatrixProvider and StateCovarianceMatrixProviderTest. --- .../propagation/StateCovarianceMatrixProvider.java | 12 ++++++------ .../StateCovarianceMatrixProviderTest.java | 3 ++- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index 7343edfd9..f54893e69 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -216,7 +216,7 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { * The transformation is based on Equation (20) of "Covariance Transformations for Satellite Flight Dynamics * Operations" by David A. Vallado". *

- * The input covariance matrix is expected to be expressed in cartesian elements. + * The input covariance matrix is necessarily expressed in cartesian elements. *

* The output covariance matrix will be expressed in cartesian elements. * @@ -283,8 +283,8 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { * 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. *

- * In case the input frame is not pseudo-inertial, the input covariance matrix is expected to - * be expressed in cartesian elements. + * In case the input frame is not pseudo-inertial, the input covariance matrix is necessarily + * expressed in cartesian elements. *

* In case the output frame is not pseudo-inertial, the output covariance matrix will be * expressed in cartesian elements. @@ -293,9 +293,9 @@ public class StateCovarianceMatrixProvider implements AdditionalStateProvider { * @param frameIn the frame in which the input covariance matrix is expressed * @param frameOut the target frame * @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}) + * @param covOrbitType orbit type of the covariance matrix (used if frameIn is pseudo-inertial) + * @param covAngleType position angle type of the covariance matrix (used if frameIn is pseudo-inertial) (not + * used if covOrbitType equals {@code CARTESIAN}) * @return the covariance matrix expressed in the target frame * @throws OrekitException if input frame is not pseudo-inertial and the input covariance is * not expressed in cartesian elements. diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java index b64a549bc..57817dc1f 100644 --- a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java +++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java @@ -733,7 +733,8 @@ public class StateCovarianceMatrixProviderTest { *

* Also, note that the conversion from the RTN to TEME tests the fact that the orbit is initially expressed in GCRF * while we want the covariance expressed in TEME. Hence, it tests that the rotation from RTN to TEME needs to be - * obtained by expressing the orbit PVCoordinates in the TEME frame (see relevent changeCovarianceFrame method). + * obtained by expressing the orbit PVCoordinates in the TEME frame (hence the use of orbit.gtPVCoordinates(frameOut) + * ,see relevant changeCovarianceFrame method). */ @Test @DisplayName("Test custom covariance conversion Vallado test case : GCRF -> TEME -> IRTF -> NTW -> RTN -> ITRF -> GCRF") -- GitLab From 961d1b386caf285810861fc99282ea963e4be9fe Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Fri, 30 Sep 2022 10:06:05 +0200 Subject: [PATCH 09/10] Minor changes to StateCovarianceMatrixProviderTest Fixed one test where the covariance matrix was diagonal. Added missing javadoc on same test. --- .../StateCovarianceMatrixProviderTest.java | 39 ++++++------------- 1 file changed, 12 insertions(+), 27 deletions(-) diff --git a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java index 57817dc1f..b0720bf94 100644 --- a/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java +++ b/src/test/java/org/orekit/propagation/StateCovarianceMatrixProviderTest.java @@ -48,8 +48,6 @@ import org.orekit.utils.Constants; import org.orekit.utils.IERSConventions; import org.orekit.utils.PVCoordinates; -import java.util.Locale; - public class StateCovarianceMatrixProviderTest { private SpacecraftState initialState; @@ -136,6 +134,10 @@ public class StateCovarianceMatrixProviderTest { } } + /** + * 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") void should_return_same_covariance_matrix() { @@ -152,12 +154,12 @@ public class StateCovarianceMatrixProviderTest { final Orbit initialOrbit = new CartesianOrbit(initialPV, initialInertialFrame, initialDate, mu); final RealMatrix initialCovarianceInInertialFrame = new BlockRealMatrix(new double[][] { - { 1, 0, 0, 0, 0, 0 }, - { 0, 1, 0, 0, 0, 0 }, + { 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 }, - { 0, 0, 0, 0, 1e-3, 0 }, - { 0, 0, 0, 0, 0, 1e-3 } + { 1e-5, 0, 0, 0, 1e-3, 0 }, + { 1e-4, 1e-5, 0, 0, 0, 1e-3 } }); // When @@ -170,12 +172,12 @@ public class StateCovarianceMatrixProviderTest { // Then final RealMatrix expectedMatrixInRTN = new BlockRealMatrix(new double[][] { - { 1, 0, 0, 0, 0, 0 }, - { 0, 1, 0, 0, 0, 0 }, + { 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 }, - { 0, 0, 0, 0, 1e-3, 0 }, - { 0, 0, 0, 0, 0, 1e-3 } + { 1e-5, 0, 0, 0, 1e-3, 0 }, + { 1e-4, 1e-5, 0, 0, 0, 1e-3 } }); compareCovariance(covarianceMatrixInRTN, expectedMatrixInRTN, 1e-20); @@ -1073,21 +1075,4 @@ public class StateCovarianceMatrixProviderTest { } - public static void printMatrix(final RealMatrix covariance) { - - // Create a string builder - final StringBuilder covToPrint = new StringBuilder(); - for (int row = 0; row < covariance.getRowDimension(); row++) { - for (int column = 0; column < covariance.getColumnDimension(); column++) { - covToPrint.append(String.format(Locale.US, "%16.16e", covariance.getEntry(row, column))); - covToPrint.append(" "); - } - covToPrint.append("\n"); - } - - // Print - System.out.println(covToPrint); - - } - } -- GitLab From d0c99dbd2278c204046429faf15f3687aab560d4 Mon Sep 17 00:00:00 2001 From: Vincent Cucchietti Date: Mon, 3 Oct 2022 09:30:49 +0200 Subject: [PATCH 10/10] Minor change to StateCovarianceMatrixProvider documentation --- .../org/orekit/propagation/StateCovarianceMatrixProvider.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java index f54893e69..4de49bcd2 100644 --- a/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java +++ b/src/main/java/org/orekit/propagation/StateCovarianceMatrixProvider.java @@ -52,7 +52,7 @@ import org.orekit.utils.CartesianDerivativesFilter; * It is possible to change the covariance frame by using the * {@link #changeCovarianceFrame(Orbit, Frame, Frame, RealMatrix, OrbitType, PositionAngle)} 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. + * Vallado. *

* Finally, covariance orbit type can be changed using the * {@link #changeCovarianceType(Orbit, OrbitType, PositionAngle, OrbitType, PositionAngle, RealMatrix)} method. -- GitLab