diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index d95c780bfa..7cd374fe4d 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -197,14 +197,14 @@ class AnalyticalAccelerationPartialSettings: public CustomAccelerationPartialSet public: AnalyticalAccelerationPartialSettings( - const std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction, - const Eigen::VectorXd& parameterPerturbation, + const std::function< Eigen::MatrixXd( const double, const Eigen::Vector3d& ) > accelerationPartialFunction, const std::string bodyUndergoingAcceleration, const std::string bodyExertingAcceleration, const basic_astrodynamics::AvailableAcceleration accelerationType ): - CustomAccelerationPartialSettings( bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType ){ } + CustomAccelerationPartialSettings( bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType ), + accelerationPartialFunction_( accelerationPartialFunction ){ } - std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction_; + std::function< Eigen::MatrixXd( const double, const Eigen::Vector3d& ) > accelerationPartialFunction_; }; //! Base class for a parameter that is to be estimated. @@ -347,8 +347,8 @@ class CustomAccelerationPartialCalculator virtual ~CustomAccelerationPartialCalculator( ){ } - virtual Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( - const double currentTime, const Eigen::Vector3d currentAcceleration, + virtual Eigen::MatrixXd computePartial( + const double currentTime, const Eigen::Vector3d& currentAcceleration, const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) = 0; protected: @@ -427,8 +427,8 @@ class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationP } - Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( - const double currentTime, const Eigen::Vector3d currentAcceleration, + Eigen::MatrixXd computePartial( + const double currentTime, const Eigen::Vector3d& currentAcceleration, const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) { currentPartial_.setZero( ); @@ -486,26 +486,30 @@ class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationP Eigen::Matrix< double, 3, 6 > currentPartial_; }; -template< typename ParameterScalarType > class AnalyticalAccelerationPartialCalculator: public CustomAccelerationPartialCalculator { public: AnalyticalAccelerationPartialCalculator( - const std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction, - const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter ): - accelerationPartialFunction_( accelerationPartialFunction ), parameter_( parameter ){ } + const std::function accelerationPartialFunction, + const int parameterSize, + const std::string parameterName ) : + accelerationPartialFunction_( accelerationPartialFunction ), parameterSize_( parameterSize ), + parameterName_( parameterName ) + { + } - Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( - const double currentTime, const Eigen::Vector3d currentAcceleration, - const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) + Eigen::MatrixXd computePartial( + const double currentTime, const Eigen::Vector3d ¤tAcceleration, + const std::shared_ptr accelerationModel ) { Eigen::MatrixXd currentAccelerationPartial = accelerationPartialFunction_( currentTime, currentAcceleration ); - if( currentAccelerationPartial.cols( ) != parameter_->getParameterSize( ) ) + if ( currentAccelerationPartial.cols( ) != parameterSize_) { throw std::runtime_error( "Error when making numerical acceleration partial for parameter " + - parameter_->getParameterDescription( ) + ", sizes are inconsistent: " + - std::to_string( currentAccelerationPartial.cols( ) ) + ", " + std::to_string( parameter_->getParameterSize( ) ) ); + parameterName_ + ", sizes are inconsistent: " + + std::to_string( currentAccelerationPartial.cols( )) + ", " + + std::to_string( parameterSize_)); } return currentAccelerationPartial; } @@ -513,9 +517,11 @@ class AnalyticalAccelerationPartialCalculator: public CustomAccelerationPartialC protected: - std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction_; + std::function accelerationPartialFunction_; + + int parameterSize_; - std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_; + std::string parameterName_; }; class CustomSingleAccelerationPartialCalculatorSet diff --git a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h index 10077b9f9a..a4653189d7 100644 --- a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h +++ b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h @@ -52,6 +52,9 @@ std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > c std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > customPartialCalculator = nullptr; std::shared_ptr< estimatable_parameters::NumericalAccelerationPartialSettings > numericalCustomPartialSettings = std::dynamic_pointer_cast< estimatable_parameters::NumericalAccelerationPartialSettings >( customPartialSettings ); + std::shared_ptr< estimatable_parameters::AnalyticalAccelerationPartialSettings > analyticalCustomPartialSettings = + std::dynamic_pointer_cast< estimatable_parameters::AnalyticalAccelerationPartialSettings >( customPartialSettings ); + if( numericalCustomPartialSettings!= nullptr ) { if( parameter->getParameterName( ).first != estimatable_parameters::initial_body_state ) @@ -81,6 +84,18 @@ std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > c bodyStateSetFunction, environmentUpdateFunction ); } + else if( analyticalCustomPartialSettings!= nullptr ) + { + if( parameter->getParameterName( ).first != estimatable_parameters::initial_body_state ) + { + throw std::runtime_error( "Error, only initial cartesian state supported for custom numerical acceleration partial" ); + } + + customPartialCalculator = std::make_shared< estimatable_parameters::AnalyticalAccelerationPartialCalculator >( + analyticalCustomPartialSettings->accelerationPartialFunction_, + parameter->getParameterSize( ), + parameter->getParameterDescription( ) ); + } else { throw std::runtime_error( "Error when creating custom acceleration partial, only numerical partial is supported" ); @@ -106,8 +121,6 @@ std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculat parameterSet->getEstimatedInitialStateParameters( ).at( i )->getCustomPartialSettings( ); if( customPartialSettings != nullptr ) { - std::cout<<"Matches "<< - customPartialSettings->accelerationMatches( acceleratedBody.first, acceleratingBody.first, accelerationType )<accelerationMatches( acceleratedBody.first, acceleratingBody.first, accelerationType ) ) { switch( parameterSet->getEstimatedInitialStateParameters( ).at( i )->getParameterName( ).first ) diff --git a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp index 3c96eb9e9e..6aa01814d6 100644 --- a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp +++ b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp @@ -56,14 +56,24 @@ Eigen::Vector3d customAccelerationFunction( Eigen::Vector3d acceleration = Eigen::Vector3d::Ones( ) * std::exp( -relativeState.segment( 0, 3 ).norm( ) / scaleDistance ); return acceleration; } -// -//Eigen::MatrixXd getCustomAccelerationPartialFunction( -// const SystemOfBodies& bodies ) -//{ -// double scaleDistance = 1.0E6; -// Eigen::Vector6d relativeState = bodies.at( "Vehicle" )->getState( ) - bodies.at( "Earth" )->getState( ); -// return Eigen::Matrix< double, 3, 6 >::Zero( ); -//} + +Eigen::MatrixXd getCustomAccelerationPartialFunction( + const SystemOfBodies& bodies, + const double time, + const Eigen::Vector3d& acceleration ) +{ + double scaleDistance = 1.0E6; + Eigen::Vector6d relativeState = bodies.at( "Vehicle" )->getState( ) - bodies.at( "Earth" )->getState( ); + Eigen::Matrix< double, 3, 6 > partial = Eigen::Matrix< double, 3, 6 >::Zero( ); + double positionNorm = relativeState.segment( 0, 3 ).norm( ); + for( unsigned int i = 0; i < 3; i ++ ) + { + partial.block( i, 0, 1, 3 ) = -relativeState.segment( 0, 3 ).transpose( ) / ( positionNorm * scaleDistance ) * + std::exp( -positionNorm / scaleDistance ); + } + + return partial; +} BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) { @@ -145,7 +155,7 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) std::shared_ptr< IntegratorSettings< double > > integratorSettings = rungeKuttaFixedStepSettings( 90.0, CoefficientSets::rungeKuttaFehlberg78 ); - for( int testCase = 0; testCase < 2; testCase ++ ) + for( int testCase = 0; testCase < 3; testCase ++ ) { std::shared_ptr > propagatorSettings = @@ -169,6 +179,13 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) std::make_shared( Eigen::Vector6d::Ones( ) * 10.0, "Vehicle", "Vehicle", custom_acceleration ); } + else if( testCase == 2 ) + { + parameterNames.at( 0 )->customPartialSettings_ = + std::make_shared( + std::bind( &getCustomAccelerationPartialFunction, bodies, std::placeholders::_1, std::placeholders::_2 ), + "Vehicle", "Vehicle", custom_acceleration ); + } // Create parameters std::shared_ptr > parametersToEstimate = @@ -269,7 +286,11 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) { BOOST_CHECK_SMALL( 1.0 / maximumError, 50.0 ); } - else + else if( testCase == 1 ) + { + BOOST_CHECK_SMALL( maximumError, 1.0E-6 ); + } + else if( testCase == 2 ) { BOOST_CHECK_SMALL( maximumError, 1.0E-6 ); }