Skip to content

Commit

Permalink
Added test of custom analytical partial
Browse files Browse the repository at this point in the history
  • Loading branch information
@DominicDirkx
DominicDirkx committed Oct 4, 2023
1 parent 449c6e0 commit 8c7c2e4
Show file tree
Hide file tree
Showing 3 changed files with 72 additions and 32 deletions.
46 changes: 26 additions & 20 deletions include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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( );
Expand Down Expand Up @@ -486,36 +486,42 @@ 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<Eigen::MatrixXd( const double, const Eigen::Vector3d & )> 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 &currentAcceleration,
const std::shared_ptr<basic_astrodynamics::AccelerationModel3d> 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;
}

protected:


std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction_;
std::function<Eigen::MatrixXd( const double, const Eigen::Vector3d & )> accelerationPartialFunction_;

int parameterSize_;

std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_;
std::string parameterName_;
};

class CustomSingleAccelerationPartialCalculatorSet
Expand Down
17 changes: 15 additions & 2 deletions include/tudat/simulation/estimation_setup/createAccelerationPartials.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
Expand Down Expand Up @@ -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" );
Expand All @@ -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 )<<std::endl;
if( customPartialSettings->accelerationMatches( acceleratedBody.first, acceleratingBody.first, accelerationType ) )
{
switch( parameterSet->getEstimatedInitialStateParameters( ).at( i )->getParameterName( ).first )
Expand Down
41 changes: 31 additions & 10 deletions tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 )
{
Expand Down Expand Up @@ -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<TranslationalStatePropagatorSettings<double> > propagatorSettings =
Expand All @@ -169,6 +179,13 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation )
std::make_shared<NumericalAccelerationPartialSettings>( Eigen::Vector6d::Ones( ) * 10.0,
"Vehicle", "Vehicle", custom_acceleration );
}
else if( testCase == 2 )
{
parameterNames.at( 0 )->customPartialSettings_ =
std::make_shared<AnalyticalAccelerationPartialSettings>(
std::bind( &getCustomAccelerationPartialFunction, bodies, std::placeholders::_1, std::placeholders::_2 ),
"Vehicle", "Vehicle", custom_acceleration );
}

// Create parameters
std::shared_ptr<estimatable_parameters::EstimatableParameterSet<double> > parametersToEstimate =
Expand Down Expand Up @@ -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 );
}
Expand Down

0 comments on commit 8c7c2e4

Please sign in to comment.