From 70288bdefab8d4ce9a8d9871c91f39d5d5e430a0 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 3 Oct 2023 21:10:20 +0200 Subject: [PATCH 1/8] Skeleton of custom partials --- .../basic_astro/customAccelerationModel.h | 2 + .../customAccelerationPartial.h | 271 ++++++++++++++++++ .../yarkovskyAccelerationPartial.h | 1 - .../estimatableParameter.h | 253 +++++++++++++++- .../createAccelerationPartials.h | 20 +- .../createEstimatableParameters.h | 15 + .../estimatableParameterSettings.h | 3 + .../propagation_setup/environmentUpdater.h | 3 +- 8 files changed, 564 insertions(+), 4 deletions(-) create mode 100644 include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h diff --git a/include/tudat/astro/basic_astro/customAccelerationModel.h b/include/tudat/astro/basic_astro/customAccelerationModel.h index 96a28279f8..8cf08a7bc0 100644 --- a/include/tudat/astro/basic_astro/customAccelerationModel.h +++ b/include/tudat/astro/basic_astro/customAccelerationModel.h @@ -25,6 +25,7 @@ class CustomAccelerationModel: public basic_astrodynamics::AccelerationModel3d const std::function< Eigen::Vector3d( const double ) > accelerationFunction ): accelerationFunction_( accelerationFunction ) { + } virtual void updateMembers( const double currentTime = TUDAT_NAN ) @@ -38,6 +39,7 @@ class CustomAccelerationModel: public basic_astrodynamics::AccelerationModel3d private: std::function< Eigen::Vector3d( const double ) > accelerationFunction_; + }; diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h new file mode 100644 index 0000000000..437b668a96 --- /dev/null +++ b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h @@ -0,0 +1,271 @@ +/* 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. + */ + +#ifndef TUDAT_CUSTOMACCELERATIONPARTIALS_H +#define TUDAT_CUSTOMACCELERATIONPARTIALS_H + +#include "tudat/astro/basic_astro/customAccelerationModel.h" + +#include "tudat/astro/orbit_determination/acceleration_partials/accelerationPartial.h" + +namespace tudat +{ + +namespace acceleration_partials +{ + + + +//! Class to calculate the partials of the custom acceleration w.r.t. parameters and states. +class CustomAccelerationPartial: public AccelerationPartial +{ +public: + + CustomAccelerationPartial( + const std::string acceleratedBody, + const std::string acceleratingBody, + const std::shared_ptr< basic_astrodynamics::CustomAccelerationModel > customAcceleration, + const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyUndergoingPositionPartial = nullptr, + const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyExertingPositionPartial = nullptr, + const std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials = + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > >( ), + const std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials = + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > ( ) ): + AccelerationPartial( acceleratedBody, acceleratingBody, basic_astrodynamics::custom_acceleration ), + customAcceleration_( customAcceleration ), + bodyUndergoingPositionPartial_( bodyUndergoingPositionPartial ), + bodyExertingPositionPartial_( bodyExertingPositionPartial ), + customDoubleParameterPartials_( customDoubleParameterPartials ), + customVectorParameterPartials_( customVectorParameterPartials ) + { + currentPartialWrtUndergoingState_.setZero( ); + currentPartialWrtExertingState_.setZero( ); + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration + * and adding it to the existing partial block + * Update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtUndergoingState_.block( 0, 0, 3, 3 ); + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtUndergoingState_.block( 0, 0, 3, 3 ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration and + * adding it to the existing partial block. + * The update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian position of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtPositionOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtExertingState_.block( 0, 0, 3, 3 ); + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtExertingState_.block( 0, 0, 3, 3 ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration + * and adding it to the existing partial block + * Update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian velocity of body + * undergoing acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtVelocityOfAcceleratedBody( + Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtUndergoingState_.block( 0, 3, 3, 3 ); + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtUndergoingState_.block( 0, 3, 3, 3 ); + } + } + + //! Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration.. + /*! + * Function for calculating the partial of the acceleration w.r.t. the velocity of body undergoing acceleration and + * adding it to the existing partial block. + * The update( ) function must have been called during current time step before calling this function. + * \param partialMatrix Block of partial derivatives of acceleration w.r.t. Cartesian velocity of body + * exerting acceleration where current partial is to be added. + * \param addContribution Variable denoting whether to return the partial itself (true) or the negative partial (false). + * \param startRow First row in partialMatrix block where the computed partial is to be added. + * \param startColumn First column in partialMatrix block where the computed partial is to be added. + */ + void wrtVelocityOfAcceleratingBody( Eigen::Block< Eigen::MatrixXd > partialMatrix, + const bool addContribution = 1, const int startRow = 0, const int startColumn = 0 ) + { + if( addContribution ) + { + partialMatrix.block( startRow, startColumn, 3, 3 ) += currentPartialWrtExertingState_.block( 0, 3, 3, 3 ); + } + else + { + partialMatrix.block( startRow, startColumn, 3, 3 ) -= currentPartialWrtExertingState_.block( 0, 3, 3, 3 ); + } + } + + //! Function for determining if the acceleration is dependent on a non-translational integrated state. + /*! + * Function for determining if the acceleration is dependent on a non-translational integrated state. + * No dependency is implemented, but a warning is provided if partial w.r.t. mass of body exerting acceleration + * (and undergoing acceleration if mutual attraction is used) is requested. + * \param stateReferencePoint Reference point id of propagated state + * \param integratedStateType Type of propagated state for which dependency is to be determined. + * \return True if dependency exists (non-zero partial), false otherwise. + */ + bool isStateDerivativeDependentOnIntegratedAdditionalStateTypes( + const std::pair< std::string, std::string >& stateReferencePoint, + const propagators::IntegratedStateType integratedStateType ) + { + return 0; + } + + void createCustomParameterPartialFunction( + Eigen::MatrixXd& partialBlock, + const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > customPartialCalculator ) + { + partialBlock = customPartialCalculator->computePartial( currentTime_, customAcceleration_->getAcceleration( ), customAcceleration_ ); + } + //! Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a double parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency, 1 otherwise). + */ + std::pair< std::function< void( Eigen::MatrixXd& ) >, int > + getParameterPartialFunction( std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter ) + { + std::function< void( Eigen::MatrixXd& ) > partialFunction; + int parameterSize = 0; + if( customDoubleParameterPartials_.count( parameter )!= 0 ) + { + partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, + std::placeholders::_1, customDoubleParameterPartials_.at( parameter ) ); + parameterSize = 1; + } + return std::make_pair( partialFunction, parameterSize ); + } + + //! Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + /*! + * Function for setting up and retrieving a function returning a partial w.r.t. a vector parameter. + * Function returns empty function and zero size indicator for parameters with no dependency for current acceleration. + * \param parameter Parameter w.r.t. which partial is to be taken. + * \return Pair of parameter partial function and number of columns in partial (0 for no dependency). + */ + std::pair< std::function< void( Eigen::MatrixXd& ) >, int > getParameterPartialFunction( + std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > > parameter ) + { + std::function< void( Eigen::MatrixXd& ) > partialFunction; + int parameterSize = 0; + if( customVectorParameterPartials_.count( parameter )!= 0 ) + { + partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, + std::placeholders::_1, customVectorParameterPartials_.at( parameter ) ); + parameterSize = parameter->getParameterSize( ); + } + return std::make_pair( partialFunction, parameterSize ); + } + + //! Function for updating partial w.r.t. the bodies' positions + /*! + * Function for updating common blocks of partial to current state. For the central gravitational acceleration, + * position partial is computed and set. + * \param currentTime Time at which partials are to be calculated + */ + void update( const double currentTime = TUDAT_NAN ) + { + customAcceleration_->updateMembers( currentTime ); + + if( !( currentTime_ == currentTime ) ) + { + if( bodyUndergoingPositionPartial_ != nullptr ) + { + currentPartialWrtUndergoingState_ = bodyUndergoingPositionPartial_->computePartial( + currentTime, customAcceleration_->getAcceleration( ), customAcceleration_ ); + } + + if( bodyExertingPositionPartial_ != nullptr ) + { + currentPartialWrtExertingState_ = bodyExertingPositionPartial_->computePartial( + currentTime, customAcceleration_->getAcceleration( ), customAcceleration_ ); + } + + currentTime_ = currentTime; + } + } + +protected: + + const std::shared_ptr< basic_astrodynamics::CustomAccelerationModel > customAcceleration_; + + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyUndergoingPositionPartial_; + + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyExertingPositionPartial_; + + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials_; + + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials_; + + Eigen::Matrix< double, 3, 6 > currentPartialWrtUndergoingState_; + + Eigen::Matrix< double, 3, 6 > currentPartialWrtExertingState_; + + +}; + +} // namespace acceleration_partials + +} // namespace tudat + +#endif // TUDAT_CUSTOMACCELERATIONPARTIALS_H diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/yarkovskyAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/yarkovskyAccelerationPartial.h index 7ab77948b6..cd5ffe1806 100644 --- a/include/tudat/astro/orbit_determination/acceleration_partials/yarkovskyAccelerationPartial.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/yarkovskyAccelerationPartial.h @@ -36,7 +36,6 @@ class YarkovskyAccelerationPartial: public AccelerationPartial { public: - YarkovskyAccelerationPartial( const std::shared_ptr< electromagnetism::YarkovskyAcceleration > yarkovskyAcceleration, const std::string acceleratedBody, diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index 32c825776f..aefe93e0bb 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -18,10 +18,12 @@ #include - +#include #include #include +#include "tudat/astro/basic_astro/accelerationModelTypes.h" +#include "tudat/astro/basic_astro/accelerationModel.h" #include "tudat/astro/propagators/singleStateTypeDerivative.h" namespace tudat @@ -139,6 +141,61 @@ bool isParameterArcWiseInitialStateProperty( const EstimatebleParametersEnum par typedef std::pair< EstimatebleParametersEnum, std::pair< std::string, std::string > > EstimatebleParameterIdentifier; +class CustomAccelerationPartialSettings +{ +public: + + CustomAccelerationPartialSettings( + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType ): + bodyUndergoingAcceleration_( bodyUndergoingAcceleration ), + bodyExertingAcceleration_( bodyExertingAcceleration ), + accelerationType_( accelerationType ) + { } + + virtual ~CustomAccelerationPartialSettings( ){ } + + std::string bodyUndergoingAcceleration_; + + std::string bodyExertingAcceleration_; + + basic_astrodynamics::AvailableAcceleration accelerationType_; +}; + +class NumericalAccelerationPartialSettings: public CustomAccelerationPartialSettings +{ +public: + + NumericalAccelerationPartialSettings( + const Eigen::VectorXd& parameterPerturbation, + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType, + const std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& environmentUpdateSettings = + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >( ) ): + CustomAccelerationPartialSettings( bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType ), + parameterPerturbation_( parameterPerturbation ), environmentUpdateSettings_( environmentUpdateSettings ){ } + + Eigen::VectorXd parameterPerturbation_; + + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > > environmentUpdateSettings_; +}; + +class AnalyticalAccelerationPartialSettings: public CustomAccelerationPartialSettings +{ +public: + + AnalyticalAccelerationPartialSettings( + const std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction, + const Eigen::VectorXd& parameterPerturbation, + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType ): + CustomAccelerationPartialSettings( bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType ){ } + + std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction_; +}; //! Base class for a parameter that is to be estimated. /*! @@ -251,14 +308,208 @@ class EstimatableParameter virtual void throwExceptionIfNotFullyDefined( ){ } + std::shared_ptr< CustomAccelerationPartialSettings > getCustomPartialSettings( ) + { + return customPartialSettings_; + } + + void setCustomPartialSettings( const std::shared_ptr< CustomAccelerationPartialSettings > customPartialSettings ) + { + customPartialSettings_ = customPartialSettings; + } protected: //! Identifier of parameter. EstimatebleParameterIdentifier parameterName_; + + std::shared_ptr< CustomAccelerationPartialSettings > customPartialSettings_; + }; + + + +class CustomAccelerationPartialCalculator +{ +public: + + CustomAccelerationPartialCalculator( ){ } + + virtual ~CustomAccelerationPartialCalculator( ){ } + + virtual Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( + const double currentTime, const Eigen::Vector3d currentAcceleration, + const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) = 0; + +protected: +// std::string bodyUndergoingAcceleration_; +// +// std::string bodyExertingAcceleration_; +// +// basic_astrodynamics::AvailableAcceleration accelerationType_; +}; +// +// +//template< typename ParameterScalarType > +//class NumericalAccelerationPartialCalculator: public CustomAccelerationPartialCalculator +//{ +//public: +// NumericalAccelerationPartialCalculator( +// const Eigen::VectorXd& parameterPerturbation, +// const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter ): +// parameterPerturbation_( parameterPerturbation ), +// parameter_( parameter ) +// { +// if( parameterPerturbation_.rows( ) != parameter_->getParameterSize( ) ) +// { +// throw std::runtime_error( "Error when making numerical acceleration partial for parameter " + +// parameter->getParameterDescription( ) + ", sizes are inconsistent: " + +// std::to_string( parameterPerturbation_.rows( ) ) + ", " + std::to_string( parameter->getParameterSize( ) ) ); +// } +// currentPartial_ = Eigen::MatrixXd::Zero( 3, parameterPerturbation_.rows( ) ); +// } +// +// ~NumericalAccelerationPartialCalculator( ){ } +// +// Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( +// const double currentTime, const Eigen::Vector3d currentAcceleration, +// const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) +// { +//// if constexpr ( std::is_same< double, ParameterScalarType >::value == true ) +//// { +//// currentPartial_.setZero( ); +//// double originalParameterValue = parameter_->getParameterValue( ); +//// +//// parameter_->setParameterValue( originalParameterValue ) +//// } +// } +//protected: +// +// Eigen::VectorXd parameterPerturbation_; +// +// std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_; +// +// Eigen::Matrix< double, 3, Eigen::Dynamic > currentPartial_; +// +//}; + + +class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationPartialCalculator +{ +public: + + NumericalAccelerationPartialWrtStateCalculator( + const Eigen::VectorXd& bodyStatePerturbations, + const std::function< Eigen::Vector6d( ) > bodyStateGetFunction, + const std::function< void( const Eigen::Vector6d& ) > bodyStateSetFunction, + const std::function< void( const double ) > environmentUpdateFunction ): + bodyStatePerturbations_( bodyStatePerturbations ), + bodyStateGetFunction_( bodyStateGetFunction ), + bodyStateSetFunction_( bodyStateSetFunction ), + environmentUpdateFunction_( environmentUpdateFunction ) + { + if( bodyStatePerturbations_.rows( ) != 6 ) + { + throw std::runtime_error( "Error when making numerical acceleration partial for initial state parameter, sizes are inconsistent: " + + std::to_string( bodyStatePerturbations_.rows( ) ) + ", " + std::to_string( 6 ) ); + } + currentPartial_ = Eigen::MatrixXd::Zero( 3, 6 ); + + } + + Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( + const double currentTime, const Eigen::Vector3d currentAcceleration, + const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) + { + bodyStatePerturbations_.setZero( ); + Eigen::Vector6d nominalState = bodyStateGetFunction_( ); + Eigen::Vector6d perturbedState; + + // Compute state partial by numerical difference + Eigen::Vector3d upperturbedAcceleration, downperturbedAcceleration; + for( unsigned int i = 0; i < 6; i++ ) + { + // Perturb state upwards + perturbedState = nominalState; + perturbedState( i ) += bodyStatePerturbations_( i ); + + // Update environment/acceleration to perturbed state. + accelerationModel->resetCurrentTime( ); + bodyStateSetFunction_( perturbedState ); + environmentUpdateFunction_( currentTime ); + accelerationModel->updateMembers( currentTime ); + + // Retrieve perturbed acceleration. + upperturbedAcceleration = accelerationModel->getAcceleration( ); + + // Perturb state downwards + perturbedState = nominalState; + perturbedState( i ) -= bodyStatePerturbations_( i ); + + // Update environment/acceleration to perturbed state. + accelerationModel->resetCurrentTime( ); + bodyStateSetFunction_( perturbedState ); + environmentUpdateFunction_( currentTime ); + accelerationModel->updateMembers( currentTime ); + + // Retrieve perturbed acceleration. + downperturbedAcceleration = accelerationModel->getAcceleration( ); + + // Compute partial + bodyStatePerturbations_.block( 0, i, 3, 1 ) = + ( upperturbedAcceleration - downperturbedAcceleration ) / ( 2.0 * bodyStatePerturbations_( i ) ); + } + return bodyStatePerturbations_; + } + +protected: + + Eigen::VectorXd bodyStatePerturbations_; + + const std::function< Eigen::Vector6d( ) > bodyStateGetFunction_; + + const std::function< void( const Eigen::Vector6d& ) > bodyStateSetFunction_; + + std::function< void( const double ) > environmentUpdateFunction_; + + 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 ){ } + + Eigen::Matrix< double, 3, Eigen::Dynamic > 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( ) ) + { + 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( ) ) ); + } + return currentAccelerationPartial; + } + +protected: + + + std::function< Eigen::Matrix< double, 3, Eigen::Dynamic >( const double, Eigen::Vector3d ) > accelerationPartialFunction_; + + std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_; +}; + + //! Function to determine if an initial state parameter is a single- or multi-arc parameter /*! * Function to determine if an initial state parameter is a single- or multi-arc parameter. Function throws an error, if diff --git a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h index 602373ae1a..d81f98805b 100644 --- a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h +++ b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h @@ -30,6 +30,7 @@ #include "tudat/astro/orbit_determination/acceleration_partials/panelledRadiationPressureAccelerationPartial.h" #include "tudat/astro/orbit_determination/acceleration_partials/thrustAccelerationPartial.h" #include "tudat/astro/orbit_determination/acceleration_partials/yarkovskyAccelerationPartial.h" +#include "tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h" #include "tudat/astro/orbit_determination/observation_partials/rotationMatrixPartial.h" #include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" #include "tudat/astro/basic_astro/accelerationModelTypes.h" @@ -544,8 +545,25 @@ std::shared_ptr< acceleration_partials::AccelerationPartial > createAnalyticalAc break; } case custom_acceleration: - std::cerr<<"Warning, custom acceleration partials implicitly set to zero - depending on thrust guidance model, this may provide biased results for variational equations"< customAccelerationModel = + std::dynamic_pointer_cast< CustomAccelerationModel >( accelerationModel ); + if( customAccelerationModel == nullptr ) + { + throw std::runtime_error( + "Acceleration class type does not match acceleration type enum (custom_acceleration) set when making " + "acceleration partial." ); + } + else + { + // Create partial-calculating object. + accelerationPartial = std::make_shared< CustomAccelerationPartial > + ( customAccelerationModel, acceleratedBody.first, acceleratingBody.first ); + + } break; + } case thrust_acceleration: { // Check if identifier is consistent with type. diff --git a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h index 13b8debc6a..b9030fcfd8 100644 --- a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h +++ b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h @@ -789,6 +789,11 @@ std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::Matrix } } + if( parameterSettings->customPartialSettings_ != nullptr ) + { + initialStateParameterToEstimate->setCustomPartialSettings( parameterSettings->customPartialSettings_ ); + } + return initialStateParameterToEstimate; } @@ -1237,6 +1242,11 @@ std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > create } } + if( doubleParameterName->customPartialSettings_ != nullptr ) + { + doubleParameterToEstimate->setCustomPartialSettings( doubleParameterName->customPartialSettings_ ); + } + return doubleParameterToEstimate; } @@ -1963,6 +1973,11 @@ std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > } } + if( vectorParameterName->customPartialSettings_ != nullptr ) + { + vectorParameterToEstimate->setCustomPartialSettings( vectorParameterName->customPartialSettings_ ); + } + return vectorParameterToEstimate; } diff --git a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h index 36214fd0db..5287b658b1 100644 --- a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h +++ b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h @@ -15,6 +15,7 @@ #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/astro/observation_models/linkTypeDefs.h" #include "tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h" +#include "tudat/astro/basic_astro/accelerationModelTypes.h" namespace tudat { @@ -58,6 +59,8 @@ class EstimatableParameterSettings */ EstimatebleParameterIdentifier parameterType_; + std::shared_ptr< CustomAccelerationPartialSettings > customPartialSettings_; + }; diff --git a/include/tudat/simulation/propagation_setup/environmentUpdater.h b/include/tudat/simulation/propagation_setup/environmentUpdater.h index 355079ad74..2853cc9cf6 100644 --- a/include/tudat/simulation/propagation_setup/environmentUpdater.h +++ b/include/tudat/simulation/propagation_setup/environmentUpdater.h @@ -804,7 +804,8 @@ class EnvironmentUpdater std::vector< boost::tuple< EnvironmentModelsToUpdate, std::string, std::function< void( const double ) > > > updateFunctionVector_; - //! List of time-dependent functions to call to reset the time of the environment (to NaN signal recomputation for next + //! List of time-dependent functions to call to reset the time of the environment (to NaN s + //! ignal recomputation for next //! time step). std::vector< boost::tuple< EnvironmentModelsToUpdate, std::string, std::function< void( ) > > > resetFunctionVector_; From 449c6e041571110336e1962e1ef0900cfcc14957 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 4 Oct 2023 20:35:21 +0200 Subject: [PATCH 2/8] First unit test of custom partial functionality working --- .../customAccelerationPartial.h | 48 +-- .../estimatableParameter.h | 39 ++- .../createAccelerationPartials.h | 126 +++++++- .../astro/orbit_determination/CMakeLists.txt | 5 + ...CustomAccelerationVariationalEquations.cpp | 284 ++++++++++++++++++ 5 files changed, 470 insertions(+), 32 deletions(-) create mode 100644 tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h index 437b668a96..f82495ff12 100644 --- a/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h @@ -32,25 +32,27 @@ class CustomAccelerationPartial: public AccelerationPartial const std::string acceleratedBody, const std::string acceleratingBody, const std::shared_ptr< basic_astrodynamics::CustomAccelerationModel > customAcceleration, - const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyUndergoingPositionPartial = nullptr, - const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyExertingPositionPartial = nullptr, - const std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials = - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > >( ), - const std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials = - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > ( ) ): + const std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > customAccelerationPartialSet = nullptr ): AccelerationPartial( acceleratedBody, acceleratingBody, basic_astrodynamics::custom_acceleration ), customAcceleration_( customAcceleration ), - bodyUndergoingPositionPartial_( bodyUndergoingPositionPartial ), - bodyExertingPositionPartial_( bodyExertingPositionPartial ), - customDoubleParameterPartials_( customDoubleParameterPartials ), - customVectorParameterPartials_( customVectorParameterPartials ) + customAccelerationPartialSet_( customAccelerationPartialSet ) { currentPartialWrtUndergoingState_.setZero( ); currentPartialWrtExertingState_.setZero( ); + + estimatable_parameters::EstimatebleParameterIdentifier undergoingBodyIdentifier = + std::make_pair( estimatable_parameters::initial_body_state, std::make_pair( acceleratedBody, "" ) ); + if( customAccelerationPartialSet->customInitialStatePartials_.count( undergoingBodyIdentifier ) > 0 ) + { + bodyUndergoingPositionPartial_ = customAccelerationPartialSet->customInitialStatePartials_.at( undergoingBodyIdentifier ); + } + + estimatable_parameters::EstimatebleParameterIdentifier exertingBodyIdentifier = + std::make_pair( estimatable_parameters::initial_body_state, std::make_pair( acceleratingBody, "" ) ); + if( customAccelerationPartialSet->customInitialStatePartials_.count( exertingBodyIdentifier ) > 0 && ( exertingBodyIdentifier != undergoingBodyIdentifier ) ) + { + bodyExertingPositionPartial_ = customAccelerationPartialSet->customInitialStatePartials_.at( exertingBodyIdentifier ); + } } //! Function for calculating the partial of the acceleration w.r.t. the position of body undergoing acceleration.. @@ -185,10 +187,11 @@ class CustomAccelerationPartial: public AccelerationPartial { std::function< void( Eigen::MatrixXd& ) > partialFunction; int parameterSize = 0; - if( customDoubleParameterPartials_.count( parameter )!= 0 ) + if( customAccelerationPartialSet_->customDoubleParameterPartials_.count( parameter )!= 0 ) { partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, - std::placeholders::_1, customDoubleParameterPartials_.at( parameter ) ); + std::placeholders::_1, + customAccelerationPartialSet_->customDoubleParameterPartials_.at( parameter ) ); parameterSize = 1; } return std::make_pair( partialFunction, parameterSize ); @@ -206,10 +209,11 @@ class CustomAccelerationPartial: public AccelerationPartial { std::function< void( Eigen::MatrixXd& ) > partialFunction; int parameterSize = 0; - if( customVectorParameterPartials_.count( parameter )!= 0 ) + if( customAccelerationPartialSet_->customVectorParameterPartials_.count( parameter )!= 0 ) { partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, - std::placeholders::_1, customVectorParameterPartials_.at( parameter ) ); + std::placeholders::_1, + customAccelerationPartialSet_->customVectorParameterPartials_.at( parameter ) ); parameterSize = parameter->getParameterSize( ); } return std::make_pair( partialFunction, parameterSize ); @@ -247,16 +251,12 @@ class CustomAccelerationPartial: public AccelerationPartial const std::shared_ptr< basic_astrodynamics::CustomAccelerationModel > customAcceleration_; + std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > customAccelerationPartialSet_; + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyUndergoingPositionPartial_; std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > bodyExertingPositionPartial_; - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials_; - - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, - std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials_; - Eigen::Matrix< double, 3, 6 > currentPartialWrtUndergoingState_; Eigen::Matrix< double, 3, 6 > currentPartialWrtExertingState_; diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index aefe93e0bb..d95c780bfa 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -161,6 +161,16 @@ class CustomAccelerationPartialSettings std::string bodyExertingAcceleration_; basic_astrodynamics::AvailableAcceleration accelerationType_; + + bool accelerationMatches( + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType ) + { + return ( accelerationType_ == accelerationType ) && + ( bodyUndergoingAcceleration_ == bodyUndergoingAcceleration ) && + ( bodyExertingAcceleration_ == bodyExertingAcceleration ); + } }; class NumericalAccelerationPartialSettings: public CustomAccelerationPartialSettings @@ -329,8 +339,6 @@ class EstimatableParameter }; - - class CustomAccelerationPartialCalculator { public: @@ -423,7 +431,8 @@ class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationP const double currentTime, const Eigen::Vector3d currentAcceleration, const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) { - bodyStatePerturbations_.setZero( ); + currentPartial_.setZero( ); + Eigen::Vector6d nominalState = bodyStateGetFunction_( ); Eigen::Vector6d perturbedState; @@ -458,10 +467,10 @@ class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationP downperturbedAcceleration = accelerationModel->getAcceleration( ); // Compute partial - bodyStatePerturbations_.block( 0, i, 3, 1 ) = + currentPartial_.block( 0, i, 3, 1 ) = ( upperturbedAcceleration - downperturbedAcceleration ) / ( 2.0 * bodyStatePerturbations_( i ) ); } - return bodyStatePerturbations_; + return currentPartial_; } protected: @@ -509,6 +518,26 @@ class AnalyticalAccelerationPartialCalculator: public CustomAccelerationPartialC std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_; }; +class CustomSingleAccelerationPartialCalculatorSet +{ +public: + CustomSingleAccelerationPartialCalculatorSet( ){ } + + std::map< estimatable_parameters::EstimatebleParameterIdentifier, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customInitialStatePartials_; + + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials_; + + std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials_; + + int getNumberOfCustomPartials( ) + { + return customInitialStatePartials_.size( ) + customDoubleParameterPartials_.size( ) + customVectorParameterPartials_.size( ); + } +}; + //! Function to determine if an initial state parameter is a single- or multi-arc parameter /*! diff --git a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h index d81f98805b..10077b9f9a 100644 --- a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h +++ b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h @@ -35,6 +35,7 @@ #include "tudat/simulation/estimation_setup/createCartesianStatePartials.h" #include "tudat/astro/basic_astro/accelerationModelTypes.h" #include "tudat/astro/orbit_determination/acceleration_partials/tidalLoveNumberPartialInterface.h" +#include "tudat/simulation/propagation_setup/environmentUpdater.h" namespace tudat { @@ -42,6 +43,116 @@ namespace tudat namespace simulation_setup { +template< typename ParameterScalarType > +std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > createCustomAccelerationPartial( + const std::shared_ptr< estimatable_parameters::CustomAccelerationPartialSettings > customPartialSettings, + const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter, + const SystemOfBodies& bodies ) +{ + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > customPartialCalculator = nullptr; + std::shared_ptr< estimatable_parameters::NumericalAccelerationPartialSettings > numericalCustomPartialSettings = + std::dynamic_pointer_cast< estimatable_parameters::NumericalAccelerationPartialSettings >( customPartialSettings ); + if( numericalCustomPartialSettings!= 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" ); + } + std::string bodyName = parameter->getParameterName( ).second.first; + if( bodies.count( bodyName ) == 0 ) + { + throw std::runtime_error( "Error when creating custom numerical acceleration partial. Could not find body " + bodyName ); + } + + std::function< Eigen::Vector6d( ) > bodyStateGetFunction = std::bind( &Body::getState, bodies.at( bodyName ) ); + std::function< void( const Eigen::Vector6d& ) > bodyStateSetFunction = std::bind( &Body::setState, bodies.at( bodyName ), std::placeholders::_1 ); + + std::function< void( const double ) > environmentUpdateFunction = std::bind( + &propagators::EnvironmentUpdater< double, double >::updateEnvironment, + std::make_shared< propagators::EnvironmentUpdater< double, double > >( + bodies, numericalCustomPartialSettings->environmentUpdateSettings_ ), + std::placeholders::_1, + std::unordered_map< propagators::IntegratedStateType, Eigen::VectorXd >( ), + std::vector< propagators::IntegratedStateType >( ) ); + + customPartialCalculator = std::make_shared< estimatable_parameters::NumericalAccelerationPartialWrtStateCalculator >( + numericalCustomPartialSettings->parameterPerturbation_, + bodyStateGetFunction, + bodyStateSetFunction, + environmentUpdateFunction ); + } + else + { + throw std::runtime_error( "Error when creating custom acceleration partial, only numerical partial is supported" ); + } + return customPartialCalculator; +} + +template< typename ParameterScalarType > +std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > createCustomAccelerationPartial( + const std::shared_ptr< estimatable_parameters::EstimatableParameterSet< ParameterScalarType > > parameterSet, + const std::shared_ptr< basic_astrodynamics::AccelerationModel< Eigen::Vector3d > > accelerationModel, + const std::pair< std::string, std::shared_ptr< simulation_setup::Body > > acceleratedBody, + const std::pair< std::string, std::shared_ptr< simulation_setup::Body > > acceleratingBody, + const SystemOfBodies& bodies ) +{ + basic_astrodynamics::AvailableAcceleration accelerationType = getAccelerationModelType( accelerationModel ); + + std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > partialCalculatorSet = + std::make_shared< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet >( ); + for( unsigned int i = 0; i < parameterSet->getEstimatedInitialStateParameters( ).size( ); i++ ) + { + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialSettings > customPartialSettings = + 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 ) + { + case estimatable_parameters::initial_body_state: + { + partialCalculatorSet->customInitialStatePartials_[ parameterSet->getEstimatedInitialStateParameters( ).at( i )->getParameterName( ) ] = + createCustomAccelerationPartial( + parameterSet->getEstimatedInitialStateParameters( ).at( i )->getCustomPartialSettings( ), + parameterSet->getEstimatedInitialStateParameters( ).at( i ), + bodies ); + + break; + } + default: + throw std::runtime_error( "Error when creating custom partial calculator for " + + parameterSet->getEstimatedInitialStateParameters( ).at( i )->getParameterDescription( ) + + ", parameter not supported for this option" ); + } + } + } + } + + for( unsigned int i = 0; i < parameterSet->getEstimatedDoubleParameters( ).size( ); i++ ) + { + if( parameterSet->getEstimatedDoubleParameters( ).at( i )->getCustomPartialSettings( ) != nullptr ) + { + throw std::runtime_error( "Error when creating custom partial calculator for " + + parameterSet->getEstimatedDoubleParameters( ).at( i )->getParameterDescription( ) + + ", parameter not supported for this option" ); + } + } + + for( unsigned int i = 0; i < parameterSet->getEstimatedVectorParameters( ).size( ); i++ ) + { + if( parameterSet->getEstimatedVectorParameters( ).at( i )->getCustomPartialSettings( ) != nullptr ) + { + throw std::runtime_error( "Error when creating custom partial calculator for " + + parameterSet->getEstimatedVectorParameters( ).at( i )->getParameterDescription( ) + + ", parameter not supported for this option" ); + } + } + return partialCalculatorSet; +} + //! Function to create a list of objects that can be used to compute partials of tidal gravity field variations /*! * Function to create a list of objects that can be used to compute partials of tidal gravity field variations @@ -80,10 +191,19 @@ std::shared_ptr< acceleration_partials::AccelerationPartial > createAnalyticalAc using namespace aerodynamics; using namespace acceleration_partials; - std::shared_ptr< acceleration_partials::AccelerationPartial > accelerationPartial = nullptr; - // Identify current acceleration model type AvailableAcceleration accelerationType = getAccelerationModelType( accelerationModel ); + + std::shared_ptr< acceleration_partials::AccelerationPartial > accelerationPartial = nullptr; + + std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > + customPartialCalculator = createCustomAccelerationPartial( + parametersToEstimate, accelerationModel, acceleratedBody, acceleratingBody, bodies ); + if( ( customPartialCalculator->getNumberOfCustomPartials( ) > 0 ) && ( accelerationType != custom_acceleration ) ) + { + throw std::runtime_error( "Error, custom acceleration partials only supported for custom acceleration (at present)" ); + } + switch( accelerationType ) { case point_mass_gravity: @@ -559,7 +679,7 @@ std::shared_ptr< acceleration_partials::AccelerationPartial > createAnalyticalAc { // Create partial-calculating object. accelerationPartial = std::make_shared< CustomAccelerationPartial > - ( customAccelerationModel, acceleratedBody.first, acceleratingBody.first ); + ( acceleratedBody.first, acceleratingBody.first, customAccelerationModel, customPartialCalculator ); } break; diff --git a/tests/src/astro/orbit_determination/CMakeLists.txt b/tests/src/astro/orbit_determination/CMakeLists.txt index 0704f77703..3c7406aff6 100644 --- a/tests/src/astro/orbit_determination/CMakeLists.txt +++ b/tests/src/astro/orbit_determination/CMakeLists.txt @@ -112,6 +112,11 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation ${Tudat_ESTIMATION_LIBRARIES} ) + TUDAT_ADD_TEST_CASE(CustomAccelerationVariationalEquations + PRIVATE_LINKS + ${Tudat_ESTIMATION_LIBRARIES} + ) + if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS ) TUDAT_ADD_TEST_CASE(EstimationFromIdealDataDoubleLongDouble diff --git a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp new file mode 100644 index 0000000000..3c96eb9e9e --- /dev/null +++ b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp @@ -0,0 +1,284 @@ +/* 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 +#include + +#include + + +#include + +#include "tudat/basics/testMacros.h" +#include "tudat/simulation/estimation.h" +#include "tudat/astro/orbit_determination/estimatable_parameters/directTidalTimeLag.h" + + +namespace tudat +{ +namespace unit_tests +{ +BOOST_AUTO_TEST_SUITE( test_custom_acceleration_estimation ) + +//Using declarations. +using namespace tudat::observation_models; +using namespace tudat::orbit_determination; +using namespace tudat::estimatable_parameters; +using namespace tudat::interpolators; +using namespace tudat::numerical_integrators; +using namespace tudat::spice_interface; +using namespace tudat::simulation_setup; +using namespace tudat::orbital_element_conversions; +using namespace tudat::ephemerides; +using namespace tudat::propagators; +using namespace tudat::basic_astrodynamics; +using namespace tudat::coordinate_conversions; +using namespace tudat::ground_stations; +using namespace tudat::observation_models; + + +Eigen::Vector3d customAccelerationFunction( + const SystemOfBodies& bodies, + const double time ) +{ + double scaleDistance = 1.0E6; + Eigen::Vector6d relativeState = bodies.at( "Vehicle" )->getState( ) - bodies.at( "Earth" )->getState( ); + 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( ); +//} + +BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) +{ + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + // Define bodies in simulation + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + + // Specify initial time + double initialEphemerisTime = double( 1.0E7 ); + double finalEphemerisTime = initialEphemerisTime + 1.0 * 86400.0; + + // Create bodies needed in simulation + BodyListSettings bodySettings = + getDefaultBodySettings( bodyNames ); + + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + bodies.createEmptyBody( "Vehicle" ); + bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// CREATE ACCELERATIONS ////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + // 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< AccelerationSettings >( + basic_astrodynamics::point_mass_gravity ) ); + accelerationsOfVehicle[ "Vehicle" ].push_back( std::make_shared< CustomAccelerationSettings >( + std::bind( &customAccelerationFunction, bodies, std::placeholders::_1 ) ) ); +// accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( +// basic_astrodynamics::point_mass_gravity ) ); + 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 ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// CREATE PROPAGATION SETTINGS //////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + // Set Keplerian elements for Asterix. + Eigen::Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.05; + 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::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; +// dependentVariables.push_back( +// std::make_shared< SingleDependentVariableSaveSettings >( +// aerodynamic_force_coefficients_dependent_variable, "Vehicle", "Earth" ) ); +// dependentVariables.push_back( +// std::make_shared< SingleDependentVariableSaveSettings >( +// radiation_pressure_coefficient_dependent_variable, "Vehicle", "Sun" ) ); + + + // Create integrator settings + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + rungeKuttaFixedStepSettings( 90.0, CoefficientSets::rungeKuttaFehlberg78 ); + + for( int testCase = 0; testCase < 2; testCase ++ ) + { + + std::shared_ptr > propagatorSettings = + std::make_shared >( + centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, initialEphemerisTime, + integratorSettings, + std::make_shared( finalEphemerisTime ), + cowell, dependentVariables ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// DEFINE PARAMETERS THAT ARE TO BE ESTIMATED //////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + // Define list of parameters to estimate. + std::vector > parameterNames; + parameterNames = getInitialStateParameterSettings( propagatorSettings, bodies ); + if( testCase == 1 ) + { + parameterNames.at( 0 )->customPartialSettings_ = + std::make_shared( Eigen::Vector6d::Ones( ) * 10.0, + "Vehicle", "Vehicle", custom_acceleration ); + } + + // Create parameters + std::shared_ptr > parametersToEstimate = + createParametersToEstimate( parameterNames, bodies ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// INITIALIZE ORBIT DETERMINATION OBJECT //////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + SingleArcVariationalEquationsSolver<> variationalEquationsSolver = SingleArcVariationalEquationsSolver<>( + bodies, propagatorSettings, parametersToEstimate ); + + std::map + stateTransitionMatrixHistory = variationalEquationsSolver.getStateTransitionMatrixSolution( ); + + double positionPerturbation = 10.0; + double velocityPerturbation = 1.0E-2; + + double statePerturbation; + Eigen::VectorXd unperturbedInitialState = propagatorSettings->getInitialStates( ); + Eigen::VectorXd perturbedInitialState; + std::map upperturbedStateHistory, downperturbedStateHistory; + std::map numericalStateTransitionMatrixHistory; + + for ( unsigned int i = 0; i < 6; i++ ) + { + if ( i < 3 ) + { + statePerturbation = positionPerturbation; + } + else + { + statePerturbation = velocityPerturbation; + } + + perturbedInitialState = unperturbedInitialState; + perturbedInitialState( i ) += statePerturbation; + + propagatorSettings->resetInitialStates( perturbedInitialState ); + SingleArcDynamicsSimulator<> upperturbedDynamicsSimulator( bodies, propagatorSettings ); + upperturbedStateHistory = upperturbedDynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + perturbedInitialState = unperturbedInitialState; + perturbedInitialState( i ) -= statePerturbation; + + propagatorSettings->resetInitialStates( perturbedInitialState ); + SingleArcDynamicsSimulator<> downperturbedDynamicsSimulator( bodies, propagatorSettings ); + downperturbedStateHistory = downperturbedDynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + for ( auto it: upperturbedStateHistory ) + { + if ( downperturbedStateHistory.count( it.first ) == 0 ) + { + throw std::runtime_error( "Error in custom acceleration partial test, state size is inconsistent" ); + } + if ( stateTransitionMatrixHistory.count( it.first ) == 0 ) + { + throw std::runtime_error( + "Error in custom acceleration partial test, variational equations size is inconsistent" ); + } + if ( i == 0 ) + { + numericalStateTransitionMatrixHistory[ it.first ] = Eigen::Matrix6d::Zero( ); + } + + numericalStateTransitionMatrixHistory[ it.first ].block( 0, i, 6, 1 ) = + ( upperturbedStateHistory.at( it.first ) - downperturbedStateHistory.at( it.first )) / + ( 2.0 * statePerturbation ); + + } + } + + + double maximumError = 0.0; + for ( auto it: numericalStateTransitionMatrixHistory ) + { + auto matrix1 = it.second; + auto matrix2 = stateTransitionMatrixHistory.at( it.first ); + + for ( int j = 0; j < 6; j++ ) + { + int rowBlock = ( j < 3 ) ? 0 : 1; + for ( int k = 0; k < 6; k++ ) + { + int columnBlock = ( k < 3 ) ? 0 : 1; + double entryDifference = matrix1( j, k ) - matrix2( j, k ); + double blockNorm = matrix1.block( rowBlock * 3, columnBlock * 3, 3, 3 ).norm( ); +// BOOST_CHECK_SMALL( std::fabs( entryDifference ), blockNorm * 1.0E-6 ); + + if ( std::fabs( entryDifference ) / blockNorm > maximumError ) + { + maximumError = std::fabs( entryDifference ) / blockNorm; + } + } + } + } + if( testCase == 0 ) + { + BOOST_CHECK_SMALL( 1.0 / maximumError, 50.0 ); + } + else + { + BOOST_CHECK_SMALL( maximumError, 1.0E-6 ); + } + std::cout << maximumError << std::endl; + } +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} From 8c7c2e40bfea0d48c5d18c086e45a9433637ffd4 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 4 Oct 2023 20:58:18 +0200 Subject: [PATCH 3/8] Added test of custom analytical partial --- .../estimatableParameter.h | 46 +++++++++++-------- .../createAccelerationPartials.h | 17 ++++++- ...CustomAccelerationVariationalEquations.cpp | 41 +++++++++++++---- 3 files changed, 72 insertions(+), 32 deletions(-) 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 ); } From 1310e181cf819e1104699310145b9d1f683602a9 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Thu, 5 Oct 2023 15:48:28 +0200 Subject: [PATCH 4/8] Added check in partial creation --- .../estimation_setup/createAccelerationPartials.h | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h index a4653189d7..f4afba2570 100644 --- a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h +++ b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h @@ -210,13 +210,17 @@ std::shared_ptr< acceleration_partials::AccelerationPartial > createAnalyticalAc std::shared_ptr< acceleration_partials::AccelerationPartial > accelerationPartial = nullptr; std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculatorSet > + customPartialCalculator; + if( parametersToEstimate != nullptr ) + { customPartialCalculator = createCustomAccelerationPartial( parametersToEstimate, accelerationModel, acceleratedBody, acceleratingBody, bodies ); - if( ( customPartialCalculator->getNumberOfCustomPartials( ) > 0 ) && ( accelerationType != custom_acceleration ) ) - { - throw std::runtime_error( "Error, custom acceleration partials only supported for custom acceleration (at present)" ); - } + if( ( customPartialCalculator->getNumberOfCustomPartials( ) > 0 ) && ( accelerationType != custom_acceleration ) ) + { + throw std::runtime_error( "Error, custom acceleration partials only supported for custom acceleration (at present)" ); + } + } switch( accelerationType ) { case point_mass_gravity: From 277e67bd3e9c639cc46bd1bfede9c3e99764ab42 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Sat, 28 Oct 2023 14:19:05 +0200 Subject: [PATCH 5/8] Custom parameter partial seems to be working --- .../customAccelerationPartial.h | 8 +- .../estimatableParameter.h | 130 ++++++++----- .../createAccelerationPartials.h | 38 +++- .../createEstimatableParameters.h | 18 ++ .../estimatableParameterSettings.h | 25 +++ .../estimatableParameter.cpp | 6 + .../createCartesianStatePartials.cpp | 2 + ...CustomAccelerationVariationalEquations.cpp | 183 ++++++++++++++++-- .../unitTestRotationalDynamicsPropagator.cpp | 4 +- 9 files changed, 340 insertions(+), 74 deletions(-) diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h index f82495ff12..07eae6a000 100644 --- a/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/customAccelerationPartial.h @@ -187,11 +187,11 @@ class CustomAccelerationPartial: public AccelerationPartial { std::function< void( Eigen::MatrixXd& ) > partialFunction; int parameterSize = 0; - if( customAccelerationPartialSet_->customDoubleParameterPartials_.count( parameter )!= 0 ) + if( customAccelerationPartialSet_->customDoubleParameterPartials_.count( parameter->getParameterName( ) )!= 0 ) { partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, std::placeholders::_1, - customAccelerationPartialSet_->customDoubleParameterPartials_.at( parameter ) ); + customAccelerationPartialSet_->customDoubleParameterPartials_.at( parameter->getParameterName( ) ) ); parameterSize = 1; } return std::make_pair( partialFunction, parameterSize ); @@ -209,11 +209,11 @@ class CustomAccelerationPartial: public AccelerationPartial { std::function< void( Eigen::MatrixXd& ) > partialFunction; int parameterSize = 0; - if( customAccelerationPartialSet_->customVectorParameterPartials_.count( parameter )!= 0 ) + if( customAccelerationPartialSet_->customVectorParameterPartials_.count( parameter->getParameterName( ) )!= 0 ) { partialFunction = std::bind( &CustomAccelerationPartial::createCustomParameterPartialFunction, this, std::placeholders::_1, - customAccelerationPartialSet_->customVectorParameterPartials_.at( parameter ) ); + customAccelerationPartialSet_->customVectorParameterPartials_.at( parameter->getParameterName( ) ) ); parameterSize = parameter->getParameterSize( ); } return std::make_pair( partialFunction, parameterSize ); diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index 7cd374fe4d..5b946f9054 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -75,7 +75,8 @@ enum EstimatebleParametersEnum constant_time_observation_bias, arc_wise_time_observation_bias, inverse_tidal_quality_factor, - yarkovsky_parameter + yarkovsky_parameter, + custom_estimated_parameter }; std::string getParameterTypeString( const EstimatebleParametersEnum parameterType ); @@ -338,6 +339,85 @@ class EstimatableParameter }; +class CustomEstimatableParameter: public EstimatableParameter< Eigen::VectorXd > +{ + +public: + //! Constructor. + /*! + * Constructor taking parameter name and associated body. All parameters are identified by a these two variables. + * Any additional information that may be required for uniquely defining a parameter is to be defined in the derived class. + * \param parameterName Enum value defining the type of the parameter. + * \param associatedBody Name of body associated with patameters + * \param pointOnBodyId Reference point on body associated with parameter (empty by default). + */ + CustomEstimatableParameter( + const std::string& customId, + const int parameterSize, + const std::function< Eigen::VectorXd( ) > getParameterFunction, + const std::function< void( const Eigen::VectorXd& ) > setParameterFunction ): + EstimatableParameter< Eigen::VectorXd >( custom_estimated_parameter, "", customId ), + parameterSize_( parameterSize ), + getParameterFunction_( getParameterFunction ), + setParameterFunction_( setParameterFunction ){ } + + //! Virtual destructor. + ~CustomEstimatableParameter( ) { } + + //! Pure virtual function to retrieve the value of the parameter + /*! + * Pure virtual function to retrieve the value of the parameter + * \return Current value of parameter. + */ + Eigen::VectorXd getParameterValue( ) + { + Eigen::VectorXd currentParameter = getParameterFunction_( ); + if( currentParameter.rows( ) != parameterSize_ ) + { + throw std::runtime_error( "Error, when getting cutom parameter value, size is incompatible " + + std::to_string( currentParameter.rows( ) ) + ", " + + std::to_string( parameterSize_ ) ); + } + return currentParameter; + } + + //! Pure virtual function to (re)set the value of the parameter. + /*! + * Pure virtual function to (re)set the value of the parameter. + * \param parameterValue to which the parameter is to be set. + */ + void setParameterValue( const Eigen::VectorXd parameterValue ) + { + if( parameterValue.rows( ) != parameterSize_ ) + { + throw std::runtime_error( "Error, when getting cutom parameter value, size is incompatible " + + std::to_string( parameterValue.rows( ) ) + ", " + + std::to_string( parameterSize_ ) ); + } + setParameterFunction_( parameterValue ); + } + + //! Function to retrieve the size of the parameter + /*! + * Pure virtual function to retrieve the size of the parameter (i.e. 1 for double parameters) + * \return Size of parameter value. + */ + int getParameterSize( ) + { + return parameterSize_; + } + + + +protected: + + int parameterSize_; + + std::function< Eigen::VectorXd( ) > getParameterFunction_; + + std::function< void( const Eigen::VectorXd& ) > setParameterFunction_; + +}; class CustomAccelerationPartialCalculator { @@ -358,50 +438,6 @@ class CustomAccelerationPartialCalculator // // basic_astrodynamics::AvailableAcceleration accelerationType_; }; -// -// -//template< typename ParameterScalarType > -//class NumericalAccelerationPartialCalculator: public CustomAccelerationPartialCalculator -//{ -//public: -// NumericalAccelerationPartialCalculator( -// const Eigen::VectorXd& parameterPerturbation, -// const std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter ): -// parameterPerturbation_( parameterPerturbation ), -// parameter_( parameter ) -// { -// if( parameterPerturbation_.rows( ) != parameter_->getParameterSize( ) ) -// { -// throw std::runtime_error( "Error when making numerical acceleration partial for parameter " + -// parameter->getParameterDescription( ) + ", sizes are inconsistent: " + -// std::to_string( parameterPerturbation_.rows( ) ) + ", " + std::to_string( parameter->getParameterSize( ) ) ); -// } -// currentPartial_ = Eigen::MatrixXd::Zero( 3, parameterPerturbation_.rows( ) ); -// } -// -// ~NumericalAccelerationPartialCalculator( ){ } -// -// Eigen::Matrix< double, 3, Eigen::Dynamic > computePartial( -// const double currentTime, const Eigen::Vector3d currentAcceleration, -// const std::shared_ptr< basic_astrodynamics::AccelerationModel3d > accelerationModel ) -// { -//// if constexpr ( std::is_same< double, ParameterScalarType >::value == true ) -//// { -//// currentPartial_.setZero( ); -//// double originalParameterValue = parameter_->getParameterValue( ); -//// -//// parameter_->setParameterValue( originalParameterValue ) -//// } -// } -//protected: -// -// Eigen::VectorXd parameterPerturbation_; -// -// std::shared_ptr< estimatable_parameters::EstimatableParameter< ParameterScalarType > > parameter_; -// -// Eigen::Matrix< double, 3, Eigen::Dynamic > currentPartial_; -// -//}; class NumericalAccelerationPartialWrtStateCalculator: public CustomAccelerationPartialCalculator @@ -532,10 +568,10 @@ class CustomSingleAccelerationPartialCalculatorSet std::map< estimatable_parameters::EstimatebleParameterIdentifier, std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customInitialStatePartials_; - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< double > >, + std::map< estimatable_parameters::EstimatebleParameterIdentifier, std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customDoubleParameterPartials_; - std::map< std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > >, + std::map< estimatable_parameters::EstimatebleParameterIdentifier, std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > > customVectorParameterPartials_; int getNumberOfCustomPartials( ) diff --git a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h index f4afba2570..a503aaba0a 100644 --- a/include/tudat/simulation/estimation_setup/createAccelerationPartials.h +++ b/include/tudat/simulation/estimation_setup/createAccelerationPartials.h @@ -86,10 +86,10 @@ std::shared_ptr< estimatable_parameters::CustomAccelerationPartialCalculator > c } 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" ); - } +// 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_, @@ -135,6 +135,7 @@ std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculat break; } + default: throw std::runtime_error( "Error when creating custom partial calculator for " + parameterSet->getEstimatedInitialStateParameters( ).at( i )->getParameterDescription( ) + @@ -156,11 +157,32 @@ std::shared_ptr< estimatable_parameters::CustomSingleAccelerationPartialCalculat for( unsigned int i = 0; i < parameterSet->getEstimatedVectorParameters( ).size( ); i++ ) { - if( parameterSet->getEstimatedVectorParameters( ).at( i )->getCustomPartialSettings( ) != nullptr ) + + std::shared_ptr< estimatable_parameters::CustomAccelerationPartialSettings > customPartialSettings = + parameterSet->getEstimatedVectorParameters( ).at( i )->getCustomPartialSettings( ); + if( customPartialSettings != nullptr ) { - throw std::runtime_error( "Error when creating custom partial calculator for " + - parameterSet->getEstimatedVectorParameters( ).at( i )->getParameterDescription( ) + - ", parameter not supported for this option" ); + if( customPartialSettings->accelerationMatches( acceleratedBody.first, acceleratingBody.first, accelerationType ) ) + { + switch( parameterSet->getEstimatedVectorParameters( ).at( i )->getParameterName( ).first ) + { + case estimatable_parameters::custom_estimated_parameter: + { + partialCalculatorSet->customVectorParameterPartials_[ parameterSet->getEstimatedVectorParameters( ).at( i )->getParameterName( ) ] = + createCustomAccelerationPartial( + parameterSet->getEstimatedVectorParameters( ).at( i )->getCustomPartialSettings( ), + parameterSet->getEstimatedVectorParameters( ).at( i ), + bodies ); + + break; + } + + default: + throw std::runtime_error( "Error when creating custom partial calculator for " + + parameterSet->getEstimatedVectorParameters( ).at( i )->getParameterDescription( ) + + ", parameter not supported for this option" ); + } + } } } return partialCalculatorSet; diff --git a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h index b9030fcfd8..49c32d25ea 100644 --- a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h +++ b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h @@ -1963,6 +1963,24 @@ std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > } break; } + case custom_estimated_parameter: + { + std::shared_ptr< CustomEstimatableParameterSettings > customParameterSettings = + std::dynamic_pointer_cast< CustomEstimatableParameterSettings >( vectorParameterName ); + if( customParameterSettings == nullptr ) + { + throw std::runtime_error( "Error, expected variable custom parameter settings " ); + } + else + { + vectorParameterToEstimate = std::make_shared< CustomEstimatableParameter > + ( customParameterSettings->parameterType_.second.second, + customParameterSettings->parameterSize_, + customParameterSettings->getParameterFunction_, + customParameterSettings->setParameterFunction_ ); + } + break; + } default: std::string errorMessage = "Warning, this vector parameter (" + std::to_string( vectorParameterName->parameterType_.first ) + diff --git a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h index 5287b658b1..a2df19d79d 100644 --- a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h +++ b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h @@ -988,6 +988,31 @@ class InverseTidalQualityFactorEstimatableParameterSettings: public EstimatableP }; +class CustomEstimatableParameterSettings: public EstimatableParameterSettings +{ +public: + + CustomEstimatableParameterSettings( + const std::string& customId, + const int parameterSize, + const std::function< Eigen::VectorXd( ) > getParameterFunction, + const std::function< void( const Eigen::VectorXd& ) > setParameterFunction ): + EstimatableParameterSettings( "", custom_estimated_parameter, customId ), + parameterSize_( parameterSize ), + getParameterFunction_( getParameterFunction ), + setParameterFunction_( setParameterFunction ) + { + + } + + int parameterSize_; + + std::function< Eigen::VectorXd( ) > getParameterFunction_; + + std::function< void( const Eigen::VectorXd& ) > setParameterFunction_; + +}; + inline std::shared_ptr< EstimatableParameterSettings > gravitationalParameter( const std::string bodyName ) { diff --git a/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp b/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp index b5ddc79e43..651c42606b 100644 --- a/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp +++ b/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp @@ -144,6 +144,9 @@ std::string getParameterTypeString( const EstimatebleParametersEnum parameterTyp case yarkovsky_parameter: parameterDescription = " Yarkovsky parameter A2 "; break; + case custom_estimated_parameter: + parameterDescription = " Custom parameter "; + break; default: std::string errorMessage = "Error when getting parameter string, did not recognize parameter " + std::to_string( parameterType ); @@ -294,6 +297,9 @@ bool isDoubleParameter( const EstimatebleParametersEnum parameterType ) case yarkovsky_parameter: isDoubleParameter = true; break; + case custom_estimated_parameter: + isDoubleParameter = false; + break; default: throw std::runtime_error( "Error, parameter type " + std::to_string( parameterType ) + " not found when getting parameter type" ); diff --git a/src/simulation/estimation_setup/createCartesianStatePartials.cpp b/src/simulation/estimation_setup/createCartesianStatePartials.cpp index b011996e52..dbe3ef6cc2 100644 --- a/src/simulation/estimation_setup/createCartesianStatePartials.cpp +++ b/src/simulation/estimation_setup/createCartesianStatePartials.cpp @@ -285,6 +285,8 @@ std::map< observation_models::LinkEndType, std::shared_ptr< CartesianStatePartia break; case estimatable_parameters::yarkovsky_parameter: break; + case estimatable_parameters::custom_estimated_parameter: + break; default: std::string errorMessage = "Parameter " + std::to_string( diff --git a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp index 6aa01814d6..2074f28ce8 100644 --- a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp +++ b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp @@ -47,22 +47,24 @@ using namespace tudat::ground_stations; using namespace tudat::observation_models; -Eigen::Vector3d customAccelerationFunction( +Eigen::Vector3d getCustomAccelerationFunction( const SystemOfBodies& bodies, - const double time ) + const double time, + const double scaleDistance, + const double coefficient ) { - double scaleDistance = 1.0E6; Eigen::Vector6d relativeState = bodies.at( "Vehicle" )->getState( ) - bodies.at( "Earth" )->getState( ); - Eigen::Vector3d acceleration = Eigen::Vector3d::Ones( ) * std::exp( -relativeState.segment( 0, 3 ).norm( ) / scaleDistance ); + Eigen::Vector3d acceleration = coefficient * Eigen::Vector3d::Ones( ) * std::exp( -relativeState.segment( 0, 3 ).norm( ) / scaleDistance ); return acceleration; } Eigen::MatrixXd getCustomAccelerationPartialFunction( const SystemOfBodies& bodies, const double time, - const Eigen::Vector3d& acceleration ) + const Eigen::Vector3d& acceleration, + const double scaleDistance, + const double coefficient ) { - 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( ); @@ -72,11 +74,78 @@ Eigen::MatrixXd getCustomAccelerationPartialFunction( std::exp( -positionNorm / scaleDistance ); } - return partial; + return coefficient * partial; } +Eigen::MatrixXd getCustomAccelerationPartialFunctionWrtParameter( + const SystemOfBodies& bodies, + const double time, + const Eigen::Vector3d& acceleration, + const double scaleDistance, + const double coefficient ) +{ + return acceleration / coefficient; +} + + +class TestAccelerationModel +{ +public: + TestAccelerationModel( + const SystemOfBodies& bodies, + const double accelerationParameter, + const double coefficient ): + bodies_( bodies ), + accelerationParameter_( accelerationParameter ), + coefficient_( coefficient ){ } + + Eigen::Vector3d customAccelerationFunction( + const double time ) + { + return getCustomAccelerationFunction( bodies_, time, accelerationParameter_, coefficient_ ); + } + + Eigen::MatrixXd customAccelerationPartialFunction( + const double time, + const Eigen::Vector3d& acceleration ) + { + return getCustomAccelerationPartialFunction( bodies_, time, acceleration, accelerationParameter_, coefficient_ ); + } + + Eigen::MatrixXd customAccelerationPartialFunctionWrtParameter( + const double time, + const Eigen::Vector3d& acceleration ) + { + return getCustomAccelerationPartialFunctionWrtParameter( bodies_, time, acceleration, accelerationParameter_, coefficient_ ); + } + + Eigen::VectorXd getParameterValue( ) + { + return ( Eigen::VectorXd( 1 ) << coefficient_ ).finished( ); + } + + void setParameterValue( const Eigen::VectorXd& parameterValue ) + { + if( parameterValue.rows( ) != 1 ) + { + throw std::runtime_error( "Error in custom parameter, size is consistent" ); + } + coefficient_ = parameterValue( 0 ); + } + +protected: + SystemOfBodies bodies_; + + double accelerationParameter_; + + double coefficient_; +}; + BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) { + double scaleDistance = 1.0E6; + double coefficient = 0.2; + //Load spice kernels. spice_interface::loadStandardSpiceKernels( ); @@ -87,7 +156,7 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) // Specify initial time double initialEphemerisTime = double( 1.0E7 ); - double finalEphemerisTime = initialEphemerisTime + 1.0 * 86400.0; + double finalEphemerisTime = initialEphemerisTime + 86400.0; // Create bodies needed in simulation BodyListSettings bodySettings = @@ -97,6 +166,8 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) bodies.createEmptyBody( "Vehicle" ); bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 ); + TestAccelerationModel testAccelerationModel( bodies, scaleDistance, coefficient ); + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////// CREATE ACCELERATIONS ////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -107,7 +178,7 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); accelerationsOfVehicle[ "Vehicle" ].push_back( std::make_shared< CustomAccelerationSettings >( - std::bind( &customAccelerationFunction, bodies, std::placeholders::_1 ) ) ); + std::bind( &TestAccelerationModel::customAccelerationFunction, &testAccelerationModel, std::placeholders::_1 ) ) ); // accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( // basic_astrodynamics::point_mass_gravity ) ); accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; @@ -155,7 +226,7 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) std::shared_ptr< IntegratorSettings< double > > integratorSettings = rungeKuttaFixedStepSettings( 90.0, CoefficientSets::rungeKuttaFehlberg78 ); - for( int testCase = 0; testCase < 3; testCase ++ ) + for( int testCase = 0; testCase < 4; testCase ++ ) { std::shared_ptr > propagatorSettings = @@ -183,7 +254,23 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) { parameterNames.at( 0 )->customPartialSettings_ = std::make_shared( - std::bind( &getCustomAccelerationPartialFunction, bodies, std::placeholders::_1, std::placeholders::_2 ), + std::bind( &TestAccelerationModel::customAccelerationPartialFunction, &testAccelerationModel, std::placeholders::_1, std::placeholders::_2 ), + "Vehicle", "Vehicle", custom_acceleration ); + } + else if( testCase == 3 ) + { + parameterNames.push_back( + std::make_shared< CustomEstimatableParameterSettings >( + "CustomModel", 1, + std::bind( &TestAccelerationModel::getParameterValue, &testAccelerationModel ), + std::bind( &TestAccelerationModel::setParameterValue, &testAccelerationModel, std::placeholders::_1 ) ) ); + parameterNames.at( 0 )->customPartialSettings_ = + std::make_shared( + std::bind( &TestAccelerationModel::customAccelerationPartialFunction, &testAccelerationModel, std::placeholders::_1, std::placeholders::_2 ), + "Vehicle", "Vehicle", custom_acceleration ); + parameterNames.at( 1 )->customPartialSettings_ = + std::make_shared( + std::bind( &TestAccelerationModel::customAccelerationPartialFunctionWrtParameter, &testAccelerationModel, std::placeholders::_1, std::placeholders::_2 ), "Vehicle", "Vehicle", custom_acceleration ); } @@ -201,6 +288,9 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) std::map stateTransitionMatrixHistory = variationalEquationsSolver.getStateTransitionMatrixSolution( ); + std::map + sensitivityMatrixHistory = variationalEquationsSolver.getSensitivityMatrixSolution( ); + double positionPerturbation = 10.0; double velocityPerturbation = 1.0E-2; @@ -209,6 +299,7 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) Eigen::VectorXd perturbedInitialState; std::map upperturbedStateHistory, downperturbedStateHistory; std::map numericalStateTransitionMatrixHistory; + std::map numericalSensitivityMatrixHistory; for ( unsigned int i = 0; i < 6; i++ ) { @@ -258,6 +349,74 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) } } + propagatorSettings->resetInitialStates( unperturbedInitialState ); + + if( testCase == 3 ) + { + std::cout<getFullParameterValues< double >( ); + + Eigen::VectorXd upperturbedParameters = nominalParameters; + upperturbedParameters( 6 ) += paramaterPerturbation; + parametersToEstimate->resetParameterValues( upperturbedParameters ); + + SingleArcDynamicsSimulator<> upperturbedDynamicsSimulator( bodies, propagatorSettings ); + upperturbedStateHistory = upperturbedDynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + Eigen::VectorXd downperturbedParameters = nominalParameters; + downperturbedParameters( 6 ) -= paramaterPerturbation; + parametersToEstimate->resetParameterValues( downperturbedParameters ); + + SingleArcDynamicsSimulator<> downperturbedDynamicsSimulator( bodies, propagatorSettings ); + downperturbedStateHistory = downperturbedDynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + + double maximumSensivitiyError = 0.0; + + for ( auto it: upperturbedStateHistory ) + { + if ( downperturbedStateHistory.count( it.first ) == 0 ) + { + throw std::runtime_error( "Error in custom acceleration partial test, state size is inconsistent" ); + } + if ( stateTransitionMatrixHistory.count( it.first ) == 0 ) + { + throw std::runtime_error( + "Error in custom acceleration partial test, variational equations size is inconsistent" ); + } + + + + numericalSensitivityMatrixHistory[ it.first ] = + ( upperturbedStateHistory.at( it.first ) - downperturbedStateHistory.at( it.first )) / + ( 2.0 * paramaterPerturbation ); + + auto matrix1 = numericalSensitivityMatrixHistory[ it.first ]; + auto matrix2 = sensitivityMatrixHistory.at( it.first ); + + if( it.first == upperturbedStateHistory.begin( )->first ) + { + for ( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_EQUAL( matrix1( i ), 0.0 ); + BOOST_CHECK_EQUAL( matrix2( i ), 0.0 ); + } + } + + double blockNorm = matrix2.norm( ); + for ( unsigned int i = 0; i < 3; i++ ) + { + double entryDifference = matrix1( i ) - matrix2( i ); + + if ( std::fabs( entryDifference ) / blockNorm > maximumSensivitiyError ) + { + maximumSensivitiyError = std::fabs( entryDifference ) / blockNorm; + } + } + } + BOOST_CHECK_SMALL( maximumSensivitiyError, 1.0E-5 ); + + } double maximumError = 0.0; for ( auto it: numericalStateTransitionMatrixHistory ) @@ -273,7 +432,6 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) int columnBlock = ( k < 3 ) ? 0 : 1; double entryDifference = matrix1( j, k ) - matrix2( j, k ); double blockNorm = matrix1.block( rowBlock * 3, columnBlock * 3, 3, 3 ).norm( ); -// BOOST_CHECK_SMALL( std::fabs( entryDifference ), blockNorm * 1.0E-6 ); if ( std::fabs( entryDifference ) / blockNorm > maximumError ) { @@ -294,7 +452,6 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) { BOOST_CHECK_SMALL( maximumError, 1.0E-6 ); } - std::cout << maximumError << std::endl; } } diff --git a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp index 6d10dda37e..63fb313575 100755 --- a/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp +++ b/tests/src/astro/propagators/unitTestRotationalDynamicsPropagator.cpp @@ -1144,7 +1144,7 @@ BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithVaryinInertiaTe Eigen::Matrix3d nominalInertiaTensor; Eigen::Vector3d nominalCosineCorrection; - Eigen::Vector3d nominalSineCorrection; + Eigen::Vector2d nominalSineCorrection; std::shared_ptr< gravitation::SphericalHarmonicsGravityField > phobosGravityField = std::dynamic_pointer_cast< gravitation::SphericalHarmonicsGravityField >( @@ -1163,7 +1163,7 @@ BOOST_AUTO_TEST_CASE( testSimpleRotationalDynamicsPropagationWithVaryinInertiaTe Eigen::Vector3d secondDegreeTorque = it.second.segment( 9, 3 ); Eigen::Vector3d totalTorque = it.second.segment( 12, 3 ); Eigen::Vector3d cosineCorrection = it.second.segment( 15, 3 ); - Eigen::Vector3d sineCorrection = it.second.segment( 18, 2 ); + Eigen::Vector2d sineCorrection = it.second.segment( 18, 2 ); Eigen::Vector3d relativePosition = it.second.segment( 20, 3 ); Eigen::Vector3d angularVelocity = stateHistory.at( it.first ).segment( 4, 3 ); From 235de7c76c18563095155c74af5e33eff38629f5 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 30 Oct 2023 15:50:12 +0100 Subject: [PATCH 6/8] Added custom estimation test --- ...CustomAccelerationVariationalEquations.cpp | 178 +++++++++++++++++- .../unitTestRotationalDynamicsPropagator.cpp | 2 +- 2 files changed, 178 insertions(+), 2 deletions(-) diff --git a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp index 2074f28ce8..182b0d4af4 100644 --- a/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp +++ b/tests/src/astro/orbit_determination/unitTestCustomAccelerationVariationalEquations.cpp @@ -141,7 +141,7 @@ class TestAccelerationModel double coefficient_; }; -BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) +BOOST_AUTO_TEST_CASE( test_CustomAccelerationPartials ) { double scaleDistance = 1.0E6; double coefficient = 0.2; @@ -455,6 +455,182 @@ BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) } } + +BOOST_AUTO_TEST_CASE( test_CustomAccelerationEstimation ) +{ + double scaleDistance = 1.0E6; + double coefficient = 0.2; + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + // Define bodies in simulation + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + + // Specify initial time + double initialEphemerisTime = double( 1.0E7 ); + double finalEphemerisTime = initialEphemerisTime + 86400.0; + + // Create bodies needed in simulation + BodyListSettings bodySettings = + getDefaultBodySettings( bodyNames, "Earth" ); + + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + bodies.createEmptyBody( "Vehicle" ); + bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 ); + + TestAccelerationModel testAccelerationModel( bodies, scaleDistance, coefficient ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// CREATE ACCELERATIONS ////////////////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + // 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< AccelerationSettings >( + basic_astrodynamics::point_mass_gravity ) ); + accelerationsOfVehicle[ "Vehicle" ].push_back( std::make_shared< CustomAccelerationSettings >( + std::bind( &TestAccelerationModel::customAccelerationFunction, &testAccelerationModel, std::placeholders::_1 ) ) ); + + 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 ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// CREATE PROPAGATION SETTINGS //////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + // Set Keplerian elements for Asterix. + Eigen::Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.05; + 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::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + + // Create integrator settings + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + rungeKuttaFixedStepSettings( 90.0, CoefficientSets::rungeKuttaFehlberg78 ); + + std::shared_ptr > propagatorSettings = + std::make_shared >( + centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, initialEphemerisTime, + integratorSettings, + std::make_shared( finalEphemerisTime ), + cowell, dependentVariables ); + + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + /////////////////////// DEFINE PARAMETERS THAT ARE TO BE ESTIMATED //////////////////////////////////////////// + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + // Define list of parameters to estimate. + std::vector > parameterNames; + parameterNames = getInitialStateParameterSettings( propagatorSettings, bodies ); + parameterNames.push_back( + std::make_shared< CustomEstimatableParameterSettings >( + "CustomModel", 1, + std::bind( &TestAccelerationModel::getParameterValue, &testAccelerationModel ), + std::bind( &TestAccelerationModel::setParameterValue, &testAccelerationModel, std::placeholders::_1 ) ) ); + + parameterNames.at( 0 )->customPartialSettings_ = + std::make_shared( + std::bind( &TestAccelerationModel::customAccelerationPartialFunction, &testAccelerationModel, std::placeholders::_1, std::placeholders::_2 ), + "Vehicle", "Vehicle", custom_acceleration ); + parameterNames.at( 1 )->customPartialSettings_ = + std::make_shared( + std::bind( &TestAccelerationModel::customAccelerationPartialFunctionWrtParameter, &testAccelerationModel, std::placeholders::_1, std::placeholders::_2 ), + "Vehicle", "Vehicle", custom_acceleration ); + + // Create parameters + std::shared_ptr > parametersToEstimate = + createParametersToEstimate( parameterNames, bodies ); + + LinkEnds observationLinkEnds; + observationLinkEnds[ observed_body ] = LinkEndId( "Vehicle" ); + + LinkDefinition linkDefinition( observationLinkEnds ); + std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; + observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + position_observable, linkDefinition ) ); + + OrbitDeterminationManager< double, double > estimator( bodies, parametersToEstimate, observationSettingsList, propagatorSettings ); + + + std::vector< double > observationTimes; + double currentTime = initialEphemerisTime + 1800.0; + double observationTimeStep = 60.0; + while( currentTime < finalEphemerisTime - 1800.0 ) + { + observationTimes.push_back( currentTime ); + currentTime += observationTimeStep; + } + std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput; + measurementSimulationInput.push_back( + std::make_shared< TabulatedObservationSimulationSettings< > >( + position_observable, linkDefinition, observationTimes, observed_body ) ); + + // Simulate observations + std::shared_ptr< ObservationCollection< > > observationsAndTimes = simulateObservations< double, double >( + measurementSimulationInput, estimator.getObservationSimulators( ), bodies ); + + // Perturb parameter estimate + Eigen::Matrix< double, Eigen::Dynamic, 1 > initialParameterEstimate = + parametersToEstimate->template getFullParameterValues< double >( ); + Eigen::Matrix< double, Eigen::Dynamic, 1 > truthParameters = initialParameterEstimate; + int numberOfParameters = initialParameterEstimate.rows( ); + initialParameterEstimate( 0 ) += 1.0; + initialParameterEstimate( 1 ) += 1.0; + initialParameterEstimate( 2 ) += 1.0; +// initialParameterEstimate( 6 ) += 0.5; + + + parametersToEstimate->resetParameterValues( initialParameterEstimate ); + + // Define estimation input + std::shared_ptr< EstimationInput< double, double > > estimationInput = + std::make_shared< EstimationInput< double, double > >( + observationsAndTimes ); + estimationInput->setConvergenceChecker( + std::make_shared< EstimationConvergenceChecker >( 5 ) ); + + // Perform estimation + std::shared_ptr< EstimationOutput< double > > estimationOutput = estimator.estimateParameters( + estimationInput ); + Eigen::VectorXd finalError = truthParameters - parametersToEstimate->template getFullParameterValues< double >( ); + std::cout< Date: Mon, 30 Oct 2023 16:04:52 +0100 Subject: [PATCH 7/8] Added exposure of new functionality for custom partials --- .../estimatableParameter.h | 23 +++++++++++++++++++ .../estimatableParameterSettings.h | 12 ++++++++++ 2 files changed, 35 insertions(+) diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index 5b946f9054..b2a04f1cad 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -208,6 +208,29 @@ class AnalyticalAccelerationPartialSettings: public CustomAccelerationPartialSet std::function< Eigen::MatrixXd( const double, const Eigen::Vector3d& ) > accelerationPartialFunction_; }; + +inline std::shared_ptr< CustomAccelerationPartialSettings > analyticalAccelerationPartialSettings( + const std::function< Eigen::MatrixXd( const double, const Eigen::Vector3d& ) > accelerationPartialFunction, + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType ) +{ + return std::make_shared< AnalyticalAccelerationPartialSettings >( + accelerationPartialFunction, bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType ); +} + +inline std::shared_ptr< CustomAccelerationPartialSettings > numericalAccelerationPartialSettings( + const Eigen::VectorXd& parameterPerturbation, + const std::string bodyUndergoingAcceleration, + const std::string bodyExertingAcceleration, + const basic_astrodynamics::AvailableAcceleration accelerationType, + const std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >& environmentUpdateSettings = + std::map< propagators::EnvironmentModelsToUpdate, std::vector< std::string > >( ) ) +{ + return std::make_shared< NumericalAccelerationPartialSettings >( + parameterPerturbation, bodyUndergoingAcceleration, bodyExertingAcceleration, accelerationType, environmentUpdateSettings ); +} + //! Base class for a parameter that is to be estimated. /*! * Base class for a parameter that is to be estimated. A separate derived class is to be made for each type of parameter diff --git a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h index a2df19d79d..96e788fe16 100644 --- a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h +++ b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h @@ -1402,6 +1402,18 @@ inline std::shared_ptr< EstimatableParameterSettings > yarkovskyParameter( const return std::make_shared< EstimatableParameterSettings >( bodyName, yarkovsky_parameter, centralBodyName ); } +inline std::shared_ptr< EstimatableParameterSettings > customParameterSettings( + const std::string& customId, + const int parameterSize, + const std::function< Eigen::VectorXd( ) > getParameterFunction, + const std::function< void( const Eigen::VectorXd& ) > setParameterFunction ) +{ + std::make_shared( + customId, parameterSize, getParameterFunction, setParameterFunction ); +} + + + } // namespace estimatable_parameters From 78f6929f8db53a36a76336172a4af70b0931f50c Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Mon, 30 Oct 2023 16:38:41 +0100 Subject: [PATCH 8/8] Small corrections --- .../simulation/estimation_setup/estimatableParameterSettings.h | 2 +- tests/src/astro/basic_astro/unitTestDateTime.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h index 96e788fe16..e7b26832f6 100644 --- a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h +++ b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h @@ -1408,7 +1408,7 @@ inline std::shared_ptr< EstimatableParameterSettings > customParameterSettings( const std::function< Eigen::VectorXd( ) > getParameterFunction, const std::function< void( const Eigen::VectorXd& ) > setParameterFunction ) { - std::make_shared( + return std::make_shared( customId, parameterSize, getParameterFunction, setParameterFunction ); } diff --git a/tests/src/astro/basic_astro/unitTestDateTime.cpp b/tests/src/astro/basic_astro/unitTestDateTime.cpp index 748596043d..d8a0a317e1 100644 --- a/tests/src/astro/basic_astro/unitTestDateTime.cpp +++ b/tests/src/astro/basic_astro/unitTestDateTime.cpp @@ -179,7 +179,7 @@ BOOST_AUTO_TEST_CASE( testIsoInitialization ) for( unsigned int i = 0; i < testStrings.size( ); i++ ) { DateTime dateTime = dateTimeFromIsoString( testStrings.at( i ) ); - std::string reconstuctedString = dateTime.isoString( true ); + std::string reconstuctedString = dateTime.isoString( true, 17 ); // TODO: fix test for long doubles with 64-bit precision if (sizeof(long double) > 8)