Feature/covariance transformation

include/tudat/astro/propagators/propagateCovariance.h

@@ -103,22 +103,23 @@ void propagateCovariance(
         const double initialTime,
         const double finalTime );
 
-//Eigen::MatrixXd convertCovarianceToFrame(
-//        const Eigen::MatrixXd inputCovariance,
-//        const Eigen::VectorXd inertialCartesianRelativeState,
-//        const reference_frames::SatelliteReferenceFrames inputFrame,
-//        const reference_frames::SatelliteReferenceFrames outputFrame );
-
-//std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame(
-//        const std::map< double, Eigen::MatrixXd > inputCovariances,
-//        const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates,
-//        const reference_frames::SatelliteReferenceFrames inputFrame,
-//        const reference_frames::SatelliteReferenceFrames outputFrame );
+Eigen::MatrixXd convertCovarianceToFrame(
+        const Eigen::MatrixXd inputCovariance,
+        const Eigen::VectorXd inertialCartesianRelativeState,
+        const reference_frames::SatelliteReferenceFrames inputFrame,
+        const reference_frames::SatelliteReferenceFrames outputFrame );
+
+std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame(
+        const std::map< double, Eigen::MatrixXd > inputCovariances,
+        const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates,
+        const reference_frames::SatelliteReferenceFrames inputFrame,
+        const reference_frames::SatelliteReferenceFrames outputFrame );
 
 void convertCovarianceHistoryToFormalErrorHistory(
         std::map< double, Eigen::VectorXd >& propagatedFormalErrors,
         std::map< double, Eigen::MatrixXd >& propagatedCovariance );
 
+
 //! Function to propagate full covariance at the initial time to state formal errors at later times
 /*!
  * Function to propagate full covariance at the initial time to state formal errors at later times

include/tudat/astro/reference_frames/referenceFrameTransformations.h

@@ -47,6 +47,16 @@ enum AerodynamicsReferenceFrames
     body_frame = 4
 };
 
+// Enum defining identifiers of frames in which a user-specified thrust is defined.
+//! @get_docstring(ThrustFrames.__docstring__)
+enum SatelliteReferenceFrames
+{
+    unspecified_reference_frame = -1,
+    global_reference_frame = 0,
+    tnw_reference_frame = 1,
+    rsw_reference_frame = 2
+};
+
 //! Function to get a string representing a 'named identification' of a reference frame.
 /*!
  * Function to get a string representing a 'named identification' of a reference frame.
@@ -678,6 +688,11 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
         const std::function< Eigen::Vector3d( ) > positionFunctionOfRelativeBody,
         const std::function< Eigen::Quaterniond( ) > orientationFunctionOfCentralBody );
 
+Eigen::Matrix3d getRotationBetweenSatelliteFrames(
+        const Eigen::Vector6d relativeInertialCartesianState,
+        const SatelliteReferenceFrames originalFrame,
+        const SatelliteReferenceFrames targetFrame );
+
 /*! Compute the rotation matrix between ITRF2014 and another ITRF frame.
  *
  * Compute the rotation matrix between ITRF2014 and another ITRF frame. According to the IERS convetions of 2010, Eq.
@@ -758,6 +773,7 @@ Eigen::Vector6d convertGroundStationStateBetweenItrfFrames(
         const std::string& baseFrame,
         const std::string& targetFrame );
 
+
 } // namespace reference_frames
 
 } // namespace tudat

include/tudat/interface/json/propagation/thrust.h

@@ -102,34 +102,6 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ThrustMagnitude
 void from_json( const nlohmann::json& jsonObject, std::shared_ptr< ThrustMagnitudeSettings >& magnitudeSettings );
 
 
-//// ThrustFrames
-
-////! Map of `ThrustFrames` string representations.
-//static std::map< ThrustFrames, std::string > thrustFrameTypes =
-//{
-//    { unspecified_thrust_frame, "unspecified" },
-//    { inertial_thurst_frame, "intertial" },
-//    { tnw_thrust_frame, "tnw" }
-//};
-
-////! `ThrustFrames` not supported by `json_interface`.
-//static std::vector< ThrustFrames > unsupportedThrustFrameTypes =
-//{
-//    unspecified_thrust_frame
-//};
-
-////! Convert `ThrustFrames` to `json`.
-//inline void to_json( nlohmann::json& jsonObject, const ThrustFrames& thrustFrameType )
-//{
-//    jsonObject = json_interface::stringFromEnum( thrustFrameType, thrustFrameTypes );
-//}
-
-////! Convert `json` to `ThrustFrames`.
-//inline void from_json( const nlohmann::json& jsonObject, ThrustFrames& thrustFrameType )
-//{
-//    thrustFrameType = json_interface::enumFromString( jsonObject, thrustFrameTypes );
-//}
-
 
 // Thrust
 

include/tudat/simulation/environment_setup/body.h

@@ -585,8 +585,6 @@ class Body {
     {
         if( !isStateSet_ )
         {
-            std::vector< double > test;
-            test.at( 0 );
             throw std::runtime_error( "Error when retrieving state from body " + bodyName_ + ", state of body is not yet defined" );
         }
         else
@@ -1689,8 +1687,6 @@ class Body {
     std::shared_ptr<ground_stations::GroundStation> getGroundStation(const std::string &stationName) const {
         if (groundStationMap.count(stationName) == 0)
         {
-            std::vector< double > a;
-            a.at( 0 );
             throw std::runtime_error("Error, station " + stationName + " does not exist");
         }
 

include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h

@@ -293,7 +293,7 @@ std::pair< std::shared_ptr< EstimationOutput< StateScalarType, TimeType > >, Eig
     {
         parameterPerturbation = Eigen::VectorXd::Zero( 7 );
     }
-    for( unsigned int i = 0; i < 7; i++ )
+    for( unsigned int i = 0; i < initialParameterEstimate.rows( ); i++ )
     {
         initialParameterEstimate( i ) += parameterPerturbation( i );
     }

include/tudat/simulation/propagation_setup/accelerationSettings.h

@@ -18,8 +18,10 @@
 #include "tudat/astro/gravitation/thirdBodyPerturbation.h"
 #include "tudat/astro/aerodynamics/aerodynamicAcceleration.h"
 #include "tudat/astro/basic_astro/accelerationModelTypes.h"
+#include "tudat/astro/reference_frames/referenceFrameTransformations.h"
 #include "tudat/basics/deprecationWarnings.h"
 #include "tudat/simulation/environment_setup/createRadiationPressureTargetModel.h"
+
 // #include "tudat/math/interpolators/createInterpolator.h"
 
 namespace tudat
@@ -490,6 +492,7 @@ class FullThrustInterpolationInterface
 
 };
 
+
 // Class for providing acceleration settings for a thrust acceleration model
 /*
  *  Class for providing acceleration settings for a thrust acceleration model. Settings for the direction and magnitude
@@ -500,6 +503,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
 {
 public:
 
+
     ThrustAccelerationSettings( const std::string& engineId ):
         AccelerationSettings( basic_astrodynamics::thrust_acceleration )
     {
@@ -528,6 +532,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
 
     bool useAllEngines_;    
 
+
     template< typename ReturnType >
     ReturnType printDeprecationError( )
     {

src/astro/propagators/propagateCovariance.cpp

@@ -104,70 +104,70 @@ void propagateCovariance(
 
 }
 
-//Eigen::MatrixXd convertCovarianceToFrame(
-//        const Eigen::MatrixXd inputCovariance,
-//        const Eigen::VectorXd inertialCartesianRelativeState,
-//        const reference_frames::SatelliteReferenceFrames inputFrame,
-//        const reference_frames::SatelliteReferenceFrames outputFrame )
-//{
-//    Eigen::MatrixXd outputCovariance;
-//    if( inertialCartesianRelativeState.rows( ) % 6 != 0 )
-//    {
-//        throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame; input size is not 6N" );
-//    }
-//    else if( ( inputCovariance.rows( ) != inertialCartesianRelativeState.rows( ) ) ||
-//             ( inputCovariance.cols( ) != inertialCartesianRelativeState.rows( ) ) )
-//    {
-//        throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame, state and covariance size don't match" );
-//    }
-//    else
-//    {
-//        int numberOfBodes = inertialCartesianRelativeState.rows( ) / 6;
-//        Eigen::MatrixXd covarianceTransformation =
-//                Eigen::MatrixXd::Zero( 6 * numberOfBodes, 6 * numberOfBodes );
-//        for( int i = 0; i < numberOfBodes; i++ )
-//        {
-//            Eigen::Matrix3d rotationMatrix = reference_frames::getRotationBetweenSatelliteFrames(
-//                        inertialCartesianRelativeState.segment( i * 6, 6 ), inputFrame, outputFrame );
-//            covarianceTransformation.block( 6 * i, 6 * i, 3, 3 ) = rotationMatrix;
-//            covarianceTransformation.block( 6 * i + 3, 6 * i + 3, 3, 3 ) = rotationMatrix;
-//        }
-//        outputCovariance = covarianceTransformation * inputCovariance * covarianceTransformation.transpose( );
-
-//    }
-//    return outputCovariance;
-//}
-
-//std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame(
-//        const std::map< double, Eigen::MatrixXd > inputCovariances,
-//        const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates,
-//        const reference_frames::SatelliteReferenceFrames inputFrame,
-//        const reference_frames::SatelliteReferenceFrames outputFrame )
-//{
-//    std::map< double, Eigen::MatrixXd > outputCovariances;
-
-//    auto covarianceIterator = inputCovariances.begin( );
-//    auto stateIterator = inertialCartesianRelativeStates.begin( );
-
-//    while( covarianceIterator != inputCovariances.end( ) )
-//    {
-//        if( stateIterator == inertialCartesianRelativeStates.end( ) )
-//        {
-//            throw std::runtime_error( "Error when converting covariance history, state history has ended before covariance history" );
-//        }
-//        if( covarianceIterator->first != stateIterator->first )
-//        {
-//            throw std::runtime_error( "Error when converting covariance history, input times are not consistent for state and covariance" );
-//        }
-//        outputCovariances[ covarianceIterator->first ] = convertCovarianceToFrame(
-//                    covarianceIterator->second, stateIterator->second,
-//                    inputFrame, outputFrame );
-//        covarianceIterator++;
-//        stateIterator++;
-//    }
-
-//    return outputCovariances;
-//}
+Eigen::MatrixXd convertCovarianceToFrame(
+        const Eigen::MatrixXd inputCovariance,
+        const Eigen::VectorXd inertialCartesianRelativeState,
+        const reference_frames::SatelliteReferenceFrames inputFrame,
+        const reference_frames::SatelliteReferenceFrames outputFrame )
+{
+    Eigen::MatrixXd outputCovariance;
+    if( inertialCartesianRelativeState.rows( ) % 6 != 0 )
+    {
+        throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame; input size is not 6N" );
+    }
+    else if( ( inputCovariance.rows( ) != inertialCartesianRelativeState.rows( ) ) ||
+             ( inputCovariance.cols( ) != inertialCartesianRelativeState.rows( ) ) )
+    {
+        throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame, state and covariance size don't match" );
+    }
+    else
+    {
+        int numberOfBodes = inertialCartesianRelativeState.rows( ) / 6;
+        Eigen::MatrixXd covarianceTransformation =
+                Eigen::MatrixXd::Zero( 6 * numberOfBodes, 6 * numberOfBodes );
+        for( int i = 0; i < numberOfBodes; i++ )
+        {
+            Eigen::Matrix3d rotationMatrix = reference_frames::getRotationBetweenSatelliteFrames(
+                        inertialCartesianRelativeState.segment( i * 6, 6 ), inputFrame, outputFrame );
+            covarianceTransformation.block( 6 * i, 6 * i, 3, 3 ) = rotationMatrix;
+            covarianceTransformation.block( 6 * i + 3, 6 * i + 3, 3, 3 ) = rotationMatrix;
+        }
+        outputCovariance = covarianceTransformation * inputCovariance * covarianceTransformation.transpose( );
+
+    }
+    return outputCovariance;
+}
+
+std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame(
+        const std::map< double, Eigen::MatrixXd > inputCovariances,
+        const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates,
+        const reference_frames::SatelliteReferenceFrames inputFrame,
+        const reference_frames::SatelliteReferenceFrames outputFrame )
+{
+    std::map< double, Eigen::MatrixXd > outputCovariances;
+
+    auto covarianceIterator = inputCovariances.begin( );
+    auto stateIterator = inertialCartesianRelativeStates.begin( );
+
+    while( covarianceIterator != inputCovariances.end( ) )
+    {
+        if( stateIterator == inertialCartesianRelativeStates.end( ) )
+        {
+            throw std::runtime_error( "Error when converting covariance history, state history has ended before covariance history" );
+        }
+        if( covarianceIterator->first != stateIterator->first )
+        {
+            throw std::runtime_error( "Error when converting covariance history, input times are not consistent for state and covariance" );
+        }
+        outputCovariances[ covarianceIterator->first ] = convertCovarianceToFrame(
+                    covarianceIterator->second, stateIterator->second,
+                    inputFrame, outputFrame );
+        covarianceIterator++;
+        stateIterator++;
+    }
+
+    return outputCovariances;
+}
 
 
 void convertCovarianceHistoryToFormalErrorHistory(
@@ -181,6 +181,7 @@ void convertCovarianceHistoryToFormalErrorHistory(
                 Eigen::VectorXd( covarianceIterator.second.diagonal( ).array( ).sqrt( ) );
     }
 }
+
 //! Function to propagate full covariance at the initial time to state formal errors at later times
 void propagateFormalErrors(
         std::map< double, Eigen::VectorXd >& propagatedFormalErrors,

src/astro/reference_frames/referenceFrameTransformations.cpp

@@ -597,12 +597,12 @@ Eigen::Matrix3d getJ2000toECLIPJ2000TransformationMatrix ()
 
 //! Get transformation matrix from ECLIPJ2000 to J2000
 Eigen::Matrix3d getECLIPJ2000toJ2000TransformationMatrix ()
-    {
-        return (Eigen::Matrix3d() <<
-                1, 0, 0,
-                0, 0.9174820620691818, -0.3977771559319137,
-                0, 0.3977771559319137, 0.9174820620691818).finished() ;
-    }
+{
+    return (Eigen::Matrix3d() <<
+            1, 0, 0,
+            0, 0.9174820620691818, -0.3977771559319137,
+            0, 0.3977771559319137, 0.9174820620691818).finished() ;
+}
 
 //! Function to compute the derivative of a rotation about the x-axis w.r.t. the rotation angle
 Eigen::Matrix3d getDerivativeOfXAxisRotationWrtAngle( const double angle )
@@ -672,6 +672,51 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
     return sphericalPosition;
 }
 
+Eigen::Matrix3d getRotationBetweenSatelliteFrames(
+        const Eigen::Vector6d relativeInertialCartesianState,
+        const SatelliteReferenceFrames originalFrame,
+        const SatelliteReferenceFrames targetFrame )
+{
+    Eigen::Matrix3d rotationMatrix;
+    if ( !( originalFrame == global_reference_frame || targetFrame == global_reference_frame ))
+    {
+        rotationMatrix = getRotationBetweenSatelliteFrames(
+            relativeInertialCartesianState, originalFrame, global_reference_frame ) *
+                         getRotationBetweenSatelliteFrames(
+                             relativeInertialCartesianState, global_reference_frame, targetFrame );
+
+    }
+    else if ( targetFrame == global_reference_frame )
+    {
+        rotationMatrix = getRotationBetweenSatelliteFrames(
+            relativeInertialCartesianState, targetFrame, originalFrame ).transpose( );
+    }
+    else if ( originalFrame == global_reference_frame )
+    {
+        switch ( targetFrame )
+        {
+        case global_reference_frame:
+            rotationMatrix = Eigen::Matrix3d::Identity( );
+            break;
+        case rsw_reference_frame:
+            rotationMatrix = getInertialToRswSatelliteCenteredFrameRotationMatrix(
+                relativeInertialCartesianState );
+            break;
+        case tnw_reference_frame:
+            rotationMatrix = getInertialToTnwRotation(
+                relativeInertialCartesianState, true );
+            break;
+        default:
+            throw std::runtime_error( "Error when converting between satellite frames, target frame not recognized" );
+        }
+    }
+    else
+    {
+        throw std::runtime_error( "Error when converting between satellite frames, could not parse frames." );
+    }
+    return rotationMatrix;
+}
+
 Eigen::Matrix3d getItrf2014ToArbitraryItrfRotationMatrix( const std::string& targetFrame )
 {
     double d = 0.0, r1 = 0.0, r2 = 0.0, r3 = 0.0;

tests/src/astro/orbit_determination/CMakeLists.txt

@@ -131,6 +131,11 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation
          ${Tudat_ESTIMATION_LIBRARIES}
  )
 
+ TUDAT_ADD_TEST_CASE(CovariancePropagation
+         PRIVATE_LINKS
+         ${Tudat_ESTIMATION_LIBRARIES}
+ )
+
 if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS )