From 9d9f6d0920551e8b52d434e18a327d0eae78986f Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Mon, 22 Nov 2021 16:17:45 +0100
Subject: [PATCH 1/9] Added covariance transformation; reused and refactored
 thrust frame

---
 .../astro/propagators/propagateCovariance.h   | 15 ++++-
 .../referenceFrameTransformations.h           | 15 +++++
 .../tudat/interface/json/propagation/thrust.h | 30 ++++-----
 .../propagation_setup/accelerationSettings.h  | 22 ++-----
 src/astro/propagators/propagateCovariance.cpp | 65 +++++++++++++++++++
 .../referenceFrameTransformations.cpp         | 58 +++++++++++++++--
 .../createAccelerationModels.cpp              | 13 ++--
 .../propulsion/unitTestThrustAcceleration.cpp |  2 +-
 8 files changed, 177 insertions(+), 43 deletions(-)

diff --git a/include/tudat/astro/propagators/propagateCovariance.h b/include/tudat/astro/propagators/propagateCovariance.h
index c7cf6772eb..e79a931240 100644
--- a/include/tudat/astro/propagators/propagateCovariance.h
+++ b/include/tudat/astro/propagators/propagateCovariance.h
@@ -11,7 +11,8 @@
 #ifndef TUDAT_PROPAGATECOVARIANCE_H
 #define TUDAT_PROPAGATECOVARIANCE_H
 
-#include<tudat/astro/propagators/stateTransitionMatrixInterface.h>
+#include <tudat/astro/reference_frames/referenceFrameTransformations.h>
+#include <tudat/astro/propagators/stateTransitionMatrixInterface.h>
 
 namespace tudat
 {
@@ -102,6 +103,18 @@ 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 );
+
 //! 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
diff --git a/include/tudat/astro/reference_frames/referenceFrameTransformations.h b/include/tudat/astro/reference_frames/referenceFrameTransformations.h
index 52b56a9bdd..184173fd98 100644
--- a/include/tudat/astro/reference_frames/referenceFrameTransformations.h
+++ b/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.
@@ -662,6 +672,11 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
         const std::function< Eigen::Vector3d( ) > positionFunctionOfRelativeBody,
         const std::function< Eigen::Quaterniond( ) > orientationFunctionOfCentralBody );
 
+Eigen::Matrix3d getRotationToFrame(
+        const Eigen::Vector6d relativeInertialCartesianState,
+        const SatelliteReferenceFrames originalFrame,
+        const SatelliteReferenceFrames targetFrame );
+
 } // namespace reference_frames
 
 } // namespace tudat
diff --git a/include/tudat/interface/json/propagation/thrust.h b/include/tudat/interface/json/propagation/thrust.h
index 982e9a4c8e..4a03b8b521 100644
--- a/include/tudat/interface/json/propagation/thrust.h
+++ b/include/tudat/interface/json/propagation/thrust.h
@@ -104,28 +104,28 @@ void from_json( const nlohmann::json& jsonObject, std::shared_ptr< ThrustMagnitu
 
 // 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
-};
+////! Map of `ThrustFrames` string representations.
+//static std::map< reference_frames::StateReferenceFrames, std::string > thrustFrameTypes =
+//{
+//    { unspecified_reference_frame, "unspecified" },
+//    { inertial_thurst_frame, "intertial" },
+//    { tnw_reference_frame, "tnw" }
+//};
+
+////! `ThrustFrames` not supported by `json_interface`.
+//static std::vector< reference_frames::StateReferenceFrames > unsupportedThrustFrameTypes =
+//{
+//    unspecified_reference_frame
+//};
 
 //! Convert `ThrustFrames` to `json`.
-inline void to_json( nlohmann::json& jsonObject, const ThrustFrames& thrustFrameType )
+inline void to_json( nlohmann::json& jsonObject, const reference_frames::SatelliteReferenceFrames& thrustFrameType )
 {
     jsonObject = json_interface::stringFromEnum( thrustFrameType, thrustFrameTypes );
 }
 
 //! Convert `json` to `ThrustFrames`.
-inline void from_json( const nlohmann::json& jsonObject, ThrustFrames& thrustFrameType )
+inline void from_json( const nlohmann::json& jsonObject, reference_frames::SatelliteReferenceFrames& thrustFrameType )
 {
     thrustFrameType = json_interface::enumFromString( jsonObject, thrustFrameTypes );
 }
diff --git a/include/tudat/simulation/propagation_setup/accelerationSettings.h b/include/tudat/simulation/propagation_setup/accelerationSettings.h
index 06d7af991e..a61718526e 100644
--- a/include/tudat/simulation/propagation_setup/accelerationSettings.h
+++ b/include/tudat/simulation/propagation_setup/accelerationSettings.h
@@ -20,6 +20,7 @@
 #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/simulation/propagation_setup/createThrustModelGuidance.h"
 // #include "tudat/math/interpolators/createInterpolator.h"
 
@@ -416,15 +417,6 @@ class FullThrustInterpolationInterface
 };
 
 
-// Enum defining identifiers of frames in which a user-specified thrust is defined.
-//! @get_docstring(ThrustFrames.__docstring__)
-enum ThrustFrames
-{
-    unspecified_thrust_frame = -1,
-    inertial_thrust_frame = 0,
-    tnw_thrust_frame = 1
-};
-
 // 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
@@ -447,7 +439,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
             AccelerationSettings( basic_astrodynamics::thrust_acceleration ),
             thrustDirectionSettings_(thrustDirectionSettings ),
             thrustMagnitudeSettings_( thrustMagnitudeSettings ),
-            thrustFrame_( unspecified_thrust_frame ){ }
+            thrustFrame_( reference_frames::unspecified_reference_frame ){ }
 
     // Constructor used for defining total thrust vector (in local or inertial frame) from interpolator using
     // variable specific impulse
@@ -461,7 +453,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
     ThrustAccelerationSettings(
             const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
             const std::function< double( const double ) > specificImpulseFunction,
-            const ThrustFrames thrustFrame = inertial_thrust_frame,
+            const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::global_reference_frame,
             const std::string centralBody = "" ):
         AccelerationSettings( basic_astrodynamics::thrust_acceleration ),
         constantSpecificImpulse_( TUDAT_NAN ), thrustFrame_( thrustFrame ),
@@ -489,7 +481,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
     ThrustAccelerationSettings(
             const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
             const double constantSpecificImpulse,
-            const ThrustFrames thrustFrame = inertial_thrust_frame,
+            const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::global_reference_frame,
             const std::string centralBody = "" ):
         ThrustAccelerationSettings( thrustForceFunction,
                                     [ = ]( const double ){ return constantSpecificImpulse; },
@@ -520,7 +512,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
      *  Identifier of frame in which thrust returned by fullThrustInterpolator is expressed. Unspecifief by default,
      *  only used if interpolatorInterface_ is set
      */
-    ThrustFrames thrustFrame_;
+    reference_frames::SatelliteReferenceFrames thrustFrame_;
 
     // Central body identifier for thrustFrame.
     /*
@@ -557,7 +549,7 @@ inline std::shared_ptr< AccelerationSettings > thrustAcceleration( const std::sh
 inline std::shared_ptr< AccelerationSettings > thrustAcceleration(
         const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
         const std::function< double( const double ) > specificImpulseFunction,
-        const ThrustFrames thrustFrame = unspecified_thrust_frame,
+        const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::unspecified_reference_frame,
 		const std::string centralBody = "" )
 {
     return std::make_shared< ThrustAccelerationSettings >( thrustForceFunction, specificImpulseFunction,
@@ -568,7 +560,7 @@ inline std::shared_ptr< AccelerationSettings > thrustAcceleration(
 inline std::shared_ptr< AccelerationSettings > thrustAcceleration(
         const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
         const double constantSpecificImpulse,
-        const ThrustFrames thrustFrame = unspecified_thrust_frame,
+        const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::unspecified_reference_frame,
 		const std::string centralBody = "" )
 {
     return std::make_shared< ThrustAccelerationSettings >( thrustForceFunction, constantSpecificImpulse,
diff --git a/src/astro/propagators/propagateCovariance.cpp b/src/astro/propagators/propagateCovariance.cpp
index 63e6c1460c..6187fd93f1 100644
--- a/src/astro/propagators/propagateCovariance.cpp
+++ b/src/astro/propagators/propagateCovariance.cpp
@@ -104,6 +104,71 @@ 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::getRotationToFrame(
+                        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;
+}
+
 //! Function to propagate full covariance at the initial time to state formal errors at later times
 void propagateFormalErrors(
         std::map< double, Eigen::VectorXd >& propagatedFormalErrors,
diff --git a/src/astro/reference_frames/referenceFrameTransformations.cpp b/src/astro/reference_frames/referenceFrameTransformations.cpp
index 232f1948b6..042659b5d6 100644
--- a/src/astro/reference_frames/referenceFrameTransformations.cpp
+++ b/src/astro/reference_frames/referenceFrameTransformations.cpp
@@ -542,12 +542,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 )
@@ -617,6 +617,52 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
     return sphericalPosition;
 }
 
+Eigen::Matrix3d getRotationToFrame(
+        const Eigen::Vector6d relativeInertialCartesianState,
+        const SatelliteReferenceFrames originalFrame,
+        const SatelliteReferenceFrames targetFrame )
+{
+    Eigen::Matrix3d rotationMatrix;
+    if( !( originalFrame == global_reference_frame || targetFrame == global_reference_frame ) )
+    {
+        rotationMatrix = getRotationToFrame(
+                    relativeInertialCartesianState, originalFrame, global_reference_frame ) *
+                getRotationToFrame(
+                    relativeInertialCartesianState, global_reference_frame, targetFrame );
+
+    }
+    else if( targetFrame == global_reference_frame )
+    {
+        rotationMatrix = getRotationToFrame(
+                    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;
+
+}
+
 } // namespace reference_frames
 
 } // namespace tudat
diff --git a/src/simulation/propagation_setup/createAccelerationModels.cpp b/src/simulation/propagation_setup/createAccelerationModels.cpp
index ffddd64d73..03d32268dd 100644
--- a/src/simulation/propagation_setup/createAccelerationModels.cpp
+++ b/src/simulation/propagation_setup/createAccelerationModels.cpp
@@ -1088,14 +1088,15 @@ createThrustAcceleratioModel(
     if( thrustAccelerationSettings->interpolatorInterface_ != nullptr )
     {
         // Check input consisten
-        if( thrustAccelerationSettings->thrustFrame_ == unspecified_thrust_frame )
+        if( thrustAccelerationSettings->thrustFrame_ == reference_frames::unspecified_reference_frame )
         {
             throw std::runtime_error( "Error when creating thrust acceleration, input frame is inconsistent with interface" );
         }
-        else if(thrustAccelerationSettings->thrustFrame_ != inertial_thrust_frame )
+        else if(thrustAccelerationSettings->thrustFrame_ != reference_frames::global_reference_frame )
         {
             // Create rotation function from thrust-frame to propagation frame.
-            if( thrustAccelerationSettings->thrustFrame_ == tnw_thrust_frame )
+            if( thrustAccelerationSettings->thrustFrame_ == reference_frames::tnw_reference_frame ||
+                    thrustAccelerationSettings->thrustFrame_ == reference_frames::rsw_reference_frame  )
             {
                 std::function< Eigen::Vector6d( ) > vehicleStateFunction =
                         std::bind( &Body::getState, bodies.at( nameOfBodyUndergoingThrust ) );
@@ -1115,8 +1116,10 @@ createThrustAcceleratioModel(
                             std::bind( &Body::getState, bodies.at( thrustAccelerationSettings->centralBody_ ) );
                 }
                 thrustAccelerationSettings->interpolatorInterface_->resetRotationFunction(
-                            std::bind( &reference_frames::getTnwToInertialRotationFromFunctions,
-                                       vehicleStateFunction, centralBodyStateFunction, true ) );
+                            [ = ]( ){ return reference_frames::getRotationToFrame(
+                                vehicleStateFunction( ) - centralBodyStateFunction( ),
+                                reference_frames::global_reference_frame,
+                                thrustAccelerationSettings->thrustFrame_ ); } );
             }
             else
             {
diff --git a/tests/src/astro/propulsion/unitTestThrustAcceleration.cpp b/tests/src/astro/propulsion/unitTestThrustAcceleration.cpp
index c46273088e..2417102d6d 100644
--- a/tests/src/astro/propulsion/unitTestThrustAcceleration.cpp
+++ b/tests/src/astro/propulsion/unitTestThrustAcceleration.cpp
@@ -1072,7 +1072,7 @@ BOOST_AUTO_TEST_CASE( testInterpolatedThrustVector )
         std::shared_ptr< ThrustAccelerationSettings > thrustSettings =
                 std::make_shared< ThrustAccelerationSettings >(
                         thrustFunction, [ & ](  const double ){ return 300.0; },
-                    testCase == 0 ? inertial_thrust_frame : tnw_thrust_frame, "Earth" );
+                    testCase == 0 ? reference_frames::global_reference_frame : reference_frames::tnw_reference_frame, "Earth" );
 
         accelerationsOfAsterix[ "Asterix" ].push_back( thrustSettings );
         accelerationMap[ "Asterix" ] = accelerationsOfAsterix;

From 807497019516e72bdb13d1707fbbbf82c429d7fc Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Mon, 22 Nov 2021 17:15:33 +0100
Subject: [PATCH 2/9] Corrected frame order error

---
 src/simulation/propagation_setup/createAccelerationModels.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/simulation/propagation_setup/createAccelerationModels.cpp b/src/simulation/propagation_setup/createAccelerationModels.cpp
index 03d32268dd..40e89d868e 100644
--- a/src/simulation/propagation_setup/createAccelerationModels.cpp
+++ b/src/simulation/propagation_setup/createAccelerationModels.cpp
@@ -1118,8 +1118,8 @@ createThrustAcceleratioModel(
                 thrustAccelerationSettings->interpolatorInterface_->resetRotationFunction(
                             [ = ]( ){ return reference_frames::getRotationToFrame(
                                 vehicleStateFunction( ) - centralBodyStateFunction( ),
-                                reference_frames::global_reference_frame,
-                                thrustAccelerationSettings->thrustFrame_ ); } );
+                                thrustAccelerationSettings->thrustFrame_,
+                                reference_frames::global_reference_frame ); };
             }
             else
             {

From a32bdce8dadf50bee1b5a5dc7f61345e0dbf996f Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Mon, 22 Nov 2021 19:42:05 +0100
Subject: [PATCH 3/9] Corrected frame order error

---
 src/simulation/propagation_setup/createAccelerationModels.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/simulation/propagation_setup/createAccelerationModels.cpp b/src/simulation/propagation_setup/createAccelerationModels.cpp
index 40e89d868e..bebec802b4 100644
--- a/src/simulation/propagation_setup/createAccelerationModels.cpp
+++ b/src/simulation/propagation_setup/createAccelerationModels.cpp
@@ -1119,7 +1119,7 @@ createThrustAcceleratioModel(
                             [ = ]( ){ return reference_frames::getRotationToFrame(
                                 vehicleStateFunction( ) - centralBodyStateFunction( ),
                                 thrustAccelerationSettings->thrustFrame_,
-                                reference_frames::global_reference_frame ); };
+                                reference_frames::global_reference_frame ); } );
             }
             else
             {

From ffa67bef2cc6b994e2be1032b32a91d5096a9f65 Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Fri, 26 Nov 2021 16:14:42 +0100
Subject: [PATCH 4/9] Added unit test for generalized frame comversions

---
 .../referenceFrameTransformations.h           |  2 +-
 src/astro/propagators/propagateCovariance.cpp |  2 +-
 .../referenceFrameTransformations.cpp         |  8 ++--
 .../createAccelerationModels.cpp              |  2 +-
 .../unitTestReferenceFrameTransformations.cpp | 44 +++++++++++++++++--
 5 files changed, 48 insertions(+), 10 deletions(-)

diff --git a/include/tudat/astro/reference_frames/referenceFrameTransformations.h b/include/tudat/astro/reference_frames/referenceFrameTransformations.h
index 184173fd98..d651b7370d 100644
--- a/include/tudat/astro/reference_frames/referenceFrameTransformations.h
+++ b/include/tudat/astro/reference_frames/referenceFrameTransformations.h
@@ -672,7 +672,7 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
         const std::function< Eigen::Vector3d( ) > positionFunctionOfRelativeBody,
         const std::function< Eigen::Quaterniond( ) > orientationFunctionOfCentralBody );
 
-Eigen::Matrix3d getRotationToFrame(
+Eigen::Matrix3d getRotationBetweenSatelliteFrames(
         const Eigen::Vector6d relativeInertialCartesianState,
         const SatelliteReferenceFrames originalFrame,
         const SatelliteReferenceFrames targetFrame );
diff --git a/src/astro/propagators/propagateCovariance.cpp b/src/astro/propagators/propagateCovariance.cpp
index 6187fd93f1..c375068501 100644
--- a/src/astro/propagators/propagateCovariance.cpp
+++ b/src/astro/propagators/propagateCovariance.cpp
@@ -127,7 +127,7 @@ Eigen::MatrixXd convertCovarianceToFrame(
                 Eigen::MatrixXd::Zero( 6 * numberOfBodes, 6 * numberOfBodes );
         for( int i = 0; i < numberOfBodes; i++ )
         {
-            Eigen::Matrix3d rotationMatrix = reference_frames::getRotationToFrame(
+            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;
diff --git a/src/astro/reference_frames/referenceFrameTransformations.cpp b/src/astro/reference_frames/referenceFrameTransformations.cpp
index 042659b5d6..72e8fc61cf 100644
--- a/src/astro/reference_frames/referenceFrameTransformations.cpp
+++ b/src/astro/reference_frames/referenceFrameTransformations.cpp
@@ -617,7 +617,7 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
     return sphericalPosition;
 }
 
-Eigen::Matrix3d getRotationToFrame(
+Eigen::Matrix3d getRotationBetweenSatelliteFrames(
         const Eigen::Vector6d relativeInertialCartesianState,
         const SatelliteReferenceFrames originalFrame,
         const SatelliteReferenceFrames targetFrame )
@@ -625,15 +625,15 @@ Eigen::Matrix3d getRotationToFrame(
     Eigen::Matrix3d rotationMatrix;
     if( !( originalFrame == global_reference_frame || targetFrame == global_reference_frame ) )
     {
-        rotationMatrix = getRotationToFrame(
+        rotationMatrix = getRotationBetweenSatelliteFrames(
                     relativeInertialCartesianState, originalFrame, global_reference_frame ) *
-                getRotationToFrame(
+                getRotationBetweenSatelliteFrames(
                     relativeInertialCartesianState, global_reference_frame, targetFrame );
 
     }
     else if( targetFrame == global_reference_frame )
     {
-        rotationMatrix = getRotationToFrame(
+        rotationMatrix = getRotationBetweenSatelliteFrames(
                     relativeInertialCartesianState, targetFrame, originalFrame ).transpose( );
     }
     else if( originalFrame == global_reference_frame )
diff --git a/src/simulation/propagation_setup/createAccelerationModels.cpp b/src/simulation/propagation_setup/createAccelerationModels.cpp
index a973ec8b4c..51621af369 100644
--- a/src/simulation/propagation_setup/createAccelerationModels.cpp
+++ b/src/simulation/propagation_setup/createAccelerationModels.cpp
@@ -1116,7 +1116,7 @@ createThrustAcceleratioModel(
                             std::bind( &Body::getState, bodies.at( thrustAccelerationSettings->centralBody_ ) );
                 }
                 thrustAccelerationSettings->interpolatorInterface_->resetRotationFunction(
-                            [ = ]( ){ return reference_frames::getRotationToFrame(
+                            [ = ]( ){ return reference_frames::getRotationBetweenSatelliteFrames(
                                 vehicleStateFunction( ) - centralBodyStateFunction( ),
                                 thrustAccelerationSettings->thrustFrame_,
                                 reference_frames::global_reference_frame ); } );
diff --git a/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp b/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp
index da683e4689..05c7c7a8b6 100644
--- a/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp
+++ b/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp
@@ -37,7 +37,8 @@ namespace tudat
 namespace unit_tests
 {
 
-using namespace unit_conversions;
+using namespace tudat::unit_conversions;
+using namespace tudat::reference_frames;
 
 BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations )
 
@@ -815,7 +816,7 @@ BOOST_AUTO_TEST_CASE( testVelocityBasedtnwFrameTransformations )
 
         // Compute angle between positive T axis and negative y axis
         double angle = std::atan2( ( vehicleStateCartesian( 3 ) - centralBodyStateCartesian( 3 ) ),
-                              ( vehicleStateCartesian( 4 ) - centralBodyStateCartesian( 4 ) ) );
+                                   ( vehicleStateCartesian( 4 ) - centralBodyStateCartesian( 4 ) ) );
 
         // Declare the expected thrust vector in the inertial (I) reference frame.
         Eigen::Vector3d expectedThrustVector;
@@ -884,7 +885,44 @@ BOOST_AUTO_TEST_CASE( testVelocityBasedtnwFrameTransformations )
     }
 }
 
-
+BOOST_AUTO_TEST_CASE( testAutomaticConversionFunctions )
+{
+    Eigen::Vector6d inertialCartesianState;
+    inertialCartesianState << 4.0E6, 5.0E6, -300.0, -2.0E3, 6.0E3, 20.0;
+
+    Eigen::Matrix3d manualInertialToRsw = getInertialToRswSatelliteCenteredFrameRotationMatrix(
+                inertialCartesianState );
+    Eigen::Matrix3d automaticInertialToRsw  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, global_reference_frame, rsw_reference_frame );
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualInertialToRsw, automaticInertialToRsw, std::numeric_limits< double >::epsilon( ) );
+
+    Eigen::Matrix3d manualRswToInertial = getRswSatelliteCenteredToInertialFrameRotationMatrix(
+                inertialCartesianState );
+    Eigen::Matrix3d automaticRswToInertial  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, rsw_reference_frame, global_reference_frame );
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualRswToInertial, automaticRswToInertial, std::numeric_limits< double >::epsilon( ) );
+
+    Eigen::Matrix3d manualInertialToTnw = getInertialToTnwRotation(
+                inertialCartesianState );
+    Eigen::Matrix3d automaticInertialToTnw  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, global_reference_frame, tnw_reference_frame );
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualInertialToTnw, automaticInertialToTnw, std::numeric_limits< double >::epsilon( ) );
+
+    Eigen::Matrix3d manualTnwToInertial = getTnwToInertialRotation(
+                inertialCartesianState );
+    Eigen::Matrix3d automaticTnwToInertial  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, tnw_reference_frame, global_reference_frame );
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualTnwToInertial, automaticTnwToInertial, std::numeric_limits< double >::epsilon( ) );
+
+
+    Eigen::Matrix3d automaticTnwToRsw  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, tnw_reference_frame, rsw_reference_frame );
+    Eigen::Matrix3d automaticRswToTnw  = getRotationBetweenSatelliteFrames(
+                inertialCartesianState, rsw_reference_frame, tnw_reference_frame );
+
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( automaticTnwToRsw, ( automaticRswToTnw.transpose( ) ), ( 5.0 * std::numeric_limits< double >::epsilon( ) ) );
+    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( automaticTnwToRsw, Eigen::Matrix3d( manualTnwToInertial * manualInertialToRsw ), std::numeric_limits< double >::epsilon( ) );
+}
 
 BOOST_AUTO_TEST_SUITE_END( )
 

From fbf1a347ad235570d1ed65505f8e3b8d4789b9e3 Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Fri, 26 Nov 2021 20:17:28 +0100
Subject: [PATCH 5/9] Added proper saving/extraction of state histories and
 dependent variables

---
 .../orbit_determination/podInputOutputTypes.h | 32 ++++++++++-
 .../orbitDeterminationManager.h               | 53 ++++++++++---------
 2 files changed, 57 insertions(+), 28 deletions(-)

diff --git a/include/tudat/astro/orbit_determination/podInputOutputTypes.h b/include/tudat/astro/orbit_determination/podInputOutputTypes.h
index 60288ea9d1..03e0614fee 100644
--- a/include/tudat/astro/orbit_determination/podInputOutputTypes.h
+++ b/include/tudat/astro/orbit_determination/podInputOutputTypes.h
@@ -553,16 +553,44 @@ struct PodOutput
     {
         return normalizedInformationMatrix_;
     }
-    // Michael
+
     std::vector< std::vector< std::map< TimeType, Eigen::VectorXd > > > getDependentVariableHistory( )
     {
+        if( dependentVariableHistoryPerIteration_.size( ) == 0 )
+        {
+            throw std::runtime_error( "Error when retrieving dependent variable histories from estimation, no dependent variable histories set." );
+        }
         return dependentVariableHistoryPerIteration_;
     }
-    // Michael
+
     std::vector< std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > > > getDynamicsHistory( )
     {
+        if( dynamicsHistoryPerIteration_.size( ) == 0 )
+        {
+            throw std::runtime_error( "Error when retrieving dynamics histories from estimation, no dynamics results set." );
+        }
         return dynamicsHistoryPerIteration_;
     }
+
+    std::vector< std::map< TimeType, Eigen::VectorXd > > getFinalDependentVariableHistory( )
+    {
+        if( dependentVariableHistoryPerIteration_.size( ) == 0 )
+        {
+            throw std::runtime_error( "Error when retrieving final dependent variable histories from estimation, no dependent variable histories set." );
+        }
+        return dependentVariableHistoryPerIteration_.at( dependentVariableHistoryPerIteration_.size( ) - 1 );
+    }
+
+    std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > > getFinalDynamicsHistory( )
+    {
+        if( dynamicsHistoryPerIteration_.size( ) == 0 )
+        {
+            throw std::runtime_error( "Error when retrieving final dynamics histories from estimation, no dynamics results set." );
+        }
+        return dynamicsHistoryPerIteration_.at( dynamicsHistoryPerIteration_.size( ) - 1 );
+    }
+
+
     Eigen::MatrixXd getNormalizedWeightedInformationMatrix( )
     {
         Eigen::MatrixXd weightedNormalizedInformationMatrix = normalizedInformationMatrix_;
diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
index 7dca869c3c..974945a3d1 100644
--- a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
+++ b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
@@ -431,7 +431,7 @@ class OrbitDeterminationManager
         int numberOfIterations = 0;
         do
         {
-//            try
+            try
             {
                 // Re-integrate equations of motion and variational equations with new parameter estimate.
                 if( ( numberOfIterations > 0 ) ||( podInput->getReintegrateEquationsOnFirstIteration( ) ) )
@@ -439,16 +439,7 @@ class OrbitDeterminationManager
                     resetParameterEstimate( newParameterEstimate, podInput->getReintegrateVariationalEquations( ) );
                 }
 
-/*
-                if( podInput->getSaveStateHistoryForEachIteration( ) )
-                {
-                    dynamicsHistoryPerIteration.push_back(
-                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ) );
-                    dependentVariableHistoryPerIteration.push_back(
-                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ) );
-                }*/
 
-                // Michael
                 if( podInput->getSaveStateHistoryForEachIteration( ) )
                 {
                     if( std::dynamic_pointer_cast< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > >(  variationalEquationsSolver_) != nullptr )
@@ -457,27 +448,37 @@ class OrbitDeterminationManager
                         std::shared_ptr< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > > hybridArcSolver =
                                 std::dynamic_pointer_cast< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > >(  variationalEquationsSolver_);
 
-                    dynamicsHistoryPerIteration.push_back(
-                            hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ));
-                    dependentVariableHistoryPerIteration.push_back(
-                        hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ));
-
-                       /* dynamicsHistoryPerIteration.push_back(
-                                hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ));
+                        std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > >  currentStateHistories;
+                        currentStateHistories = hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( );
+                        currentStateHistories.insert(
+                                    currentStateHistories.begin(),
+                                    hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ).at( 0 ) );
+                        dynamicsHistoryPerIteration.push_back( currentStateHistories );
+
+                        std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > >  currentDependentVariableHistories;
+                        currentDependentVariableHistories = hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( );
+                        currentDependentVariableHistories.insert(
+                                    currentDependentVariableHistories.begin(),
+                                    hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ).at( 0 ) );
+                        dependentVariableHistoryPerIteration.push_back( currentDependentVariableHistories );
+                    }
+                    else
+                    {
+                        dynamicsHistoryPerIteration.push_back(
+                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ));
                         dependentVariableHistoryPerIteration.push_back(
-                                hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ));
-*/
+                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ));
                     }
                 }
 
             }
-//            catch( std::runtime_error& error )
-//            {
-//                std::cerr<<"Error when resetting parameters during parameter estimation: "<<std::endl<<
-//                           error.what( )<<std::endl<<"Terminating estimation"<<std::endl;
-//                exceptionDuringPropagation = true;
-//                break;
-//            }
+            catch( std::runtime_error& error )
+            {
+                std::cerr<<"Error when resetting parameters during parameter estimation: "<<std::endl<<
+                           error.what( )<<std::endl<<"Terminating estimation"<<std::endl;
+                exceptionDuringPropagation = true;
+                break;
+            }
 
             oldParameterEstimate = newParameterEstimate;
 

From 9538617ccfe377127dd059c05f2d945b23f0aebb Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Wed, 18 Sep 2024 20:10:05 +0200
Subject: [PATCH 6/9] Merged headers

---
 .../astro/propagators/propagateCovariance.h   |  15 +--
 .../referenceFrameTransformations.h           |   5 +-
 .../orbitDeterminationManager.h               |  52 ---------
 .../propagation_setup/accelerationSettings.h  | 110 +-----------------
 4 files changed, 5 insertions(+), 177 deletions(-)

diff --git a/include/tudat/astro/propagators/propagateCovariance.h b/include/tudat/astro/propagators/propagateCovariance.h
index 608a8e0c39..83f653b682 100644
--- a/include/tudat/astro/propagators/propagateCovariance.h
+++ b/include/tudat/astro/propagators/propagateCovariance.h
@@ -103,7 +103,6 @@ void propagateCovariance(
         const double initialTime,
         const double finalTime );
 
-<<<<<<< HEAD
 Eigen::MatrixXd convertCovarianceToFrame(
         const Eigen::MatrixXd inputCovariance,
         const Eigen::VectorXd inertialCartesianRelativeState,
@@ -115,23 +114,11 @@ std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame(
         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 );
->>>>>>> origin/develop
+
 
 //! Function to propagate full covariance at the initial time to state formal errors at later times
 /*!
diff --git a/include/tudat/astro/reference_frames/referenceFrameTransformations.h b/include/tudat/astro/reference_frames/referenceFrameTransformations.h
index 1e2c78e74f..f723366a48 100644
--- a/include/tudat/astro/reference_frames/referenceFrameTransformations.h
+++ b/include/tudat/astro/reference_frames/referenceFrameTransformations.h
@@ -688,12 +688,11 @@ Eigen::Vector3d getBodyFixedSphericalPosition(
         const std::function< Eigen::Vector3d( ) > positionFunctionOfRelativeBody,
         const std::function< Eigen::Quaterniond( ) > orientationFunctionOfCentralBody );
 
-<<<<<<< HEAD
 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.
@@ -773,7 +772,7 @@ Eigen::Vector6d convertGroundStationStateBetweenItrfFrames(
         double epoch,
         const std::string& baseFrame,
         const std::string& targetFrame );
->>>>>>> origin/develop
+
 
 } // namespace reference_frames
 
diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
index c7ba1fdc75..f6f61487e4 100644
--- a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
+++ b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h
@@ -742,58 +742,6 @@ class OrbitDeterminationManager
         int numberOfIterations = 0;
         while( true )
         {
-<<<<<<< HEAD
-            try
-            {
-                // Re-integrate equations of motion and variational equations with new parameter estimate.
-                if( ( numberOfIterations > 0 ) ||( podInput->getReintegrateEquationsOnFirstIteration( ) ) )
-                {
-                    resetParameterEstimate( newParameterEstimate, podInput->getReintegrateVariationalEquations( ) );
-                }
-
-
-                if( podInput->getSaveStateHistoryForEachIteration( ) )
-                {
-                    if( std::dynamic_pointer_cast< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > >(  variationalEquationsSolver_) != nullptr )
-                    {
-
-                        std::shared_ptr< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > > hybridArcSolver =
-                                std::dynamic_pointer_cast< propagators::HybridArcVariationalEquationsSolver< ObservationScalarType, TimeType > >(  variationalEquationsSolver_);
-
-                        std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > >  currentStateHistories;
-                        currentStateHistories = hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( );
-                        currentStateHistories.insert(
-                                    currentStateHistories.begin(),
-                                    hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ).at( 0 ) );
-                        dynamicsHistoryPerIteration.push_back( currentStateHistories );
-
-                        std::vector< std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > >  currentDependentVariableHistories;
-                        currentDependentVariableHistories = hybridArcSolver->getMultiArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( );
-                        currentDependentVariableHistories.insert(
-                                    currentDependentVariableHistories.begin(),
-                                    hybridArcSolver->getSingleArcSolver( )->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ).at( 0 ) );
-                        dependentVariableHistoryPerIteration.push_back( currentDependentVariableHistories );
-                    }
-                    else
-                    {
-                        dynamicsHistoryPerIteration.push_back(
-                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getEquationsOfMotionNumericalSolutionBase( ));
-                        dependentVariableHistoryPerIteration.push_back(
-                                variationalEquationsSolver_->getDynamicsSimulatorBase( )->getDependentVariableNumericalSolutionBase( ));
-                    }
-                }
-
-            }
-            catch( std::runtime_error& error )
-            {
-                std::cerr<<"Error when resetting parameters during parameter estimation: "<<std::endl<<
-                           error.what( )<<std::endl<<"Terminating estimation"<<std::endl;
-                exceptionDuringPropagation = true;
-                break;
-            }
-
-=======
->>>>>>> origin/develop
             oldParameterEstimate = newParameterEstimate;
             newFullParameterEstimate.segment( 0, numberEstimatedParameters_ ) = newParameterEstimate;
             if ( considerParametersIncluded_ )
diff --git a/include/tudat/simulation/propagation_setup/accelerationSettings.h b/include/tudat/simulation/propagation_setup/accelerationSettings.h
index 6ecc625acd..670d292d6b 100644
--- a/include/tudat/simulation/propagation_setup/accelerationSettings.h
+++ b/include/tudat/simulation/propagation_setup/accelerationSettings.h
@@ -18,13 +18,11 @@
 #include "tudat/astro/gravitation/thirdBodyPerturbation.h"
 #include "tudat/astro/aerodynamics/aerodynamicAcceleration.h"
 #include "tudat/astro/basic_astro/accelerationModelTypes.h"
-<<<<<<< HEAD
 #include "tudat/astro/reference_frames/referenceFrameTransformations.h"
 #include "tudat/simulation/propagation_setup/createThrustModelGuidance.h"
-=======
 #include "tudat/basics/deprecationWarnings.h"
 #include "tudat/simulation/environment_setup/createRadiationPressureTargetModel.h"
->>>>>>> origin/develop
+
 // #include "tudat/math/interpolators/createInterpolator.h"
 
 namespace tudat
@@ -495,10 +493,7 @@ class FullThrustInterpolationInterface
 
 };
 
-<<<<<<< HEAD
 
-=======
->>>>>>> origin/develop
 // 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
@@ -509,76 +504,19 @@ class ThrustAccelerationSettings: public AccelerationSettings
 {
 public:
 
-<<<<<<< HEAD
-    // Constructor from separate magnitude and diretion settings.
-    /*
-     * Constructor from separate magnitude and diretion settings.
-     * \param thrustDirectionSettings Settings for the direction of the thrust
-     * \param thrustMagnitudeSettings Settings for the magnitude of the thrust
-     */
-    ThrustAccelerationSettings(
-            const std::shared_ptr< ThrustDirectionSettings > thrustDirectionSettings,
-            const std::shared_ptr< ThrustMagnitudeSettings > thrustMagnitudeSettings ):
-            AccelerationSettings( basic_astrodynamics::thrust_acceleration ),
-            thrustDirectionSettings_(thrustDirectionSettings ),
-            thrustMagnitudeSettings_( thrustMagnitudeSettings ),
-            thrustFrame_( reference_frames::unspecified_reference_frame ){ }
-
-    // Constructor used for defining total thrust vector (in local or inertial frame) from interpolator using
-    // variable specific impulse
-    /*
-     * Constructor used for defining total thrust vector (in local or inertial frame) from interpolator using
-     * variable specific impulse
-     * \param specificImpulseFunction Function returning the specific impulse as a function of time
-     * \param thrustFrame Identifier of frame in which thrust returned by fullThrustInterpolator is expressed
-     * \param centralBody Central body identifier for thrustFrame (if needed; empty by default).
-     */
-    ThrustAccelerationSettings(
-            const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
-            const std::function< double( const double ) > specificImpulseFunction,
-            const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::global_reference_frame,
-            const std::string centralBody = "" ):
-        AccelerationSettings( basic_astrodynamics::thrust_acceleration ),
-        constantSpecificImpulse_( TUDAT_NAN ), thrustFrame_( thrustFrame ),
-        centralBody_( centralBody )
-=======
+
     ThrustAccelerationSettings( const std::string& engineId ):
         AccelerationSettings( basic_astrodynamics::thrust_acceleration )
->>>>>>> origin/develop
     {
         engineIds_.push_back( engineId );
         useAllEngines_ = false;
     }
 
-<<<<<<< HEAD
-    // Constructor used for defining total thrust vector (in local or inertial frame) from interpolator using constant
-    // specific impulse
-    /*
-     * Constructor used for defining total thrust vector (in local or inertial frame) from interpolator using constant
-     * specific impulse
-     * \param constantSpecificImpulse Constant specific impulse
-     * \param thrustFrame Identifier of frame in which thrust returned by fullThrustInterpolator is expressed
-     * \param centralBody Central body identifier for thrustFrame (if needed; empty by default).
-     */
-    ThrustAccelerationSettings(
-            const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
-            const double constantSpecificImpulse,
-            const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::global_reference_frame,
-            const std::string centralBody = "" ):
-        ThrustAccelerationSettings( thrustForceFunction,
-                                    [ = ]( const double ){ return constantSpecificImpulse; },
-    thrustFrame,
-    centralBody )
-    {
-        constantSpecificImpulse_ = constantSpecificImpulse;
-    }
-=======
    ThrustAccelerationSettings( const std::vector< std::string >& engineIds ):
             AccelerationSettings( basic_astrodynamics::thrust_acceleration ), engineIds_( engineIds )
    {
         useAllEngines_ = false;
    }
->>>>>>> origin/develop
 
    ThrustAccelerationSettings( ):
        AccelerationSettings( basic_astrodynamics::thrust_acceleration )
@@ -595,34 +533,7 @@ class ThrustAccelerationSettings: public AccelerationSettings
 
     bool useAllEngines_;    
 
-<<<<<<< HEAD
-    // Settings for the magnitude of the thrust
-    std::shared_ptr< ThrustMagnitudeSettings > thrustMagnitudeSettings_;
-
-    // Constant specific impulse used when determining the direction and magnitude of thrust from an interpolator.
-    // NaN if the specific impulse is not constant (i.e. is defined using a std::function).
-    double constantSpecificImpulse_ = TUDAT_NAN;
-
-    // Identifier of frame in which thrust returned by fullThrustInterpolator is expressed.
-    /*
-     *  Identifier of frame in which thrust returned by fullThrustInterpolator is expressed. Unspecifief by default,
-     *  only used if interpolatorInterface_ is set
-     */
-    reference_frames::SatelliteReferenceFrames thrustFrame_;
-
-    // Central body identifier for thrustFrame.
-    /*
-     *  Central body identifier for thrustFrame. Empty by default,
-     *  only used if interpolatorInterface_ is set
-     */
-    std::string centralBody_;
 
-    // Settings to create the interpolator interface
-    std::shared_ptr< interpolators::DataInterpolationSettings< double, Eigen::Vector3d > > dataInterpolationSettings_;
-
-    // Interface object used when full thrust (direction and magnitude) are defined by a single user-supplied interpolation.
-    std::shared_ptr< FullThrustInterpolationInterface > interpolatorInterface_;
-=======
     template< typename ReturnType >
     ReturnType printDeprecationError( )
     {
@@ -631,7 +542,6 @@ class ThrustAccelerationSettings: public AccelerationSettings
                     "https://docs.tudat.space/en/stable/_src_user_guide/state_propagation/environment_setup/thrust_refactor/thrust_refactor.html#thrust-acceleration" );
         return nullptr;
     }
->>>>>>> origin/develop
 
 };
 
@@ -645,29 +555,13 @@ inline Eigen::Vector3d applyAccelerationScalingFunction(
 
 //! @get_docstring(thrustAcceleration, 1)
 inline std::shared_ptr< AccelerationSettings > thrustAcceleration(
-<<<<<<< HEAD
-        const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
-        const std::function< double( const double ) > specificImpulseFunction,
-        const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::unspecified_reference_frame,
-		const std::string centralBody = "" )
-=======
         const std::vector< std::string >& engineIds )
->>>>>>> origin/develop
 {
     return std::make_shared< ThrustAccelerationSettings >( engineIds );
 }
 
-<<<<<<< HEAD
-//! @get_docstring(thrustAcceleration, 3)
-inline std::shared_ptr< AccelerationSettings > thrustAcceleration(
-        const std::function< Eigen::Vector3d( const double ) > thrustForceFunction,
-        const double constantSpecificImpulse,
-        const reference_frames::SatelliteReferenceFrames thrustFrame = reference_frames::unspecified_reference_frame,
-		const std::string centralBody = "" )
-=======
 inline std::shared_ptr< AccelerationSettings > thrustAccelerationFromSingleEngine(
         const std::string& engineId )
->>>>>>> origin/develop
 {
     return std::make_shared< ThrustAccelerationSettings >( std::vector< std::string >( { engineId } ) );
 }

From 4a2210f20ebaabdaeeddded52341e8eb7e65a8eb Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Wed, 18 Sep 2024 21:20:06 +0200
Subject: [PATCH 7/9] Removed incorrect include

---
 .../tudat/interface/json/propagation/thrust.h | 55 -------------------
 .../propagation_setup/accelerationSettings.h  |  1 -
 2 files changed, 56 deletions(-)

diff --git a/include/tudat/interface/json/propagation/thrust.h b/include/tudat/interface/json/propagation/thrust.h
index 9adb112bd0..aeb84ee3e3 100644
--- a/include/tudat/interface/json/propagation/thrust.h
+++ b/include/tudat/interface/json/propagation/thrust.h
@@ -102,61 +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.
-<<<<<<< HEAD
-//static std::map< reference_frames::StateReferenceFrames, std::string > thrustFrameTypes =
-//{
-//    { unspecified_reference_frame, "unspecified" },
-//    { inertial_thurst_frame, "intertial" },
-//    { tnw_reference_frame, "tnw" }
-//};
-
-////! `ThrustFrames` not supported by `json_interface`.
-//static std::vector< reference_frames::StateReferenceFrames > unsupportedThrustFrameTypes =
-//{
-//    unspecified_reference_frame
-//};
-
-//! Convert `ThrustFrames` to `json`.
-inline void to_json( nlohmann::json& jsonObject, const reference_frames::SatelliteReferenceFrames& thrustFrameType )
-{
-    jsonObject = json_interface::stringFromEnum( thrustFrameType, thrustFrameTypes );
-}
-
-//! Convert `json` to `ThrustFrames`.
-inline void from_json( const nlohmann::json& jsonObject, reference_frames::SatelliteReferenceFrames& thrustFrameType )
-{
-    thrustFrameType = json_interface::enumFromString( jsonObject, thrustFrameTypes );
-}
-=======
-//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 );
-//}
->>>>>>> origin/develop
-
 
 // Thrust
 
diff --git a/include/tudat/simulation/propagation_setup/accelerationSettings.h b/include/tudat/simulation/propagation_setup/accelerationSettings.h
index 670d292d6b..f0b3dd9a11 100644
--- a/include/tudat/simulation/propagation_setup/accelerationSettings.h
+++ b/include/tudat/simulation/propagation_setup/accelerationSettings.h
@@ -19,7 +19,6 @@
 #include "tudat/astro/aerodynamics/aerodynamicAcceleration.h"
 #include "tudat/astro/basic_astro/accelerationModelTypes.h"
 #include "tudat/astro/reference_frames/referenceFrameTransformations.h"
-#include "tudat/simulation/propagation_setup/createThrustModelGuidance.h"
 #include "tudat/basics/deprecationWarnings.h"
 #include "tudat/simulation/environment_setup/createRadiationPressureTargetModel.h"
 

From fc9d928595f13f6af801f3278461035014d4bf11 Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Wed, 18 Sep 2024 21:28:36 +0200
Subject: [PATCH 8/9] Silencing warnings

---
 include/tudat/simulation/environment_setup/body.h | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/include/tudat/simulation/environment_setup/body.h b/include/tudat/simulation/environment_setup/body.h
index 2911b0f00f..633d290305 100644
--- a/include/tudat/simulation/environment_setup/body.h
+++ b/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");
         }
 

From 58f87f368582c146d59555d459943a2d7197ae67 Mon Sep 17 00:00:00 2001
From: Dominic Dirkx <d.dirkx@tudelft.nl>
Date: Thu, 19 Sep 2024 15:27:29 +0200
Subject: [PATCH 9/9] Added tests for covariance propagation

---
 .../orbitDeterminationTestCases.h             |   2 +-
 .../astro/orbit_determination/CMakeLists.txt  |   5 +
 .../unitTestCovariancePropagation.cpp         | 303 ++++++++++++++++++
 3 files changed, 309 insertions(+), 1 deletion(-)
 create mode 100644 tests/src/astro/orbit_determination/unitTestCovariancePropagation.cpp

diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h b/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h
index 7a1064b0e1..e7ab45dd2e 100644
--- a/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h
+++ b/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 );
     }
diff --git a/tests/src/astro/orbit_determination/CMakeLists.txt b/tests/src/astro/orbit_determination/CMakeLists.txt
index 5bd0ec883e..16bb74f8bf 100644
--- a/tests/src/astro/orbit_determination/CMakeLists.txt
+++ b/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 )
 
 
diff --git a/tests/src/astro/orbit_determination/unitTestCovariancePropagation.cpp b/tests/src/astro/orbit_determination/unitTestCovariancePropagation.cpp
new file mode 100644
index 0000000000..a757fb5f61
--- /dev/null
+++ b/tests/src/astro/orbit_determination/unitTestCovariancePropagation.cpp
@@ -0,0 +1,303 @@
+/*    Copyright (c) 2010-2019, Delft University of Technology
+ *    All rigths reserved
+ *
+ *    This file is part of the Tudat. Redistribution and use in source and
+ *    binary forms, with or without modification, are permitted exclusively
+ *    under the terms of the Modified BSD license. You should have received
+ *    a copy of the license with this file. If not, please or visit:
+ *    http://tudat.tudelft.nl/LICENSE.
+ */
+
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN
+
+
+#include <limits>
+
+#include <boost/test/unit_test.hpp>
+
+#include "tudat/basics/testMacros.h"
+
+#include "tudat/simulation/estimation_setup/orbitDeterminationTestCases.h"
+#include "tudat/simulation/estimation_setup/podProcessing.h"
+#include "tudat/astro/propagators/propagateCovariance.h"
+
+namespace tudat
+{
+namespace unit_tests
+{
+BOOST_AUTO_TEST_SUITE( test_covariance_propagation )
+
+BOOST_AUTO_TEST_CASE( test_EstimationInputAndOutput )
+{
+    const double startTime = 1.0E7;
+
+    //Load spice kernels.
+    spice_interface::loadStandardSpiceKernels( );
+
+    for( int test = 0; test < 2; test++ )
+    {
+        // Define bodies in simulation
+        std::vector< std::string > bodyNames;
+        bodyNames.push_back( "Earth" );
+        bodyNames.push_back( "Sun" );
+        bodyNames.push_back( "Moon" );
+        bodyNames.push_back( "Mars" );
+
+        // Specify initial time
+        double initialEphemerisTime = startTime;
+        double finalEphemerisTime = initialEphemerisTime + 4.0 * 3600.0;
+
+        // Create bodies needed in simulation
+        BodyListSettings bodySettings =
+            getDefaultBodySettings( bodyNames, "Earth", "ECLIPJ2000" );
+        bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >(
+            "ECLIPJ2000", "IAU_Earth",
+            spice_interface::computeRotationQuaternionBetweenFrames(
+                "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ),
+            initialEphemerisTime, 2.0 * mathematical_constants::PI /
+                                  ( physical_constants::JULIAN_DAY ) );
+
+        SystemOfBodies bodies = createSystemOfBodies( bodySettings );
+        bodies.createEmptyBody( "Vehicle" );
+        bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 );
+
+        // Create aerodynamic coefficient interface settings.
+        double referenceArea = 4.0;
+        double aerodynamicCoefficient = 1.2;
+        std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings =
+            std::make_shared< ConstantAerodynamicCoefficientSettings >(
+                referenceArea, aerodynamicCoefficient * ( Eigen::Vector3d( ) << 1.2, -0.1, -0.4 ).finished( ),
+                negative_aerodynamic_frame_coefficients );
+
+        // Create and set aerodynamic coefficnumberOfDaysOfDataients object
+        bodies.at( "Vehicle" )->setAerodynamicCoefficientInterface(
+            createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle", bodies ) );
+
+        // Create radiation pressure settings
+        double referenceAreaRadiation = 4.0;
+        double radiationPressureCoefficient = 1.2;
+        std::vector< std::string > occultingBodies;
+        occultingBodies.push_back( "Earth" );
+        std::shared_ptr< RadiationPressureInterfaceSettings > asterixRadiationPressureSettings =
+            std::make_shared< CannonBallRadiationPressureInterfaceSettings >(
+                "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies );
+
+        // Create and set radiation pressure settings
+        bodies.at( "Vehicle" )->setRadiationPressureInterface(
+            "Sun", createRadiationPressureInterface(
+                asterixRadiationPressureSettings, "Vehicle", bodies ) );
+
+    //    bodies.at( "Vehicle" )->setEphemeris( std::make_shared< TabulatedCartesianEphemeris< > >(
+    //        std::shared_ptr< interpolators::OneDimensionalInterpolator
+    //            < double, Eigen::Vector6d > >( ), "Earth", "ECLIPJ2000" ) );
+
+        // Set accelerations on Vehicle that are to be taken into account.
+        SelectedAccelerationMap accelerationMap;
+        std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle;
+        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 2, 2 ) );
+        accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
+            basic_astrodynamics::point_mass_gravity ) );
+        accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >(
+            basic_astrodynamics::point_mass_gravity ) );
+        accelerationsOfVehicle[ "Mars" ].push_back( std::make_shared< AccelerationSettings >(
+            basic_astrodynamics::point_mass_gravity ) );
+        accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >(
+            basic_astrodynamics::cannon_ball_radiation_pressure ) );
+        accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
+            basic_astrodynamics::aerodynamic ) );
+        accelerationMap[ "Vehicle" ] = accelerationsOfVehicle;
+
+        // Set bodies for which initial state is to be estimated and integrated.
+        std::vector< std::string > bodiesToIntegrate;
+        std::vector< std::string > centralBodies;
+        bodiesToIntegrate.push_back( "Vehicle" );
+        centralBodies.push_back( "Earth" );
+
+        // Create acceleration models
+        AccelerationMap accelerationModelMap = createAccelerationModelsMap(
+            bodies, accelerationMap, bodiesToIntegrate, centralBodies );
+
+        // Set Keplerian elements for Asterix.
+        Eigen::Vector6d asterixInitialStateInKeplerianElements;
+        asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3;
+        asterixInitialStateInKeplerianElements( eccentricityIndex ) = 1.0E-5;
+        asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 );
+        asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
+            = unit_conversions::convertDegreesToRadians( 235.7 );
+        asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
+            = unit_conversions::convertDegreesToRadians( 23.4 );
+        asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 );
+
+        double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
+
+        // Set (perturbed) initial state.
+        Eigen::Matrix< double, 6, 1 > systemInitialState = convertKeplerianToCartesianElements(
+            asterixInitialStateInKeplerianElements, earthGravitationalParameter );
+
+        // Create propagator settings
+        std::shared_ptr< IntegratorSettings< double > > integratorSettings =
+            rungeKuttaFixedStepSettings( 400.0, CoefficientSets::rungeKuttaFehlberg78 );
+        std::shared_ptr< TranslationalStatePropagatorSettings< double, double > > propagatorSettings =
+            std::make_shared< TranslationalStatePropagatorSettings< double, double > >
+                ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, initialEphemerisTime,
+                  integratorSettings, propagationTimeTerminationSettings( finalEphemerisTime ), cowell );
+
+
+        // Define parameters.
+        LinkEnds linkEnds;
+        linkEnds[ observed_body ] = LinkEndId( "Vehicle", "" );
+
+        std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable;
+        linkEndsPerObservable[ position_observable ].push_back( linkEnds );
+
+        std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames;
+        parameterNames.push_back(
+            std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >(
+                "Vehicle", systemInitialState, "Earth" ) );
+
+        if( test > 0 )
+        {
+            parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", radiation_pressure_coefficient ) );
+            parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", constant_drag_coefficient ) );
+        }
+
+        // Create parameters
+        std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate =
+            createParametersToEstimate< double, double >( parameterNames, bodies );
+
+        printEstimatableParameterEntries( parametersToEstimate );
+
+        std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList;
+        observationSettingsList.push_back( std::make_shared< ObservationModelSettings >(
+            position_observable, linkEnds ) );
+
+        // Create orbit determination object.
+        OrbitDeterminationManager< double, double > orbitDeterminationManager =
+            OrbitDeterminationManager< double, double >(
+                bodies, parametersToEstimate, observationSettingsList, propagatorSettings );
+
+        std::vector< double > baseTimeList;
+        double observationTimeStart = initialEphemerisTime + 100.0;
+        double observationInterval = 20.0;
+        double currentObservationTime = observationTimeStart;
+        while( currentObservationTime < finalEphemerisTime - 100.0 )
+        {
+            baseTimeList.push_back( currentObservationTime );
+            currentObservationTime += observationInterval;
+        }
+
+        std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput =
+            getObservationSimulationSettings< double >(
+                linkEndsPerObservable, baseTimeList, observed_body );
+
+        // Simulate observations
+        std::shared_ptr< ObservationCollection< double, double > > simulatedObservations =
+            simulateObservations< double, double >(
+                measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies );
+
+
+
+        // Define estimation input
+        std::shared_ptr< CovarianceAnalysisInput< double, double  > > estimationInput =
+            std::make_shared< CovarianceAnalysisInput< double, double > >(
+                simulatedObservations );
+
+        std::map< observation_models::ObservableType, double > weightPerObservable;
+        weightPerObservable[ position_observable ] = 1.0;
+        estimationInput->setConstantPerObservableWeightsMatrix( weightPerObservable );
+
+        // Perform estimation
+        std::shared_ptr< CovarianceAnalysisOutput< double > > estimationOutput = orbitDeterminationManager.computeCovariance(
+            estimationInput );
+        auto stateHistory = std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( )
+        )->getDynamicsSimulator( )->getEquationsOfMotionNumericalSolution( );
+        auto stateTransitionMatrixHistory =
+            std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( )
+            )->getStateTransitionMatrixSolution( );
+        auto sensitivityMatrixHistory =
+            std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( )
+            )->getSensitivityMatrixSolution( );
+
+        std::map< double, Eigen::MatrixXd > propagatedCovariance;
+        propagateCovariance(
+            propagatedCovariance,
+            estimationOutput->getUnnormalizedCovarianceMatrix( ),
+            orbitDeterminationManager.getStateTransitionAndSensitivityMatrixInterface( ),
+            utilities::createVectorFromMapKeys( stateTransitionMatrixHistory ) );
+
+        std::map< double, Eigen::MatrixXd > rswPropagatedCovariance = convertCovarianceHistoryToFrame(
+            propagatedCovariance,
+            stateHistory,
+            reference_frames::global_reference_frame,
+            reference_frames::rsw_reference_frame );
+        std::map< double, Eigen::MatrixXd > tnwPropagatedCovariance = convertCovarianceHistoryToFrame(
+            propagatedCovariance,
+            stateHistory,
+            reference_frames::global_reference_frame,
+            reference_frames::tnw_reference_frame );
+
+        for( auto it : propagatedCovariance )
+        {
+            Eigen::MatrixXd currentStateTransition = stateTransitionMatrixHistory.at( it.first );
+            if( test == 0 )
+            {
+                Eigen::MatrixXd manualCovariance = currentStateTransition * estimationOutput->getUnnormalizedCovarianceMatrix( ) * currentStateTransition.transpose( );
+                TUDAT_CHECK_MATRIX_CLOSE_FRACTION( it.second,  manualCovariance, 1.0E-15 );
+
+                Eigen::Matrix3d rotationToRsw = reference_frames::getInertialToRswSatelliteCenteredFrameRotationMatrix( stateHistory.at( it.first ) );
+                Eigen::Matrix6d rotationToRswFull = Eigen::Matrix6d::Zero( );
+                rotationToRswFull.block( 0, 0, 3, 3 ) = rotationToRsw;
+                rotationToRswFull.block( 3, 3, 3, 3 ) = rotationToRsw;
+                Eigen::MatrixXd manualRswCovariance = rotationToRswFull * it.second * rotationToRswFull.transpose( );
+                TUDAT_CHECK_MATRIX_CLOSE_FRACTION( rswPropagatedCovariance.at( it.first ),  manualRswCovariance, 1.0E-15 );
+
+                Eigen::Matrix3d rotationToTnw = reference_frames::getInertialToTnwRotation( stateHistory.at( it.first ) );
+                Eigen::Matrix6d rotationToTnwFull = Eigen::Matrix6d::Zero( );
+                rotationToTnwFull.block( 0, 0, 3, 3 ) = rotationToTnw;
+                rotationToTnwFull.block( 3, 3, 3, 3 ) = rotationToTnw;
+                Eigen::MatrixXd manualTnwCovariance = rotationToTnwFull * it.second * rotationToTnwFull.transpose( );
+                TUDAT_CHECK_MATRIX_CLOSE_FRACTION( tnwPropagatedCovariance.at( it.first ),  manualTnwCovariance, 1.0E-15 );
+
+                Eigen::VectorXd rswFormalError = rswPropagatedCovariance.at( it.first ).diagonal( ).cwiseSqrt( );
+                Eigen::VectorXd tnwFormalError = tnwPropagatedCovariance.at( it.first ).diagonal( ).cwiseSqrt( );
+
+                // Weak check on radial and along-track errors.
+                BOOST_CHECK_SMALL( rswFormalError( 0 ) / rswFormalError( 1 ), 0.6 );
+                BOOST_CHECK_SMALL( rswFormalError( 4 ) / rswFormalError( 3 ), 0.6 );
+
+                // Check if in-plane RSW and TNW are similar
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 0 ), tnwFormalError( 1 ), 1.0E-3 );
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 1 ), tnwFormalError( 0 ), 1.0E-3 );
+
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 3 ), tnwFormalError( 4 ), 1.0E-3 );
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 4 ), tnwFormalError( 3 ), 1.0E-3 );
+
+                // Check if cross-plane RSW and TNW are equal
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 2 ), tnwFormalError( 2 ), std::numeric_limits< double >::epsilon( ) );
+                BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 5 ), tnwFormalError( 5 ), std::numeric_limits< double >::epsilon( ) );
+            }
+            else
+            {
+                Eigen::MatrixXd currentSensitivity = sensitivityMatrixHistory.at( it.first );
+                Eigen::MatrixXd fullStateTransition = Eigen::MatrixXd::Zero( 6, 8 );
+                fullStateTransition.block( 0, 0, 6, 6 ) = currentStateTransition;
+                fullStateTransition.block( 0, 6, 6, 2 ) = currentSensitivity;
+                Eigen::MatrixXd manualCovariance = fullStateTransition * estimationOutput->getUnnormalizedCovarianceMatrix( ) * fullStateTransition.transpose( );
+                TUDAT_CHECK_MATRIX_CLOSE_FRACTION( it.second,  manualCovariance, 1.0E-12 );
+
+            }
+        }
+//        std::cout <<"formal error: "<< ( estimationOutput->getFormalErrorVector( ) ).transpose( ) << std::endl;
+    }
+
+}
+
+BOOST_AUTO_TEST_SUITE_END( )
+
+}
+
+}
+
+
+