diff --git a/.bumpversion.cfg b/.bumpversion.cfg index 859cd5a88e..0f6a8a5f89 100644 --- a/.bumpversion.cfg +++ b/.bumpversion.cfg @@ -1,5 +1,5 @@ [bumpversion] -current_version = 2.13.0.dev11 +current_version = 2.13.0.dev17 commit = True tag = True parse = (?P\d+)\.(?P\d+)\.(?P\d+)(\.(?P[a-z]+)(?P\d+))? diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index a91e9cf7e7..13d70e8f64 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -98,7 +98,7 @@ jobs: - name: Create conda environment from environment.yml # Update the tudat conda environment created using the environment.yml file if the cache is not restored run: mamba env update -n tudat -f environment.yml - if: steps.cache.outputs.cache-hit != 'true' + #(commenting line for test) if: steps.cache.outputs.cache-hit != 'true' - name: Download tudat resources data files for Linux and MacOS @@ -110,12 +110,12 @@ jobs: # be downloaded manually when the cache is hit. # This step downloads the data files in the tudat-resources package by running # the post-link scripts in https://github.com/tudat-team/tudat-resources-feedstock/blob/master/recipe/ - if: (runner.os == 'Linux' || runner.os == 'MacOS') && steps.cache.outputs.cache-hit == 'true' + if: (runner.os == 'Linux' || runner.os == 'MacOS') #(commenting line for test) && steps.cache.outputs.cache-hit == 'true' run: bash -c "$(curl -fsSL https://raw.githubusercontent.com/tudat-team/tudat-resources-feedstock/master/recipe/post-link.sh)" - name: Download tudat-resources data files for Windows - if: runner.os == 'Windows' && steps.cache.outputs.cache-hit == 'true' + if: runner.os == 'Windows' #(commenting line for test) && steps.cache.outputs.cache-hit == 'true' run: cmd /c "curl -fsSL https://raw.githubusercontent.com/tudat-team/tudat-resources-feedstock/master/recipe/post-link.bat | cmd" @@ -151,4 +151,4 @@ jobs: working-directory: "${{ github.workspace }}/build" # Execute tests defined by the CMake configuration. Note that --build-config is needed because the default Windows generator is a multi-config generator (Visual Studio generator). # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail - run: ctest --build-config ${{ matrix.build_type }} --output-on-failure -j4 \ No newline at end of file + run: ctest --build-config ${{ matrix.build_type }} --output-on-failure -j4 diff --git a/CMakeLists.txt b/CMakeLists.txt index 217e82a328..c126df6705 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -74,9 +74,6 @@ option(TUDAT_BUILD_WITH_SOFA_INTERFACE "Build Tudat with sofa interface." ON) # Build json interface. option(TUDAT_BUILD_WITH_JSON_INTERFACE "Build Tudat with json interface." OFF) -# Build with nrlmsise-00 atmosphere model. -option(TUDAT_BUILD_WITH_NRLMSISE00 "Build with nrlmsise-00 atmosphere model." OFF) - # Build pagmo-dependent code option(TUDAT_BUILD_WITH_PAGMO "Build Tudat with pagmo." OFF) if(CMAKE_CXX_SIMULATE_ID MATCHES "MSVC") @@ -120,12 +117,10 @@ message(STATUS "TUDAT_BUILD_STATIC_LIBRARY ${TUDAT_BU message(STATUS "TUDAT_BUILD_WITH_FILTERS ${TUDAT_BUILD_WITH_FILTERS}") message(STATUS "TUDAT_BUILD_WITH_SOFA_INTERFACE ${TUDAT_BUILD_WITH_SOFA_INTERFACE}") message(STATUS "TUDAT_BUILD_WITH_JSON_INTERFACE ${TUDAT_BUILD_WITH_JSON_INTERFACE}") -message(STATUS "TUDAT_BUILD_WITH_NRLMSISE00 ${TUDAT_BUILD_WITH_NRLMSISE00}") message(STATUS "TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS ${TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS}") message(STATUS "TUDAT_DOWNLOAD_AND_BUILD_BOOST ${TUDAT_DOWNLOAD_AND_BUILD_BOOST}") set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_FILTERS=${TUDAT_BUILD_WITH_FILTERS}") -set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_NRLMSISE00=${TUDAT_BUILD_WITH_NRLMSISE00}") set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_SOFA_INTERFACE=${TUDAT_BUILD_WITH_SOFA_INTERFACE}") set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_JSON_INTERFACE=${TUDAT_BUILD_WITH_JSON_INTERFACE}") set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS=${TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS}") @@ -134,13 +129,19 @@ set(Tudat_DEFINITIONS "${Tudat_DEFINITIONS} -DTUDAT_BUILD_WITH_EXTENDED_PRECISIO # Offer the user the choice of overriding the installation directories. # +============================================================================ # TODO: Consider if this can work with going forward. -set(INSTALL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/lib/" CACHE PATH "Installation directory for libraries") -set(INSTALL_BIN_DIR "${CMAKE_INSTALL_PREFIX}/bin/" CACHE PATH "Installation directory for executables") -set(INSTALL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/include/" CACHE PATH "Installation directory for header files") -set(INSTALL_DATA_DIR "${CMAKE_INSTALL_PREFIX}/data/" CACHE PATH "Installation directory for data files") -set(INSTALL_MAN_DIR "${CMAKE_INSTALL_PREFIX}/" CACHE PATH "Installation directory for man documentation") -#set(INSTALL_TESTS_DIR "${CMAKE_INSTALL_PREFIX}/tests/" CACHE PATH "Installation directory for tests (default=OFF)") -set(INSTALL_CMAKE_DIR "${INSTALL_LIB_DIR}/cmake/${PROJECT_NAME_LOWER}" CACHE PATH "Installation directory for cmake config files") + +set(INSTALL_LIB_DIR "${CMAKE_INSTALL_PREFIX}/lib") +set(INSTALL_BIN_DIR "${CMAKE_INSTALL_PREFIX}/bin") +set(INSTALL_INCLUDE_DIR "${CMAKE_INSTALL_PREFIX}/include") +set(INSTALL_DATA_DIR "${CMAKE_INSTALL_PREFIX}/data") +set(INSTALL_MAN_DIR "${CMAKE_INSTALL_PREFIX}") +#set(INSTALL_TESTS_DIR "${CMAKE_INSTALL_PREFIX}/tests") +set(INSTALL_CMAKE_DIR "${INSTALL_LIB_DIR}/cmake/${PROJECT_NAME_LOWER}") + +message("") +message(STATUS "INSTALLATION PREFIX: ${CMAKE_INSTALL_PREFIX}") +message("") + # Make relative paths absolute (needed later on) foreach (p LIB BIN INCLUDE DATA CMAKE) @@ -321,13 +322,10 @@ if (TUDAT_BUILD_WITH_SOFA_INTERFACE) add_definitions(-DTUDAT_BUILD_WITH_SOFA_INTERFACE=1) endif () -if (TUDAT_BUILD_WITH_NRLMSISE00) - find_package(nrlmsise00 REQUIRED 0.1) - message(STATUS ${NRLMSISE00_LIBRARIES}) - add_definitions(-DTUDAT_BUILD_WITH_NRLMSISE=1) -else( ) - add_definitions(-DTUDAT_BUILD_WITH_NRLMSISE=0) -endif () +find_package(nrlmsise00 REQUIRED 0.1) +message(STATUS ${NRLMSISE00_LIBRARIES}) +add_definitions(-DTUDAT_BUILD_WITH_NRLMSISE=1) + # JSON if (TUDAT_BUILD_WITH_JSON_INTERFACE) diff --git a/cmake_modules/TudatLinkLibraries.cmake b/cmake_modules/TudatLinkLibraries.cmake index 538f3b3429..b910e36f91 100644 --- a/cmake_modules/TudatLinkLibraries.cmake +++ b/cmake_modules/TudatLinkLibraries.cmake @@ -23,9 +23,7 @@ if (TUDAT_BUILD_WITH_SOFA_INTERFACE) list(APPEND TUDAT_ITRS_LIBRARIES Tudat::tudat_earth_orientation) endif () -if (TUDAT_BUILD_WITH_NRLMSISE00) - list(APPEND TUDAT_EXTERNAL_INTERFACE_LIBRARIES ${NRLMSISE00_LIBRARIES}) -endif () +list(APPEND TUDAT_EXTERNAL_INTERFACE_LIBRARIES ${NRLMSISE00_LIBRARIES}) if (TUDAT_BUILD_WITH_JSON_INTERFACE) diff --git a/cmake_modules/compiler.cmake b/cmake_modules/compiler.cmake index 67e82d2700..91e3f5dcb4 100644 --- a/cmake_modules/compiler.cmake +++ b/cmake_modules/compiler.cmake @@ -367,7 +367,7 @@ if (MSVC) add_compile_options(/bigobj) else() set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0") + #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-backtrace-limit=0") endif () diff --git a/include/tudat/astro/aerodynamics.h b/include/tudat/astro/aerodynamics.h index e67b081b51..5f777108a2 100644 --- a/include/tudat/astro/aerodynamics.h +++ b/include/tudat/astro/aerodynamics.h @@ -26,8 +26,8 @@ #include "aerodynamics/exponentialAtmosphere.h" #include "aerodynamics/flightConditions.h" #include "aerodynamics/hypersonicLocalInclinationAnalysis.h" -//#include "aerodynamics/nrlmsise00Atmosphere.h" -//#include "aerodynamics/nrlmsise00InputFunctions.h" +#include "aerodynamics/nrlmsise00Atmosphere.h" +#include "aerodynamics/nrlmsise00InputFunctions.h" #include "aerodynamics/standardAtmosphere.h" #include "aerodynamics/tabulatedAtmosphere.h" #include "aerodynamics/trimOrientation.h" diff --git a/include/tudat/astro/aerodynamics/nrlmsise00Atmosphere.h b/include/tudat/astro/aerodynamics/nrlmsise00Atmosphere.h index 6b6172fd05..5261c142fd 100644 --- a/include/tudat/astro/aerodynamics/nrlmsise00Atmosphere.h +++ b/include/tudat/astro/aerodynamics/nrlmsise00Atmosphere.h @@ -417,7 +417,20 @@ class NRLMSISE00Atmosphere : public AtmosphereModel return inputData_; } - private: + NRLMSISE00InputFunction getNrlmsise00InputFunction( ) + { + return nrlmsise00InputFunction_; + } + + nrlmsise_input getNRLMSISE00InputStruct( ) + { + return input_; + } + + void setInputStruct( const double altitude, const double longitude, + const double latitude, const double time ); + +private: //! Shared pointer to solar activity function NRLMSISE00InputFunction nrlmsise00InputFunction_; @@ -565,6 +578,7 @@ class NRLMSISE00Atmosphere : public AtmosphereModel return seed; } + //! Compute the local atmospheric properties. /*! * Computes the local atmospheric density, pressure and temperature. @@ -586,6 +600,11 @@ class NRLMSISE00Atmosphere : public AtmosphereModel }; +Eigen::VectorXd getNrlmsiseInputAsVector( const nrlmsise_input& input ); + + + + } // namespace aerodynamics } // namespace tudat diff --git a/include/tudat/astro/gravitation/basicSolidBodyTideGravityFieldVariations.h b/include/tudat/astro/gravitation/basicSolidBodyTideGravityFieldVariations.h index 31e30b5beb..253133caa9 100644 --- a/include/tudat/astro/gravitation/basicSolidBodyTideGravityFieldVariations.h +++ b/include/tudat/astro/gravitation/basicSolidBodyTideGravityFieldVariations.h @@ -104,9 +104,9 @@ std::pair< Eigen::MatrixXd, Eigen::MatrixXd > calculateSolidBodyTideSingleCoeffi const double referenceRadius, const Eigen::Vector3d& relativeBodyFixedPosition, const int maximumDegree, const int maximumOrder ); -//! Class to calculate first-order solid body tide gravity field variations on a single body raised -//! by any number of bodies up to any degree and order. -class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations + + +class SolidBodyTideGravityFieldVariations: public GravityFieldVariations { public: //! Constructor @@ -122,50 +122,41 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * deformed. * \param deformingBodyMasses List of functions returning masses (or gravitational parameters) * of bodies causing deformation. - * \param loveNumbers List of love numbers for each degree and order. First vector level - * denotes degree (index 0 = degree 2), second vector level denotes order and must be of maximum size - * (loveNumbers.size( ) + 2, i.e. maximum degree >= maximum order) * \param deformingBodies List of names of bodies causing deformation */ - BasicSolidBodyTideGravityFieldVariations( - const std::function< Eigen::Vector6d( const double ) > - deformedBodyStateFunction, - const std::function< Eigen::Quaterniond( const double ) > - deformedBodyOrientationFunction, - const std::vector< std::function< Eigen::Vector6d( const double ) > > - deformingBodyStateFunctions, - const double deformedBodyReferenceRadius, - const std::function< double( ) > deformedBodyMass, - const std::vector< std::function< double( ) > > deformingBodyMasses, - const std::map< int, std::vector< std::complex< double > > > loveNumbers, - const std::vector< std::string > deformingBodies ): - // LOVE NUMBERS TODO: FIX MIN/MAX STUFF - GravityFieldVariations( 2, 0, loveNumbers.rbegin( )->first, loveNumbers.rbegin( )->first ), + SolidBodyTideGravityFieldVariations( + const std::function< Eigen::Vector6d( const double ) > + deformedBodyStateFunction, + const std::function< Eigen::Quaterniond( const double ) > + deformedBodyOrientationFunction, + const std::vector< std::function< Eigen::Vector6d( const double ) > > + deformingBodyStateFunctions, + const double deformedBodyReferenceRadius, + const std::function< double( ) > deformedBodyMass, + const std::vector< std::function< double( ) > > deformingBodyMasses, + const std::vector< std::string > deformingBodies, + const int maximumDegree, const int maximumOrder ): + // LOVE NUMBERS TODO: FIX MIN/MAX STUFF + GravityFieldVariations( 2, 0, maximumDegree, maximumOrder ), deformedBodyStateFunction_( deformedBodyStateFunction ), deformedBodyOrientationFunction_( deformedBodyOrientationFunction ), deformingBodyStateFunctions_( deformingBodyStateFunctions ), deformedBodyReferenceRadius_( deformedBodyReferenceRadius ), deformedBodyMass_( deformedBodyMass ), deformingBodyMasses_( deformingBodyMasses ), - loveNumbers_( loveNumbers ), deformingBodies_( deformingBodies ) { - // Set basic deformation functon as function to be evaluated when requesting variations. - correctionFunctions.push_back( - std::bind( - &BasicSolidBodyTideGravityFieldVariations::addBasicSolidBodyTideCorrections, - this, std::placeholders::_1, std::placeholders::_2 ) ); currentCosineCorrections_ = Eigen::MatrixXd::Zero( - maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ); + maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ); currentSineCorrections_ = Eigen::MatrixXd::Zero( - maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ); + maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ); } //! Destructor /*! * Destructor */ - virtual ~BasicSolidBodyTideGravityFieldVariations( ){ } + virtual ~SolidBodyTideGravityFieldVariations( ){ } //! Function for calculating basic spherical harmonic coefficient corrections. /*! @@ -174,7 +165,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * \return Pair of matrices containing variations in (cosine,sine) coefficients. */ std::pair< Eigen::MatrixXd, Eigen::MatrixXd > calculateBasicSphericalHarmonicsCorrections( - const double time ); + const double time ); //! Derived function for calculating spherical harmonic coefficient corrections. /*! @@ -183,68 +174,11 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * \return Pair of matrices containing variations in (cosine,sine) coefficients. */ virtual std::pair< Eigen::MatrixXd, Eigen::MatrixXd > calculateSphericalHarmonicsCorrections( - const double time ) + const double time ) { return calculateBasicSphericalHarmonicsCorrections( time ); } - //! Function to retrieve the love numbers at given degree. - /*! - * Function to retrieve the love numbers at given degree. Returns a vector containing (complex) - * love numbers at all orders in current degree. - * \param degree Degree from which love numbers are to be retrieved. - * \return Vector of love numbers (i^{th} entry representing i^{th} order in requested degree) - * containing love numbers at current degree. - */ - std::vector< std::complex< double > > getLoveNumbersOfDegree( const int degree ) - { - return loveNumbers_[ degree ]; - } - - //! Function to return all love numbers. - /*! - * Function to return all love numbers, i.e. at all degrees and orders. - * \return Complete set of available love numbers. - */ - const std::map< int, std::vector< std::complex< double > > > getLoveNumbers( ) - { - return loveNumbers_; - } - - //! Function to reset the love numbers at given degree. - /*! - * Function to reset the love numbers at given degree. Input requires a vector containing - * (complex) love numbers at all orders in current degree. - * \param degree Degree from which love numbers are to be retrieved. - * \param loveNumbers Vector of love numbers (i^{th} entry representing i^{th} order in requested degree) - * containing new love numbers at current degree. - */ - void resetLoveNumbersOfDegree( const std::vector< std::complex< double > > loveNumbers, - const int degree ) - { - if( loveNumbers_.count( degree ) == 0 ) - { - std::string errorMessage = "Error, tried to set love numbers at degree " + - std::to_string( degree ) + - " in BasicSolidBodyTideGravityFieldVariations: not available"; - throw std::runtime_error( errorMessage ); - } - else - { - if( loveNumbers.size( ) <= static_cast< unsigned int >( degree + 1 ) ) - { - loveNumbers_[ degree ] = loveNumbers; - } - else - { - std::string errorMessage = "Error, tried to set love numbers at degree " + - std::to_string( degree ) + " in BasicSolidBodyTideGravityFieldVariations with" + - std::to_string( loveNumbers.size( ) ) + " orders"; - throw std::runtime_error( errorMessage ); - } - } - } - //! Function to return reference radius the spherical harmonic gravity field of deformed body. /*! * Function to return reference radius (typically equatorial) the spherical harmonic gravity @@ -335,7 +269,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations id += deformingBodies_[ i ]; if( i != deformingBodies_.size( ) - 1 ) { - id += "_"; + id += "_"; } } @@ -373,7 +307,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * the required (tidal) correction. */ std::vector< std::function< void( Eigen::MatrixXd&, Eigen::MatrixXd& ) > > - correctionFunctions; + correctionFunctions; //! Calculates basic solid body gravity field corrections due to single body. /*! @@ -389,7 +323,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * (passed by reference; correction added to input value). */ virtual void addBasicSolidBodyTideCorrections( - Eigen::MatrixXd& cTermCorrections, Eigen::MatrixXd& sTermCorrections ); + Eigen::MatrixXd& cTermCorrections, Eigen::MatrixXd& sTermCorrections ) = 0; //! Sets current properties (mass state) of body causing tidal deformation. /*! @@ -399,7 +333,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * \param evaluationTime Time at which properties are to be evaluated. */ virtual void setBodyGeometryParameters( - const int bodyIndex, const double evaluationTime); + const int bodyIndex, const double evaluationTime); //! Calculate tidal amplitude and argument at current degree and order. /*! @@ -408,10 +342,10 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * \param order Order of tide. */ virtual void updateTidalAmplitudeAndArgument( - const int degree, const int order ) + const int degree, const int order ) { tideAmplitude = basic_mathematics::computeLegendrePolynomialExplicit( - degree, order, sineOfLatitude ); + degree, order, sineOfLatitude ); tideArgument = static_cast< double >( order ) * iLongitude; } @@ -433,7 +367,7 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations * List of state functions of body causing deformations. */ std::vector< std::function< Eigen::Vector6d( const double ) > > - deformingBodyStateFunctions_; + deformingBodyStateFunctions_; //! Reference radius (typically equatorial) of body being deformed's spherical harmonic //! gravity field. @@ -455,16 +389,6 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations */ std::vector< std::function< double( ) > > deformingBodyMasses_; - // LOVE NUMBER TODO: FIX DOCS - //! List of love numbers for each degree and order - /*! - * List of love numbers for each degree and order. First vector level denotes degree - * (index 0 = degree 2), second vector level must be of size (loveNumbers.size( ) + 2, i.e. - * maximum degree == maximum order. - */ - std::map< int, std::vector< std::complex< double > > > loveNumbers_; - - //! List of names of bodies causing deformation. /*! * List of names of bodies causing deformation. @@ -537,6 +461,222 @@ class BasicSolidBodyTideGravityFieldVariations: public GravityFieldVariations }; + +//! Class to calculate first-order solid body tide gravity field variations on a single body raised +//! by any number of bodies up to any degree and order. +class BasicSolidBodyTideGravityFieldVariations: public SolidBodyTideGravityFieldVariations +{ +public: + //! Constructor + /*! + * Contructor, sets Love numbers and properties of deformed and tide-raising bodies. + * \param deformedBodyStateFunction Function returning state of body being deformed. + * \param deformedBodyOrientationFunction Function providing rotation from inertial to body + * being deformed-fixed frame + * \param deformingBodyStateFunctions List of state functions of body causing deformations. + * \param deformedBodyReferenceRadius Reference radius (typically equatorial) of body being + * deformed's spherical harmonic gravity field. + * \param deformedBodyMass Function returning mass (or gravitational parameter) of body being + * deformed. + * \param deformingBodyMasses List of functions returning masses (or gravitational parameters) + * of bodies causing deformation. + * \param loveNumbers List of love numbers for each degree and order. First vector level + * denotes degree (index 0 = degree 2), second vector level denotes order and must be of maximum size + * (loveNumbers.size( ) + 2, i.e. maximum degree >= maximum order) + * \param deformingBodies List of names of bodies causing deformation + */ + BasicSolidBodyTideGravityFieldVariations( + const std::function< Eigen::Vector6d( const double ) > + deformedBodyStateFunction, + const std::function< Eigen::Quaterniond( const double ) > + deformedBodyOrientationFunction, + const std::vector< std::function< Eigen::Vector6d( const double ) > > + deformingBodyStateFunctions, + const double deformedBodyReferenceRadius, + const std::function< double( ) > deformedBodyMass, + const std::vector< std::function< double( ) > > deformingBodyMasses, + const std::map< int, std::vector< std::complex< double > > > loveNumbers, + const std::vector< std::string > deformingBodies ): + // LOVE NUMBERS TODO: FIX MIN/MAX STUFF + SolidBodyTideGravityFieldVariations( + deformedBodyStateFunction, deformedBodyOrientationFunction, deformingBodyStateFunctions, deformedBodyReferenceRadius, + deformedBodyMass, deformingBodyMasses, deformingBodies, loveNumbers.rbegin( )->first, loveNumbers.rbegin( )->first ), + loveNumbers_( loveNumbers ) + { + // Set basic deformation functon as function to be evaluated when requesting variations. + correctionFunctions.push_back( + std::bind( + &BasicSolidBodyTideGravityFieldVariations::addBasicSolidBodyTideCorrections, + this, std::placeholders::_1, std::placeholders::_2 )); + } + + //! Destructor + /*! + * Destructor + */ + virtual ~BasicSolidBodyTideGravityFieldVariations( ){ } + + + //! Function to retrieve the love numbers at given degree. + /*! + * Function to retrieve the love numbers at given degree. Returns a vector containing (complex) + * love numbers at all orders in current degree. + * \param degree Degree from which love numbers are to be retrieved. + * \return Vector of love numbers (i^{th} entry representing i^{th} order in requested degree) + * containing love numbers at current degree. + */ + std::vector< std::complex< double > > getLoveNumbersOfDegree( const int degree ) + { + return loveNumbers_[ degree ]; + } + + //! Function to return all love numbers. + /*! + * Function to return all love numbers, i.e. at all degrees and orders. + * \return Complete set of available love numbers. + */ + const std::map< int, std::vector< std::complex< double > > > getLoveNumbers( ) + { + return loveNumbers_; + } + + //! Function to reset the love numbers at given degree. + /*! + * Function to reset the love numbers at given degree. Input requires a vector containing + * (complex) love numbers at all orders in current degree. + * \param degree Degree from which love numbers are to be retrieved. + * \param loveNumbers Vector of love numbers (i^{th} entry representing i^{th} order in requested degree) + * containing new love numbers at current degree. + */ + void resetLoveNumbersOfDegree( const std::vector< std::complex< double > > loveNumbers, + const int degree ) + { + if( loveNumbers_.count( degree ) == 0 ) + { + std::string errorMessage = "Error, tried to set love numbers at degree " + + std::to_string( degree ) + + " in BasicSolidBodyTideGravityFieldVariations: not available"; + throw std::runtime_error( errorMessage ); + } + else + { + if( loveNumbers.size( ) <= static_cast< unsigned int >( degree + 1 ) ) + { + loveNumbers_[ degree ] = loveNumbers; + } + else + { + std::string errorMessage = "Error, tried to set love numbers at degree " + + std::to_string( degree ) + " in BasicSolidBodyTideGravityFieldVariations with" + + std::to_string( loveNumbers.size( ) ) + " orders"; + throw std::runtime_error( errorMessage ); + } + } + } + +protected: + + + //! Calculates basic solid body gravity field corrections due to single body. + /*! + * Calculates basic solid body gravity field corrections for all degrees and orders set. + * The arguments are modified as they are passed by reference, through which the corrections + * are returned. + * Class variables denoting properties of currently considered body must have been set before + * this function is called. + * \param cTermCorrections Corrections to cosine terms + * (passed by reference; correction added to input value). + * \param sTermCorrections Corrections to sine terms. + * + * (passed by reference; correction added to input value). + */ + virtual void addBasicSolidBodyTideCorrections( + Eigen::MatrixXd& cTermCorrections, Eigen::MatrixXd& sTermCorrections ); + + + // LOVE NUMBER TODO: FIX DOCS + //! List of love numbers for each degree and order + /*! + * List of love numbers for each degree and order. First vector level denotes degree + * (index 0 = degree 2), second vector level must be of size (loveNumbers.size( ) + 2, i.e. + * maximum degree == maximum order. + */ + std::map< int, std::vector< std::complex< double > > > loveNumbers_; + +}; + +int getModeCoupledMaximumResponseDegree( + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > >& loveNumbers ); + +int getModeCoupledMaximumResponseOrder( + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > >& loveNumbers ); + + +class ModeCoupledSolidBodyTideGravityFieldVariations: public SolidBodyTideGravityFieldVariations +{ +public: + + ModeCoupledSolidBodyTideGravityFieldVariations( + const std::function< Eigen::Vector6d( const double ) > deformedBodyStateFunction, + const std::function< Eigen::Quaterniond( const double ) > deformedBodyOrientationFunction, + const std::vector< std::function< Eigen::Vector6d( const double ) > > deformingBodyStateFunctions, + const double deformedBodyReferenceRadius, + const std::function< double( ) > deformedBodyMass, + const std::vector< std::function< double( ) > > deformingBodyMasses, + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers, + const std::vector< std::string > deformingBodies ):SolidBodyTideGravityFieldVariations( + deformedBodyStateFunction, deformedBodyOrientationFunction, deformingBodyStateFunctions, deformedBodyReferenceRadius, + deformedBodyMass, deformingBodyMasses, deformingBodies, + getModeCoupledMaximumResponseDegree( loveNumbers ), + getModeCoupledMaximumResponseOrder( loveNumbers ) ), loveNumbers_( loveNumbers ) + { + // Set basic deformation functon as function to be evaluated when requesting variations. + correctionFunctions.push_back( + std::bind( + &ModeCoupledSolidBodyTideGravityFieldVariations::addBasicSolidBodyTideCorrections, + this, std::placeholders::_1, std::placeholders::_2 ) ); + } + + //! Destructor + /*! + * Destructor + */ + virtual ~ModeCoupledSolidBodyTideGravityFieldVariations( ){ } + + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > getLoveNumbers( ) + { + return loveNumbers_; + } + + void resetLoveNumber( + const std::pair< int, int > forcingIndices, const std::pair< int, int > responseIndices, const double loveNumber ) + { + if( loveNumbers_.count( forcingIndices ) == 0 ) + { + throw std::runtime_error( "Error when resetting mode-coupled Love number, no number at forcing D/O " + + std::to_string( forcingIndices.first ) + "/" + + std::to_string( forcingIndices.second ) + " found " ); + } + else + { + if( loveNumbers_.at( forcingIndices ).count( responseIndices ) == 0 ) + { + throw std::runtime_error( "Error when resetting mode-coupled Love number, no number at forcing D/O " + + std::to_string( forcingIndices.first ) + "/" + std::to_string( forcingIndices.second ) + + " and response D/O " + std::to_string( responseIndices.first ) + "/" + std::to_string( responseIndices.second ) +" found "); + } + } + loveNumbers_[ forcingIndices ][ responseIndices ] = loveNumber; + } + +protected: + + virtual void addBasicSolidBodyTideCorrections( + Eigen::MatrixXd& cTermCorrections, Eigen::MatrixXd& sTermCorrections ); + + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers_; + +}; } // namespace gravitation } // namespace tudat diff --git a/include/tudat/astro/gravitation/gravityFieldVariations.h b/include/tudat/astro/gravitation/gravityFieldVariations.h index 29183b714d..4b9d46f8bb 100644 --- a/include/tudat/astro/gravitation/gravityFieldVariations.h +++ b/include/tudat/astro/gravitation/gravityFieldVariations.h @@ -26,6 +26,7 @@ namespace gravitation enum BodyDeformationTypes { basic_solid_body, + mode_coupled_solid_body, tabulated_variation, periodic_variation, polynomial_variation, @@ -428,7 +429,8 @@ class GravityFieldVariationsSet * \return The tidal gravity field variation with the specified bodies causing deformation */ std::shared_ptr< GravityFieldVariations > getDirectTidalGravityFieldVariation( - const std::vector< std::string >& deformingBodies ); + const std::vector< std::string >& deformingBodies, + const BodyDeformationTypes tideType = basic_solid_body ); //! Function to retrieve the tidal gravity field variations /*! diff --git a/include/tudat/astro/observation_models/observationBias.h b/include/tudat/astro/observation_models/observationBias.h index 28f5fd9c4a..3996ec511f 100644 --- a/include/tudat/astro/observation_models/observationBias.h +++ b/include/tudat/astro/observation_models/observationBias.h @@ -22,6 +22,8 @@ #include "tudat/astro/basic_astro/physicalConstants.h" #include "tudat/basics/basicTypedefs.h" +#include "tudat/astro/earth_orientation/terrestrialTimeScaleConverter.h" +#include "tudat/astro/ground_stations/groundStationState.h" #include "tudat/astro/observation_models/linkTypeDefs.h" #include "tudat/astro/observation_models/observableTypes.h" #include "tudat/math/interpolators/lookupScheme.h" @@ -44,7 +46,8 @@ enum ObservationBiasTypes constant_time_drift_bias, arc_wise_time_drift_bias, constant_time_bias, - arc_wise_time_bias + arc_wise_time_bias, + two_way_range_time_scale_bias }; //! Base class (non-functional) for describing observation biases @@ -1214,6 +1217,71 @@ class ArcWiseTimeBias: public ObservationBias< ObservationSize > }; +//! Class for a constant time observation bias of a given size +/*! + * Class for a constant time observation bias of a given size. For unbiases observation h and time bias c, the biased observation + * is computed as h(t+c) + */ +template< int ObservationSize = 1 > +class TwoWayTimeScaleRangeBias: public ObservationBias< ObservationSize > +{ +public: + + //! Constructor + /*! + * Constructor + * \param timeBias Constant (entry-wise) time bias. + * \param linkEndIndexForTime Link end index from which the 'current time' is determined + */ + TwoWayTimeScaleRangeBias( + const std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > timeScaleConverter, + const std::shared_ptr< ground_stations::GroundStationState > transmittingStationState, + const std::shared_ptr< ground_stations::GroundStationState > receivingStationState ): + ObservationBias< ObservationSize >( false ), timeScaleConverter_( timeScaleConverter ), + transmittingStationState_( transmittingStationState ),receivingStationState_( receivingStationState ){ } + + //! Destructor + ~TwoWayTimeScaleRangeBias( ){ } + + //! Function to retrieve the constant time drift bias. + /*! + * Function to retrieve the constant time drift bias. + * \param linkEndTimes List of times at each link end during observation (unused). + * \param linkEndStates List of states at each link end during observation (unused). + * \param currentObservableValue Unbiased value of the observable (unused and default NAN). + * \return Constant time drift bias. + */ + Eigen::Matrix< double, ObservationSize, 1 > getObservationBias( + const std::vector< double >& linkEndTimes, + const std::vector< Eigen::Matrix< double, 6, 1 > >& linkEndStates, + const Eigen::Matrix< double, ObservationSize, 1 >& currentObservableValue ) + { + + double receptionTimeDifference = timeScaleConverter_->getCurrentTimeDifference( + computedTimeScale_, observedTimeScale_, linkEndTimes.at( 3 ), receivingStationState_->getNominalCartesianPosition( ) ); + double transmissionTimeDifference = timeScaleConverter_->getCurrentTimeDifference( + computedTimeScale_, observedTimeScale_, linkEndTimes.at( 0 ), transmittingStationState_->getNominalCartesianPosition( ) ); + Eigen::Matrix< double, ObservationSize, 1 > biasValue = Eigen::Matrix< double, ObservationSize, 1 >::Zero( ); + biasValue( 0 ) = ( receptionTimeDifference - transmissionTimeDifference ) * physical_constants::SPEED_OF_LIGHT; + + return biasValue; + } + +private: + + std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > timeScaleConverter_; + + std::shared_ptr< ground_stations::GroundStationState > transmittingStationState_; + + std::shared_ptr< ground_stations::GroundStationState > receivingStationState_; + + basic_astrodynamics::TimeScales observedTimeScale_ = basic_astrodynamics::utc_scale; + + basic_astrodynamics::TimeScales computedTimeScale_ = basic_astrodynamics::tdb_scale; + +}; + + //! Function to retrieve the type of an observation bias /*! * Function to retrieve the type of an observation bias. diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.h b/include/tudat/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.h index 6404fb6ed2..02303f6856 100644 --- a/include/tudat/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.h @@ -416,6 +416,13 @@ class SphericalHarmonicsGravityPartial: public AccelerationPartial const int parameterSize, Eigen::MatrixXd& accelerationPartial ); + void wrtModeCoupledLoveNumbers( + const std::function< std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > >( ) > coefficientPartialFunctions, + const std::vector< int >& responseIndices, + const std::vector< std::pair< int, int > >& responseDegreeOrders, + const int parameterSize, + Eigen::MatrixXd& partialMatrix ); + //! Function to return the gravitational parameter used for calculating the acceleration. std::function< double( ) > gravitationalParameterFunction_; diff --git a/include/tudat/astro/orbit_determination/acceleration_partials/tidalLoveNumberPartialInterface.h b/include/tudat/astro/orbit_determination/acceleration_partials/tidalLoveNumberPartialInterface.h index c8a5c0b905..db7a22bc7e 100755 --- a/include/tudat/astro/orbit_determination/acceleration_partials/tidalLoveNumberPartialInterface.h +++ b/include/tudat/astro/orbit_determination/acceleration_partials/tidalLoveNumberPartialInterface.h @@ -53,7 +53,7 @@ class TidalLoveNumberPartialInterface * \param deformedBody Name of body being tidally deformed. */ TidalLoveNumberPartialInterface( - const std::shared_ptr< gravitation::BasicSolidBodyTideGravityFieldVariations > gravityFieldVariations, + const std::shared_ptr< gravitation::SolidBodyTideGravityFieldVariations > gravityFieldVariations, const std::function< Eigen::Vector3d( ) > deformedBodyPositionFunction, const std::vector< std::function< Eigen::Vector3d( ) > > deformingBodyStateFunctions, const std::function< Eigen::Quaterniond( ) > rotationToDeformedBodyFrameFrameFunction, @@ -163,6 +163,14 @@ class TidalLoveNumberPartialInterface const int maximumDegree, const int maximumOrder ); + std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > > + calculateSphericalHarmonicCoefficientsPartialWrtModeCoupledTidalLoveNumbers( + const std::vector< std::pair< int, int > > parameterDegreeAndOrderIndices, + const std::map< int, std::vector< int > >& ordersPerDegree, + const std::vector< int >& deformingBodyIndices, + const int maximumDegree, + const int maximumOrder ); + //! Function to calculate the partial of spherical harmonic acceleration w.r.t. real tidal love numbers. /*! * Function to calculate the partial of spherical harmonic acceleration w.r.t. real tidal love numbers at all orders of diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h index de3b047aa0..34f0f891a0 100644 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/estimatableParameter.h @@ -81,7 +81,8 @@ enum EstimatebleParametersEnum polynomial_gravity_field_variation_amplitudes, periodic_gravity_field_variation_amplitudes, source_direction_radiation_pressure_scaling_factor, - source_perpendicular_direction_radiation_pressure_scaling_factor + source_perpendicular_direction_radiation_pressure_scaling_factor, + mode_coupled_tidal_love_numbers }; std::string getParameterTypeString( const EstimatebleParametersEnum parameterType ); diff --git a/include/tudat/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.h b/include/tudat/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.h index 8a18c50d4d..358146dcc5 100755 --- a/include/tudat/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.h +++ b/include/tudat/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.h @@ -256,6 +256,124 @@ class FullDegreeTidalLoveNumber: public TidalLoveNumber< Eigen::VectorXd > }; + +//! Class for estimating the tidal Love number k_{n} at a single degree that is constant for all orders +class ModeCoupledTidalLoveNumber: public EstimatableParameter< Eigen::VectorXd > +{ +public: + + //! Constructor + /*! + * Constructor + * \param gravityFieldVariationModel Tidal gravity field variation object of which estimated paraemeter is a property + * \param associatedBody Deformed body + * \param degree Degree of Love number that is to be estimateds + * \param useComplexComponents True if the complex Love number is estimated, false if only the real part is considered + */ + ModeCoupledTidalLoveNumber( + const std::shared_ptr< gravitation::ModeCoupledSolidBodyTideGravityFieldVariations > gravityFieldVariationModel, + const std::string& associatedBody, + const std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices, + const bool useComplexComponents = 0 ); + + Eigen::VectorXd getParameterValue( ) + { + Eigen::VectorXd currentParameters = Eigen::VectorXd::Zero( parameterSize_ ); + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers = gravityFieldVariationModel_->getLoveNumbers( ); + + int counter = 0; + for( auto it: loveNumberIndices_ ) + { + for( unsigned int i = 0; i < it.second.size( ); i++ ) + { + currentParameters( counter ) = loveNumbers.at( it.first ).at( it.second.at( i ) ); + counter++; + } + } + return currentParameters; + } + + + void setParameterValue( Eigen::VectorXd parameterValue ) + { + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers = gravityFieldVariationModel_->getLoveNumbers( ); + + int counter = 0; + for( auto it: loveNumberIndices_ ) + { + for( unsigned int i = 0; i < it.second.size( ); i++ ) + { + gravityFieldVariationModel_->resetLoveNumber( it.first, it.second.at( i ), parameterValue( counter ) ); + counter++; + } + } + } + + int getParameterSize( ) + { + return parameterSize_; + } + + int getMaximumForcingDegree( ) + { + return maximumForcingDegree_; + } + + std::vector< std::string > getDeformingBodies( ) + { + return gravityFieldVariationModel_->getDeformingBodies( ); + } + + + std::vector< std::pair< int, int > > getParameterForcingDegreeAndOrderIndices( ) + { + return parameterForcingDegreeAndOrderIndices_; + } + + std::map< int, std::vector< int > > getForcingOrdersPerDegree( ) + { + return forcingOrdersPerDegree_; + } + + std::vector< std::pair< int, int > > getResponseDegreeOrders( ) + { + return responseDegreeOrders_; + } + + std::vector< int > getResponseIndices( ) + { + return responseIndices_; + } + +private: + std::shared_ptr< gravitation::ModeCoupledSolidBodyTideGravityFieldVariations > gravityFieldVariationModel_; + + // Map with forcing degree/orders (key) and list of associated response degree orders (values) + std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices_; + + // NUmber of separate Love numbers + int parameterSize_; + + // Maximum degree of forcing + int maximumForcingDegree_; + + // The list of forcing orders per forcing degree + std::map< int, std::vector< int > > forcingOrdersPerDegree_; + + // For each independent Love number, the forcing degree, and the index in the associated vector of orders in forcingOrdersPerDegree_ + std::vector< std::pair< int, int > > parameterForcingDegreeAndOrderIndices_; + + // List of all response degree/orders + std::vector< std::pair< int, int > > responseDegreeOrders_; + + // For each independent Love number, the response degree/order (as an index in responseDegreeOrders_); + std::vector< int > responseIndices_; + + + +}; + + //! Class for estimating the tidal Love numbers k_{n,m} at a single degree that may vary for different orders class SingleDegreeVariableTidalLoveNumber: public TidalLoveNumber< Eigen::VectorXd > { @@ -338,8 +456,6 @@ class SingleDegreeVariableTidalLoveNumber: public TidalLoveNumber< Eigen::Vector } return parameterDescription; } - - }; } diff --git a/include/tudat/astro/propagators/propagateCovariance.h b/include/tudat/astro/propagators/propagateCovariance.h index f28d9f83ec..83f653b682 100644 --- a/include/tudat/astro/propagators/propagateCovariance.h +++ b/include/tudat/astro/propagators/propagateCovariance.h @@ -103,22 +103,23 @@ void propagateCovariance( const double initialTime, const double finalTime ); -//Eigen::MatrixXd convertCovarianceToFrame( -// const Eigen::MatrixXd inputCovariance, -// const Eigen::VectorXd inertialCartesianRelativeState, -// const reference_frames::SatelliteReferenceFrames inputFrame, -// const reference_frames::SatelliteReferenceFrames outputFrame ); - -//std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame( -// const std::map< double, Eigen::MatrixXd > inputCovariances, -// const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates, -// const reference_frames::SatelliteReferenceFrames inputFrame, -// const reference_frames::SatelliteReferenceFrames outputFrame ); +Eigen::MatrixXd convertCovarianceToFrame( + const Eigen::MatrixXd inputCovariance, + const Eigen::VectorXd inertialCartesianRelativeState, + const reference_frames::SatelliteReferenceFrames inputFrame, + const reference_frames::SatelliteReferenceFrames outputFrame ); + +std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame( + const std::map< double, Eigen::MatrixXd > inputCovariances, + const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates, + const reference_frames::SatelliteReferenceFrames inputFrame, + const reference_frames::SatelliteReferenceFrames outputFrame ); void convertCovarianceHistoryToFormalErrorHistory( std::map< double, Eigen::VectorXd >& propagatedFormalErrors, std::map< double, Eigen::MatrixXd >& propagatedCovariance ); + //! Function to propagate full covariance at the initial time to state formal errors at later times /*! * Function to propagate full covariance at the initial time to state formal errors at later times diff --git a/include/tudat/astro/reference_frames/referenceFrameTransformations.h b/include/tudat/astro/reference_frames/referenceFrameTransformations.h index 0ec0102510..f723366a48 100644 --- a/include/tudat/astro/reference_frames/referenceFrameTransformations.h +++ b/include/tudat/astro/reference_frames/referenceFrameTransformations.h @@ -47,6 +47,16 @@ enum AerodynamicsReferenceFrames body_frame = 4 }; +// Enum defining identifiers of frames in which a user-specified thrust is defined. +//! @get_docstring(ThrustFrames.__docstring__) +enum SatelliteReferenceFrames +{ + unspecified_reference_frame = -1, + global_reference_frame = 0, + tnw_reference_frame = 1, + rsw_reference_frame = 2 +}; + //! Function to get a string representing a 'named identification' of a reference frame. /*! * Function to get a string representing a 'named identification' of a reference frame. @@ -678,6 +688,11 @@ Eigen::Vector3d getBodyFixedSphericalPosition( const std::function< Eigen::Vector3d( ) > positionFunctionOfRelativeBody, const std::function< Eigen::Quaterniond( ) > orientationFunctionOfCentralBody ); +Eigen::Matrix3d getRotationBetweenSatelliteFrames( + const Eigen::Vector6d relativeInertialCartesianState, + const SatelliteReferenceFrames originalFrame, + const SatelliteReferenceFrames targetFrame ); + /*! Compute the rotation matrix between ITRF2014 and another ITRF frame. * * Compute the rotation matrix between ITRF2014 and another ITRF frame. According to the IERS convetions of 2010, Eq. @@ -758,6 +773,7 @@ Eigen::Vector6d convertGroundStationStateBetweenItrfFrames( const std::string& baseFrame, const std::string& targetFrame ); + } // namespace reference_frames } // namespace tudat diff --git a/include/tudat/astro/system_models/vehicleSystems.h b/include/tudat/astro/system_models/vehicleSystems.h index 1fc68c5474..fca0b952d4 100644 --- a/include/tudat/astro/system_models/vehicleSystems.h +++ b/include/tudat/astro/system_models/vehicleSystems.h @@ -245,6 +245,12 @@ class VehicleSystems transponderTurnaroundRatio_ = transponderRatioFunction; } + + void setDefaultTransponderTurnaroundRatio( ) + { + setTransponderTurnaroundRatio( ); + } + void setTransponderTurnaroundRatio( std::map< std::pair< observation_models::FrequencyBands, observation_models::FrequencyBands >, double >& transponderRatioPerUplinkAndDownlinkFrequencyBand ) diff --git a/include/tudat/interface/json/propagation/thrust.h b/include/tudat/interface/json/propagation/thrust.h index e9a51490ee..aeb84ee3e3 100644 --- a/include/tudat/interface/json/propagation/thrust.h +++ b/include/tudat/interface/json/propagation/thrust.h @@ -102,34 +102,6 @@ void to_json( nlohmann::json& jsonObject, const std::shared_ptr< ThrustMagnitude void from_json( const nlohmann::json& jsonObject, std::shared_ptr< ThrustMagnitudeSettings >& magnitudeSettings ); -//// ThrustFrames - -////! Map of `ThrustFrames` string representations. -//static std::map< ThrustFrames, std::string > thrustFrameTypes = -//{ -// { unspecified_thrust_frame, "unspecified" }, -// { inertial_thurst_frame, "intertial" }, -// { tnw_thrust_frame, "tnw" } -//}; - -////! `ThrustFrames` not supported by `json_interface`. -//static std::vector< ThrustFrames > unsupportedThrustFrameTypes = -//{ -// unspecified_thrust_frame -//}; - -////! Convert `ThrustFrames` to `json`. -//inline void to_json( nlohmann::json& jsonObject, const ThrustFrames& thrustFrameType ) -//{ -// jsonObject = json_interface::stringFromEnum( thrustFrameType, thrustFrameTypes ); -//} - -////! Convert `json` to `ThrustFrames`. -//inline void from_json( const nlohmann::json& jsonObject, ThrustFrames& thrustFrameType ) -//{ -// thrustFrameType = json_interface::enumFromString( jsonObject, thrustFrameTypes ); -//} - // Thrust diff --git a/include/tudat/io/solarActivityData.h b/include/tudat/io/solarActivityData.h index 16b1750a4b..c70ae1326c 100644 --- a/include/tudat/io/solarActivityData.h +++ b/include/tudat/io/solarActivityData.h @@ -52,7 +52,7 @@ struct SolarActivityData /*! * Default constructor. */ - SolarActivityData( ); + SolarActivityData( const int yearInput = 0, const int monthInput = 0, const int dayInput = 0 ); //! Year. unsigned int year; @@ -194,6 +194,11 @@ struct SolarActivityContainer return solarActivityDataMap_.at( nearestJulianDay ); } + std::map< double, SolarActivityDataPtr > getSolarActivityDataMap( ) + { + return solarActivityDataMap_; + } + private: diff --git a/include/tudat/math/basic/nearestNeighbourSearch.h b/include/tudat/math/basic/nearestNeighbourSearch.h index 360dd16a62..e266724398 100644 --- a/include/tudat/math/basic/nearestNeighbourSearch.h +++ b/include/tudat/math/basic/nearestNeighbourSearch.h @@ -165,8 +165,15 @@ bool isIndependentVariableInInterval( const int lowerIndex, bool isInInterval = false; //Check if value is in interval. - if ( independentVariableValue >= independentValues[ lowerIndex ] && - independentVariableValue < independentValues[ lowerIndex + 1 ] ) + if( lowerIndex == static_cast< int >( independentValues.size( ) - 1 ) ) + { + if ( independentVariableValue > independentValues.at( lowerIndex )) + { + isInInterval = true; + } + } + else if ( independentVariableValue >= independentValues.at( lowerIndex ) && + independentVariableValue < independentValues.at( lowerIndex + 1 ) ) { isInInterval = true; } diff --git a/include/tudat/simulation/environment_setup/body.h b/include/tudat/simulation/environment_setup/body.h index 2911b0f00f..633d290305 100644 --- a/include/tudat/simulation/environment_setup/body.h +++ b/include/tudat/simulation/environment_setup/body.h @@ -585,8 +585,6 @@ class Body { { if( !isStateSet_ ) { - std::vector< double > test; - test.at( 0 ); throw std::runtime_error( "Error when retrieving state from body " + bodyName_ + ", state of body is not yet defined" ); } else @@ -1689,8 +1687,6 @@ class Body { std::shared_ptr getGroundStation(const std::string &stationName) const { if (groundStationMap.count(stationName) == 0) { - std::vector< double > a; - a.at( 0 ); throw std::runtime_error("Error, station " + stationName + " does not exist"); } diff --git a/include/tudat/simulation/environment_setup/createAerodynamicCoefficientInterface.h b/include/tudat/simulation/environment_setup/createAerodynamicCoefficientInterface.h index 21e5fe3e55..e136636c2e 100644 --- a/include/tudat/simulation/environment_setup/createAerodynamicCoefficientInterface.h +++ b/include/tudat/simulation/environment_setup/createAerodynamicCoefficientInterface.h @@ -787,6 +787,7 @@ class TabulatedAerodynamicCoefficientSettings: public TabulatedAerodynamicCoeffi boost::multi_array< Eigen::Vector3d, NumberOfDimensions > momentCoefficients_; }; + // Object for setting aerodynamic coefficients from a user-defined 1-dimensional table. /* * Object for setting aerodynamic coefficients from a user-defined 1-dimensional table. @@ -826,7 +827,7 @@ class TabulatedAerodynamicCoefficientSettings< 1 >: public TabulatedAerodynamicC * \param interpolatorSettings Pointer to an interpolator settings object, where the * conditions for interpolation are saved. */ - TabulatedAerodynamicCoefficientSettings< 1 >( + TabulatedAerodynamicCoefficientSettings( const std::vector< double > independentVariables, const std::vector< Eigen::Vector3d > forceCoefficients, const std::vector< Eigen::Vector3d > momentCoefficients, @@ -888,7 +889,7 @@ class TabulatedAerodynamicCoefficientSettings< 1 >: public TabulatedAerodynamicC * \param interpolatorSettings Pointer to an interpolator settings object, where the * conditions for interpolation are saved. */ - TabulatedAerodynamicCoefficientSettings< 1 >( + TabulatedAerodynamicCoefficientSettings( const std::vector< std::vector< double > > independentVariables, const boost::multi_array< Eigen::Vector3d, 1 > forceCoefficients, const boost::multi_array< Eigen::Vector3d, 1 > momentCoefficients, @@ -944,7 +945,7 @@ class TabulatedAerodynamicCoefficientSettings< 1 >: public TabulatedAerodynamicC * \param interpolatorSettings Pointer to an interpolator settings object, where the * conditions for interpolation are saved. */ - TabulatedAerodynamicCoefficientSettings< 1 >( + TabulatedAerodynamicCoefficientSettings( const std::vector< double > independentVariables, const std::vector< Eigen::Vector3d > forceCoefficients, const double referenceArea, @@ -990,7 +991,7 @@ class TabulatedAerodynamicCoefficientSettings< 1 >: public TabulatedAerodynamicC * \param interpolatorSettings Pointer to an interpolator settings object, where the * conditions for interpolation are saved. */ - TabulatedAerodynamicCoefficientSettings< 1 >( + TabulatedAerodynamicCoefficientSettings( const std::vector< std::vector< double > > independentVariables, const boost::multi_array< Eigen::Vector3d, 1 > forceCoefficients, const double referenceArea, @@ -1017,7 +1018,7 @@ class TabulatedAerodynamicCoefficientSettings< 1 >: public TabulatedAerodynamicC } // Destructor - ~TabulatedAerodynamicCoefficientSettings< 1 >( ){ } + ~TabulatedAerodynamicCoefficientSettings( ){ } // Function to return values of force coefficients in table. /* diff --git a/include/tudat/simulation/environment_setup/createGravityFieldVariations.h b/include/tudat/simulation/environment_setup/createGravityFieldVariations.h index be70f42d94..dfb8cba864 100644 --- a/include/tudat/simulation/environment_setup/createGravityFieldVariations.h +++ b/include/tudat/simulation/environment_setup/createGravityFieldVariations.h @@ -182,6 +182,53 @@ class BasicSolidBodyGravityFieldVariationSettings: public GravityFieldVariationS }; + +class ModeCoupledSolidBodyGravityFieldVariationSettings: public GravityFieldVariationSettings +{ +public: + + + ModeCoupledSolidBodyGravityFieldVariationSettings( + const std::vector< std::string > deformingBodies, + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers ): + GravityFieldVariationSettings( gravitation::mode_coupled_solid_body ), + deformingBodies_( deformingBodies ), loveNumbers_( loveNumbers ){ } + + virtual ~ModeCoupledSolidBodyGravityFieldVariationSettings( ){ } + + //! Function to retrieve list of bodies causing tidal deformation + /*! + * \brief Function to retrieve list of bodies causing tidal deformation + * \return List of bodies causing tidal deformation + */ + std::vector< std::string > getDeformingBodies( ){ return deformingBodies_;} + + //! Function to retrieve list of Love number for the deformed body. + /*! + * \brief Function to retrieve list of Love number for the deformed body. + * \return List of Love number for the deformed body. + */ + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > getLoveNumbers( ){ return loveNumbers_; } + + + //! Function to reset list of bodies causing tidal deformation + /*! + * \brief Function to reset list of bodies causing tidal deformation + * \param deformingBodies New list of bodies causing tidal deformation + */ + void resetDeformingBodies( const std::vector< std::string >& deformingBodies ){ + deformingBodies_ = deformingBodies; } + +protected: + + //! List of bodies causing tidal deformation + std::vector< std::string > deformingBodies_; + + //! List of Love number for the deformed body. + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers_; + +}; + //! Class to define settings for tabulated gravity field variations. class TabulatedGravityFieldVariationSettings: public GravityFieldVariationSettings { @@ -614,6 +661,14 @@ inline std::shared_ptr< GravityFieldVariationSettings > polynomialGravityFieldVa cosineAmplitudes, sineAmplitudes, referenceEpoch, minimumDegree, minimumOrder ); } +inline std::shared_ptr< GravityFieldVariationSettings > modeCoupledSolidBodyGravityFieldVariationSettings( + const std::vector< std::string > deformingBodies, + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers ) +{ + return std::make_shared< ModeCoupledSolidBodyGravityFieldVariationSettings >( deformingBodies, loveNumbers ); +} + + //! Function to create a set of gravity field variations, stored in the associated interface class /*! * Function to create a set of gravity field variations, stored in the associated interface class of diff --git a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h index 4259d3686d..f9851bc82b 100644 --- a/include/tudat/simulation/estimation_setup/createEstimatableParameters.h +++ b/include/tudat/simulation/estimation_setup/createEstimatableParameters.h @@ -1982,6 +1982,58 @@ std::shared_ptr< estimatable_parameters::EstimatableParameter< Eigen::VectorXd > } break; } + case mode_coupled_tidal_love_numbers: + { + // Check input consistency + std::shared_ptr< ModeCoupledTidalLoveNumberEstimatableParameterSettings > tidalLoveNumberSettings = + std::dynamic_pointer_cast< ModeCoupledTidalLoveNumberEstimatableParameterSettings >( vectorParameterName ); + if( tidalLoveNumberSettings == nullptr ) + { + throw std::runtime_error( "Error, expected mode-coupled tidal love number parameter settings " ); + } + else + { + // Check consistency of body gravity field + std::shared_ptr< TimeDependentSphericalHarmonicsGravityField > timeDepGravityField = + std::dynamic_pointer_cast< TimeDependentSphericalHarmonicsGravityField >( + currentBody->getGravityFieldModel( ) ); + if( timeDepGravityField == nullptr ) + { + throw std::runtime_error( + "Error, requested mode-coupled tidal love number parameter of " + + vectorParameterName->parameterType_.second.first + + ", but body does not have a time dependent spherical harmonic gravity field." ); + } + else if( currentBody->getGravityFieldVariationSet( ) == nullptr ) + { + throw std::runtime_error( "Error, requested mode-coupled tidal love number parameter of " + + vectorParameterName->parameterType_.second.first + + ", but body does not have gravity field variations" ); + } + else + { + // Get associated gravity field variation + std::shared_ptr< gravitation::ModeCoupledSolidBodyTideGravityFieldVariations > gravityFieldVariation = + std::dynamic_pointer_cast< gravitation::ModeCoupledSolidBodyTideGravityFieldVariations >( + currentBody->getGravityFieldVariationSet( )->getDirectTidalGravityFieldVariation( + tidalLoveNumberSettings->deformingBodies_, mode_coupled_solid_body ) ); + + // Create parameter object + if( gravityFieldVariation != nullptr ) + { + vectorParameterToEstimate = std::make_shared< ModeCoupledTidalLoveNumber >( + gravityFieldVariation, currentBodyName, tidalLoveNumberSettings->loveNumberIndices_, + tidalLoveNumberSettings->useComplexValue_ ); + } + else + { + throw std::runtime_error( + "Error, expected ModeCoupledSolidBodyTideGravityFieldVariations for variable tidal love number" ); + } + } + } + break; + } case desaturation_delta_v_values: { if( propagatorSettings == nullptr ) diff --git a/include/tudat/simulation/estimation_setup/createObservationManager.h b/include/tudat/simulation/estimation_setup/createObservationManager.h index 3f24805f54..51ef5a3440 100644 --- a/include/tudat/simulation/estimation_setup/createObservationManager.h +++ b/include/tudat/simulation/estimation_setup/createObservationManager.h @@ -43,7 +43,6 @@ void performObservationParameterEstimationClosureForSingleModelSet( const LinkEnds linkEnds, const ObservableType observableType ) { - std::cout<<"Performing closure "< timeBiasParameter = std::dynamic_pointer_cast< estimatable_parameters::ArcWiseTimeBiasParameter >( parameter ); @@ -338,13 +336,8 @@ void performObservationParameterEstimationClosureForSingleModelSet( std::shared_ptr< ArcWiseTimeBias< ObservationSize > > timeBiasObject = std::dynamic_pointer_cast< ArcWiseTimeBias< ObservationSize > >( observationBias ); - std::cout<<"Is arc-wise time bias: "<getLinkEnds( ) ) <<" "<< ( observableType == timeBiasParameter->getObservableType( ) ) <<" "<< - ( timeBiasObject->getLinkEndIndexForTime( ) == timeBiasParameter->getLinkEndIndex( ) ) <<" "<< - ( timeBiasObject->getArcStartTimes( ).size( ) == timeBiasParameter->getArcStartTimes( ).size( ) ) <getLinkEnds( ) ) && ( observableType == timeBiasParameter->getObservableType( ) ) && ( timeBiasObject->getLinkEndIndexForTime( ) == timeBiasParameter->getLinkEndIndex( ) ) && @@ -360,7 +353,6 @@ void performObservationParameterEstimationClosureForSingleModelSet( doTimesMatch = false; } } - std::cout<<"Times match "< > > observationBiases = extractObservationBiasList( observationModels ); - std::cout<<"Number of extracted biases "< arcWiseTimeBias( return std::make_shared< ArcWiseTimeBiasSettings >( timeBiases, linkEndForTime ); } +inline std::shared_ptr< ObservationBiasSettings > twoWayTimeScaleRangeBias( ) +{ + return std::make_shared< ObservationBiasSettings >( two_way_range_time_scale_bias ); +} + + //! Class used for defining the settings for an observation model that is to be created. /*! * Class used for defining the settings for an observation model that is to be created. This class allows the type, light-time @@ -1489,6 +1495,38 @@ std::shared_ptr< ObservationBias< ObservationSize > > createObservationBiasCalcu observableType, arcwiseBiasSettings->linkEndForTime_, linkEnds.size( ) ).at( 0 ) ); break; } + case two_way_range_time_scale_bias: + { + if( observableType != n_way_range ) + { + throw std::runtime_error( "Error when making two-way range time scale bias, bias is to be applied to wrong observable: " + + getObservableName( observableType, linkEnds.size( ) ) ); + } + else if( linkEnds.size( ) != 3 ) + { + throw std::runtime_error( "Error when making two-way range time scale bias, bias is to be applied to observable with wrong number of link ends: " + + std::to_string( linkEnds.size( ) ) ); + } + else + { + if( linkEnds.at( transmitter ).bodyName_ != "Earth" ) + { + std::cerr<<" Warning when making two-way range time scale bias, bias is to be applied observable with transmitter not on Earth: "< >( + earth_orientation::createDefaultTimeConverter( ), + bodies.at( linkEnds.at( transmitter ).bodyName_ )->getGroundStation( linkEnds.at( transmitter ).stationName_ )->getNominalStationState( ), + bodies.at( linkEnds.at( transmitter ).bodyName_ )->getGroundStation( linkEnds.at( receiver ).stationName_ )->getNominalStationState( ) ); + + break; + } case multiple_observation_biases: { // Check input consistency diff --git a/include/tudat/simulation/estimation_setup/createObservationPartials.h b/include/tudat/simulation/estimation_setup/createObservationPartials.h index fe28e9c6de..7cc53e7244 100644 --- a/include/tudat/simulation/estimation_setup/createObservationPartials.h +++ b/include/tudat/simulation/estimation_setup/createObservationPartials.h @@ -644,7 +644,7 @@ std::shared_ptr< PositionPartialScaling > > createDifferencedObservablePartials( isParameterObservationLinkProperty( parameterIterator->second->getParameterName( ).first ) ) { currentDifferencedObservationPartial = createObservationPartialWrtLinkProperty< ObservationSize >( - linkEnds, undifferencedObservableType, parameterIterator->second, bodies, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); + linkEnds, differencedObservableType, parameterIterator->second, bodies, isPartialForDifferencedObservable, isPartialForConcatenatedObservable ); } // Check if partial is non-nullptr (i.e. whether dependency exists between current doppler and current parameter) diff --git a/include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h b/include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h index ef547ba97a..d36e669d3d 100644 --- a/include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h +++ b/include/tudat/simulation/estimation_setup/createObsevationBiasPartial.h @@ -434,7 +434,6 @@ std::shared_ptr< ObservationPartial< ObservationSize > > createObservationPartia std::bind( &ephemerides::Ephemeris::getCartesianAcceleration, transmitterInertialEphemeris, std::placeholders::_1, 30.0 ), std::bind( &ephemerides::Ephemeris::getCartesianAcceleration, receiverInertialEphemeris, std::placeholders::_1, 30.0 ) ); - std::cout<<"CREATING PARTIAL "<getLookupScheme( )< >( timeBiasPartial, arcwiseTimeBias->getLookupScheme( ), arcwiseTimeBias->getLinkEndIndex( ), diff --git a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h index fd85944f6c..ef4e0e1224 100644 --- a/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h +++ b/include/tudat/simulation/estimation_setup/estimatableParameterSettings.h @@ -507,7 +507,7 @@ class ArcWiseInitialTranslationalStateEstimatableParameterSettings: public Estim //! Constructor, sets initial value of translational state and a single central body. /*! * Constructor, sets initial value of translational state and a single central body - * \param associatedBody Body for which initial state is to be estimated. + * \param associatedBody Bo9dy for which initial state is to be estimated. * \param initialStateValue Current value of initial arc states (concatenated in same order as arcs) * \param arcStartTimes Start times for separate arcs * \param centralBody Body w.r.t. which the initial state is to be estimated. @@ -899,6 +899,57 @@ class SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings: public Es }; +inline std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > getFullModeCoupledLoveNumbers( + const unsigned int maximumForcingDegree, const unsigned int maximumResponseDegree ) +{ + std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices; + + for( unsigned int i = 2; i < maximumForcingDegree; i++ ) + { + for( unsigned int j = 0; j <= i; j++ ) + { + for( unsigned int k = 2; k < maximumResponseDegree; k++ ) + { + for( unsigned int l = 0; l <= k; k++ ) + { + loveNumberIndices[{i,j}].push_back({k,l}); + } + } + } + } + return loveNumberIndices; +} + +class ModeCoupledTidalLoveNumberEstimatableParameterSettings: public EstimatableParameterSettings +{ +public: + + //! Constructor for a single deforming body + /*! + * Constructor for a single deforming body + * \param associatedBody Deformed body + * \param degree Degree of Love number that is to be estimated + * \param deformingBody Name of body causing tidal deformation + * \param useComplexValue True if the complex Love number is estimated, false if only the real part is considered + */ + ModeCoupledTidalLoveNumberEstimatableParameterSettings( const std::string& associatedBody, + const std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices, + const std::vector< std::string >& deformingBodies, + const bool useComplexValue = 0 ): + EstimatableParameterSettings( associatedBody, mode_coupled_tidal_love_numbers ), + loveNumberIndices_( loveNumberIndices ), + deformingBodies_( deformingBodies ), useComplexValue_( useComplexValue ){ } + + std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices_; + + //! Names of bodies causing tidal deformation + std::vector< std::string > deformingBodies_; + + bool useComplexValue_; + +}; + + //! Class to define settings for estimating the tidal time lag of a direct tidal acceleration model /*! * Class to define settings for estimating the tidal time lag of a direct tidal acceleration model, it links to one or more @@ -1439,6 +1490,14 @@ inline std::shared_ptr< EstimatableParameterSettings > orderVaryingKLoveNumber( associatedBody, degree, orders, std::vector< std::string >( ), useComplexValue ); } +inline std::shared_ptr< EstimatableParameterSettings > modeCoupledTidalLoveNumberEstimatableParameterSettings( + const std::string& associatedBody, + const std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices, + const std::vector< std::string >& deformingBodies ) +{ + return std::make_shared< ModeCoupledTidalLoveNumberEstimatableParameterSettings >( + associatedBody, loveNumberIndices, deformingBodies, 0 ); +} inline std::shared_ptr< EstimatableParameterSettings > coreFactor( const std::string& associatedBody ) diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h b/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h index 87e2fde822..664895c4a4 100644 --- a/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h +++ b/include/tudat/simulation/estimation_setup/orbitDeterminationTestCases.h @@ -306,7 +306,7 @@ std::pair< std::shared_ptr< EstimationOutput< StateScalarType, TimeType > >, Eig { parameterPerturbation = Eigen::VectorXd::Zero( 7 ); } - for( unsigned int i = 0; i < 7; i++ ) + for( unsigned int i = 0; i < initialParameterEstimate.rows( ); i++ ) { initialParameterEstimate( i ) += parameterPerturbation( i ); } diff --git a/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h b/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h index b7a0fac794..2e42b5271a 100644 --- a/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h +++ b/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h @@ -599,7 +599,7 @@ bool checkMultiArcPropagatorSettingsAndParameterEstimationConsistency( { auto currentListToTest = propagatedBodiesPerArc.at( i ); auto itr = std::find( currentListToTest.begin( ), currentListToTest.end( ), estimatedBodiesPerArc.at( i ).at( j ) ); - if ( itr == estimatedBodiesPerArc.at( i ).end( ) ) + if ( itr == currentListToTest.end( ) ) { isInputConsistent = false; std::string currentPropagatedBodies = ""; diff --git a/include/tudat/simulation/propagation_setup/accelerationSettings.h b/include/tudat/simulation/propagation_setup/accelerationSettings.h index fdac20bcb4..f0b3dd9a11 100644 --- a/include/tudat/simulation/propagation_setup/accelerationSettings.h +++ b/include/tudat/simulation/propagation_setup/accelerationSettings.h @@ -18,8 +18,10 @@ #include "tudat/astro/gravitation/thirdBodyPerturbation.h" #include "tudat/astro/aerodynamics/aerodynamicAcceleration.h" #include "tudat/astro/basic_astro/accelerationModelTypes.h" +#include "tudat/astro/reference_frames/referenceFrameTransformations.h" #include "tudat/basics/deprecationWarnings.h" #include "tudat/simulation/environment_setup/createRadiationPressureTargetModel.h" + // #include "tudat/math/interpolators/createInterpolator.h" namespace tudat @@ -490,6 +492,7 @@ class FullThrustInterpolationInterface }; + // Class for providing acceleration settings for a thrust acceleration model /* * Class for providing acceleration settings for a thrust acceleration model. Settings for the direction and magnitude @@ -500,6 +503,7 @@ class ThrustAccelerationSettings: public AccelerationSettings { public: + ThrustAccelerationSettings( const std::string& engineId ): AccelerationSettings( basic_astrodynamics::thrust_acceleration ) { @@ -528,6 +532,7 @@ class ThrustAccelerationSettings: public AccelerationSettings bool useAllEngines_; + template< typename ReturnType > ReturnType printDeprecationError( ) { diff --git a/include/tudat/simulation/propagation_setup/dynamicsSimulator.h b/include/tudat/simulation/propagation_setup/dynamicsSimulator.h index c122b9a17a..6c3fd4b52a 100644 --- a/include/tudat/simulation/propagation_setup/dynamicsSimulator.h +++ b/include/tudat/simulation/propagation_setup/dynamicsSimulator.h @@ -675,7 +675,8 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim const std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > propagatorSettings, const bool areEquationsOfMotionToBeIntegrated = true, const PredefinedSingleArcStateDerivativeModels< StateScalarType, TimeType >& predefinedStateDerivativeModels = - PredefinedSingleArcStateDerivativeModels< StateScalarType, TimeType >( ) ): + PredefinedSingleArcStateDerivativeModels< StateScalarType, TimeType >( ), + const bool isPartOfMultiArc = false ): DynamicsSimulator< StateScalarType, TimeType >( bodies, propagatorSettings ), propagatorSettings_( propagatorSettings ) @@ -699,7 +700,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim { throw std::runtime_error( "Error in dynamics simulator, integrator settings not defined." ); } - checkPropagatedStatesFeasibility( propagatorSettings_, bodies_ ); + checkPropagatedStatesFeasibility( propagatorSettings_, bodies_, isPartOfMultiArc ); // Create objects that reset the environment (e.g. ephemerides) after propagation is required if( propagatorSettings_->getOutputSettings( )->getSetIntegratedResult( ) ) @@ -1719,7 +1720,7 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time for ( unsigned int i = 0; i < singleArcSettings.size( ); i++ ) { singleArcDynamicsSimulators_.push_back( std::make_shared >( - bodies, singleArcSettings.at( i ), false )); + bodies, singleArcSettings.at( i ), false, PredefinedSingleArcStateDerivativeModels< StateScalarType, TimeType >( ), true ) ); singleArcResults.push_back( singleArcDynamicsSimulators_.at( i )->getSingleArcPropagationResults( )); singleArcDynamicsSimulators_.at( i )->createAndSetIntegratedStateProcessors( ); } diff --git a/include/tudat/simulation/propagation_setup/propagationOutput.h b/include/tudat/simulation/propagation_setup/propagationOutput.h index fc50c0dd09..ae03b29442 100644 --- a/include/tudat/simulation/propagation_setup/propagationOutput.h +++ b/include/tudat/simulation/propagation_setup/propagationOutput.h @@ -25,6 +25,7 @@ #include "tudat/simulation/propagation_setup/propagationSettings.h" #include "tudat/simulation/environment_setup/createFlightConditions.h" #include "tudat/math/basic/rotationRepresentations.h" +#include "tudat/astro/aerodynamics/nrlmsise00Atmosphere.h" namespace tudat { @@ -1761,6 +1762,44 @@ std::pair< std::function< Eigen::VectorXd( ) >, int > getVectorDependentVariable } break; } + case nrlmsise_input_data: + { + std::string acceleratedBody = dependentVariableSettings->associatedBody_; + std::string centralBody = dependentVariableSettings->secondaryBody_; + + + if( std::dynamic_pointer_cast< aerodynamics::AtmosphericFlightConditions >( + bodies.at( acceleratedBody )->getFlightConditions( ) ) == nullptr ) + { + simulation_setup::addAtmosphericFlightConditions( + bodies, acceleratedBody, secondaryBody ); + } + + std::shared_ptr< aerodynamics::AtmosphericFlightConditions > flightConditions = + std::dynamic_pointer_cast< aerodynamics::AtmosphericFlightConditions >( + bodies.at( acceleratedBody )->getFlightConditions( ) ); + + if ( std::dynamic_pointer_cast( + bodies.at( centralBody )->getAtmosphereModel( )) == nullptr ) + { + throw std::runtime_error( + "Error when saving nrlmsise input of " + centralBody + ", body has no NRLMSISE atmosphere." ); + } + auto atmosphereModel = + std::dynamic_pointer_cast( + bodies.at( centralBody )->getAtmosphereModel( ) ); + variableFunction = [=]( ) + { + atmosphereModel->setInputStruct( + flightConditions->getCurrentAltitude( ), + flightConditions->getCurrentLongitude( ), + flightConditions->getCurrentLatitude( ), + flightConditions->getCurrentTime( ) ); + return aerodynamics::getNrlmsiseInputAsVector( atmosphereModel->getNRLMSISE00InputStruct() ); + }; + parameterSize = 17; + break; + } case custom_dependent_variable: { std::shared_ptr< CustomDependentVariableSaveSettings > customVariableSettings = diff --git a/include/tudat/simulation/propagation_setup/propagationOutputSettings.h b/include/tudat/simulation/propagation_setup/propagationOutputSettings.h index 9790bc5bd5..31757f528e 100644 --- a/include/tudat/simulation/propagation_setup/propagationOutputSettings.h +++ b/include/tudat/simulation/propagation_setup/propagationOutputSettings.h @@ -141,7 +141,9 @@ enum PropagationDependentVariables vehicle_panel_body_fixed_surface_normals = 68, vehicle_surface_panel_radiation_pressure_force = 69, paneled_radiation_source_per_panel_irradiance = 70, - paneled_radiation_source_geometry = 71 + paneled_radiation_source_geometry = 71, + nrlmsise_input_data = 72 + }; // Functional base class for defining settings for dependent variables that are to be saved during propagation @@ -1448,6 +1450,13 @@ inline std::shared_ptr< SingleDependentVariableSaveSettings > paneledRadiationSo paneled_radiation_source_geometry, bodyName, sourceName ); } +inline std::shared_ptr< SingleDependentVariableSaveSettings > nrlmsiseInputDependentVariable( + const std::string& bodyName, + const std::string& centralBodyName ) +{ + return std::make_shared< SingleDependentVariableSaveSettings >( + nrlmsise_input_data, bodyName, centralBodyName ); +} } // namespace propagators diff --git a/include/tudat/simulation/propagation_setup/propagationResults.h b/include/tudat/simulation/propagation_setup/propagationResults.h index f46022d6aa..015d8ac1c2 100644 --- a/include/tudat/simulation/propagation_setup/propagationResults.h +++ b/include/tudat/simulation/propagation_setup/propagationResults.h @@ -214,7 +214,7 @@ namespace tudat std::map< double, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > getEquationsOfMotionNumericalSolutionDouble( ) { - return utilities::staticCastMapKeys< double, Time, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( equationsOfMotionNumericalSolution_ ); + return utilities::staticCastMapKeys< double, TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >( equationsOfMotionNumericalSolution_ ); } std::pair< std::vector< double >, std::vector< Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > > getEquationsOfMotionNumericalSolutionDoubleSplit( ) diff --git a/include/tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h b/include/tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h index 4dd2e5c7d3..8dcdb35cf3 100644 --- a/include/tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h +++ b/include/tudat/simulation/propagation_setup/setNumericallyIntegratedStates.h @@ -32,16 +32,28 @@ namespace propagators template< typename StateScalarType, typename TimeType > void addEmptyTabulatedEphemeris( - const simulation_setup::SystemOfBodies& bodies, const std::string& bodyName, const std::string& ephemerisOrigin = "" ) + const simulation_setup::SystemOfBodies& bodies, const std::string& bodyName, const std::string& ephemerisOrigin = "", + const bool isPartOfMultiArc = false ) { if( bodies.count( bodyName ) == 0 ) { throw std::runtime_error( "Error when setting empty tabulated ephemeris for body " + bodyName + ", no such body found" ); } std::string ephemerisOriginToUse = ( ephemerisOrigin == "" ) ? bodies.getFrameOrigin( ) : ephemerisOrigin; - bodies.at( bodyName )->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< StateScalarType, TimeType > >( - std::shared_ptr< interpolators::OneDimensionalInterpolator - < TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > >( ), ephemerisOriginToUse, bodies.getFrameOrientation( ) ) ); + std::shared_ptr< ephemerides::Ephemeris > tabulatedEphemeris = + std::make_shared< ephemerides::TabulatedCartesianEphemeris< StateScalarType, TimeType > >( + std::shared_ptr< interpolators::OneDimensionalInterpolator + < TimeType, Eigen::Matrix< StateScalarType, 6, 1 > > >( ), ephemerisOriginToUse, bodies.getFrameOrientation( ) ); + if( !isPartOfMultiArc ) + { + bodies.at( bodyName )->setEphemeris( tabulatedEphemeris ); + } + else + { + bodies.at( bodyName )->setEphemeris( std::make_shared< ephemerides::MultiArcEphemeris >( + std::map< double, std::shared_ptr< ephemerides::Ephemeris > >( + { { 0.0, tabulatedEphemeris } } ), ephemerisOriginToUse, bodies.getFrameOrientation( ) ) ); + } bodies.processBodyFrameDefinitions( ); } @@ -1181,7 +1193,8 @@ void checkTranslationalStatesFeasibility( const std::vector< std::string >& bodiesToIntegrate, const std::vector< std::string >& centralBodies, const simulation_setup::SystemOfBodies& bodies, - const bool setIntegratedResult = false ) + const bool setIntegratedResult = false, + const bool isPartOfMultiArc = false ) { // Check feasibility of epheme ris origins. @@ -1222,15 +1235,21 @@ void checkTranslationalStatesFeasibility( { if( setIntegratedResult ) { - if( bodies.at( bodyToIntegrate )->getEphemeris( ) == nullptr ) + if ( bodies.at( bodyToIntegrate )->getEphemeris( ) == nullptr ) { - addEmptyTabulatedEphemeris< StateScalarType, TimeType >( bodies, bodyToIntegrate, centralBody ); + addEmptyTabulatedEphemeris( bodies, bodyToIntegrate, centralBody, isPartOfMultiArc ); } - else if( !ephemerides::isTabulatedEphemeris( bodies.at( bodyToIntegrate )->getEphemeris( ) ) ) + else if ( !ephemerides::isTabulatedEphemeris( bodies.at( bodyToIntegrate )->getEphemeris( ) ) && !isPartOfMultiArc ) { throw std::runtime_error( "Error when checking translational dynamics feasibility of body " + bodyToIntegrate + " ephemeris exists, but is not tabulated." ); } + else if ( ( std::dynamic_pointer_cast< ephemerides::MultiArcEphemeris >( bodies.at( bodyToIntegrate )->getEphemeris( ) ) == nullptr ) + && isPartOfMultiArc ) + { + throw std::runtime_error( "Error when checking translational dynamics feasibility of body " + + bodyToIntegrate + " ephemeris exists, but is not multi-arc." ); + } } } @@ -1247,7 +1266,8 @@ void checkTranslationalStatesFeasibility( template< typename StateScalarType, typename TimeType > void checkPropagatedStatesFeasibility( const std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > propagatorSettings, - const simulation_setup::SystemOfBodies& bodies ) + const simulation_setup::SystemOfBodies& bodies, + const bool isPartOfMultiArc ) { // Check dynamics type. switch( propagatorSettings->getStateType( ) ) @@ -1278,7 +1298,7 @@ void checkPropagatedStatesFeasibility( } // Create state processor - checkPropagatedStatesFeasibility( typeIterator.second.at( i ), bodies ); + checkPropagatedStatesFeasibility( typeIterator.second.at( i ), bodies, isPartOfMultiArc ); } } else @@ -1302,7 +1322,8 @@ void checkPropagatedStatesFeasibility( checkTranslationalStatesFeasibility< StateScalarType, TimeType >( translationalPropagatorSettings->bodiesToIntegrate_, translationalPropagatorSettings->centralBodies_, bodies, - translationalPropagatorSettings->getOutputSettings( )->getSetIntegratedResult( ) ); + translationalPropagatorSettings->getOutputSettings( )->getSetIntegratedResult( ), + isPartOfMultiArc ); break; } case rotational_state: diff --git a/src/astro/aerodynamics/CMakeLists.txt b/src/astro/aerodynamics/CMakeLists.txt index 00fd5a0401..c24212b368 100644 --- a/src/astro/aerodynamics/CMakeLists.txt +++ b/src/astro/aerodynamics/CMakeLists.txt @@ -48,7 +48,6 @@ set(aerodynamics_HEADERS "windModel.h" ) -if (TUDAT_BUILD_WITH_NRLMSISE00) set(aerodynamics_SOURCES "${aerodynamics_SOURCES}" "nrlmsise00Atmosphere.cpp" @@ -70,19 +69,3 @@ if (TUDAT_BUILD_WITH_NRLMSISE00) # tudat_interpolators # PRIVATE_LINKS "${Boost_LIBRARIES}" ) - else () - # Add library. - TUDAT_ADD_LIBRARY("aerodynamics" - "${aerodynamics_SOURCES}" - "${aerodynamics_HEADERS}" - # PUBLIC_LINKS - # tudat_input_output - # tudat_geometric_shapes - # tudat_reference_frames - # tudat_interpolators - # PRIVATE_LINKS "${Boost_LIBRARIES}" - # PRIVATE_INCLUDES "${EIGEN3_INCLUDE_DIRS}" "${Boost_INCLUDE_DIRS}" - ) - -endif () - diff --git a/src/astro/aerodynamics/nrlmsise00Atmosphere.cpp b/src/astro/aerodynamics/nrlmsise00Atmosphere.cpp index c9affe9334..099557d109 100644 --- a/src/astro/aerodynamics/nrlmsise00Atmosphere.cpp +++ b/src/astro/aerodynamics/nrlmsise00Atmosphere.cpp @@ -18,6 +18,28 @@ namespace tudat namespace aerodynamics { +void NRLMSISE00Atmosphere::setInputStruct( const double altitude, const double longitude, + const double latitude, const double time ) +{ +// Retrieve input data. + inputData_ = nrlmsise00InputFunction_( + altitude, longitude, latitude, time ); + std::copy( inputData_.apVector.begin( ), inputData_.apVector.end( ), aph_.a ); + std::copy( inputData_.switches.begin( ), inputData_.switches.end( ), flags_.switches ); + + input_.g_lat = latitude * 180.0 / mathematical_constants::PI; // rad to deg + input_.g_long = longitude * 180.0 / mathematical_constants::PI; // rad to deg + input_.alt = altitude * 1.0E-3; // m to km + input_.year = inputData_.year; + input_.doy = inputData_.dayOfTheYear; + input_.sec = inputData_.secondOfTheDay; + input_.lst = inputData_.localSolarTime; + input_.f107 = inputData_.f107; + input_.f107A = inputData_.f107a; + input_.ap = inputData_.apDaily; + input_.ap_a = &aph_; +} + void NRLMSISE00Atmosphere::computeProperties( const double altitude, const double longitude, const double latitude, const double time ) @@ -32,23 +54,7 @@ void NRLMSISE00Atmosphere::computeProperties( } hashKey_ = hashKey; - // Retrieve input data. - inputData_ = nrlmsise00InputFunction_( - altitude, longitude, latitude, time ); - std::copy( inputData_.apVector.begin( ), inputData_.apVector.end( ), aph_.a ); - std::copy( inputData_.switches.begin( ), inputData_.switches.end( ), flags_.switches); - - input_.g_lat = latitude * 180.0 / mathematical_constants::PI; // rad to deg - input_.g_long = longitude * 180.0 / mathematical_constants::PI; // rad to deg - input_.alt = altitude * 1.0E-3; // m to km - input_.year = inputData_.year; - input_.doy = inputData_.dayOfTheYear; - input_.sec = inputData_.secondOfTheDay; - input_.lst = inputData_.localSolarTime; - input_.f107 = inputData_.f107; - input_.f107A = inputData_.f107a; - input_.ap = inputData_.apDaily; - input_.ap_a = &aph_; + setInputStruct( altitude, longitude, latitude, time ); // Call NRLMSISE00 gtd7(&input_, &flags_, &output_); @@ -160,5 +166,27 @@ NRLMSISE00Atmosphere::getFullOutput( const double altitude, const double longitu return output; } +Eigen::VectorXd getNrlmsiseInputAsVector( const nrlmsise_input& input ) +{ + static Eigen::VectorXd inputVector( 17 ); + inputVector( 0 ) = static_cast< double >( input.year ); + inputVector( 1 ) = static_cast< double >( input.doy ); + inputVector( 2 ) = input.sec; + inputVector( 3 ) = input.alt; + inputVector( 4 ) = input.g_lat; + inputVector( 5 ) = input.g_long; + inputVector( 6 ) = input.lst; + inputVector( 7 ) = input.f107; + inputVector( 8 ) = input.f107A; + inputVector( 9 ) = input.ap; + for( int i = 0; i < 7; i++ ) + { + inputVector( 10 + i ) = input.ap_a->a[ i ]; + } + return inputVector; + + +} + } // namespace aerodynamics } // namespace tudat diff --git a/src/astro/gravitation/basicSolidBodyTideGravityFieldVariations.cpp b/src/astro/gravitation/basicSolidBodyTideGravityFieldVariations.cpp index 330b09ccb2..ca35fc4574 100644 --- a/src/astro/gravitation/basicSolidBodyTideGravityFieldVariations.cpp +++ b/src/astro/gravitation/basicSolidBodyTideGravityFieldVariations.cpp @@ -91,7 +91,7 @@ std::pair< Eigen::MatrixXd, Eigen::MatrixXd > calculateSolidBodyTideSingleCoeffi //! Sets current properties (mass state) of body involved in tidal deformation. -void BasicSolidBodyTideGravityFieldVariations::setBodyGeometryParameters( +void SolidBodyTideGravityFieldVariations::setBodyGeometryParameters( const int bodyIndex, const double evaluationTime ) { // Calculate current state and orientation of deformed body. @@ -116,7 +116,7 @@ void BasicSolidBodyTideGravityFieldVariations::setBodyGeometryParameters( } //! Function for calculating spherical harmonic coefficient corrections. -std::pair< Eigen::MatrixXd, Eigen::MatrixXd > BasicSolidBodyTideGravityFieldVariations:: +std::pair< Eigen::MatrixXd, Eigen::MatrixXd > SolidBodyTideGravityFieldVariations:: calculateBasicSphericalHarmonicsCorrections( const double time ) { @@ -186,6 +186,94 @@ void BasicSolidBodyTideGravityFieldVariations::addBasicSolidBodyTideCorrections( } + +int getModeCoupledMaximumResponseDegree( + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > >& loveNumbers ) +{ + int maximumDegree = 0; + for( auto it : loveNumbers ) + { + for( auto it2 : it.second ) + { + if( it2.first.first > maximumDegree ) + { + maximumDegree = it2.first.first; + } + } + } + return maximumDegree; +} + +int getModeCoupledMaximumResponseOrder( + const std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > >& loveNumbers ) +{ + int maximumOrder = 0; + for( auto it : loveNumbers ) + { + for( auto it2 : it.second ) + { + if( it2.first.second > maximumOrder ) + { + maximumOrder = it2.first.second; + } + } + } + return maximumOrder; +} + + +//! Calculates basic solid body gravity field corrections due to single body. +void ModeCoupledSolidBodyTideGravityFieldVariations::addBasicSolidBodyTideCorrections( + Eigen::MatrixXd& cTermCorrections, + Eigen::MatrixXd& sTermCorrections ) +{ +// // Initialize power of radiusRatio^(N+1) (calculation starts at N=2) + + currentCosineCorrections_.setZero( ); + currentSineCorrections_.setZero( ); + + // // Iterate over all love + std::complex< double > stokesCoefficientCorrection( 0.0, 0.0 ); + + for( auto loveNumberIt : loveNumbers_ ) + { + // Retrieve forcing degree/order + std::pair< int, int > forcingDegreeOrder = loveNumberIt.first; + int n = forcingDegreeOrder.first; + int m = forcingDegreeOrder.second; + + // Compute forcing quantities + radiusRatioPower = basic_mathematics::raiseToIntegerPower( radiusRatio, n + 1 ); + updateTidalAmplitudeAndArgument( n, m ); + + // Compute response for unity Love umber + std::complex< double > unityLoveNumberStokesCoefficientCorrection = + calculateSolidBodyTideSingleCoefficientSetCorrectionFromAmplitude( + std::complex< double >( 1.0, 0.0 ), massRatio, radiusRatioPower, tideAmplitude, + tideArgument, n, m ); + + // Compute response at required degrees/orders with required love numbers + for( auto responseIt : loveNumberIt.second ) + { + int nResponse = responseIt.first.first; + int mResponse = responseIt.first.second; + stokesCoefficientCorrection = std::complex< double >( responseIt.second, 0.0 ) * + unityLoveNumberStokesCoefficientCorrection; + currentCosineCorrections_( nResponse - 2, mResponse ) += stokesCoefficientCorrection.real( ); + if ( mResponse != 0 ) + { + currentSineCorrections_( nResponse - 2, mResponse ) -= stokesCoefficientCorrection.imag( ); + } + } + } + + cTermCorrections.block( 0, 0, maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ) += + currentCosineCorrections_; + sTermCorrections.block( 0, 0, maximumDegree_ - minimumDegree_ + 1, maximumOrder_ - minimumOrder_ + 1 ) += + currentSineCorrections_; + +} + } } diff --git a/src/astro/gravitation/gravityFieldVariations.cpp b/src/astro/gravitation/gravityFieldVariations.cpp index 4eba3b4b11..43dae9cc9d 100644 --- a/src/astro/gravitation/gravityFieldVariations.cpp +++ b/src/astro/gravitation/gravityFieldVariations.cpp @@ -270,12 +270,13 @@ GravityFieldVariationsSet::getVariationFunctions( ) //! Function to retrieve the tidal gravity field variation with the specified bodies causing deformation std::shared_ptr< GravityFieldVariations > GravityFieldVariationsSet::getDirectTidalGravityFieldVariation( - const std::vector< std::string >& namesOfBodiesCausingDeformation ) + const std::vector< std::string >& namesOfBodiesCausingDeformation, + const BodyDeformationTypes tideType ) { std::shared_ptr< GravityFieldVariations > gravityFieldVariation; // Check number of variation objects of correct type - int numberOfBasicModels = std::count( variationType_.begin( ), variationType_.end( ), basic_solid_body ); + int numberOfBasicModels = std::count( variationType_.begin( ), variationType_.end( ), tideType ); // If one model of correct type is found, check if it is consistent with input if( ( numberOfBasicModels ) == 1 ) @@ -283,9 +284,9 @@ std::shared_ptr< GravityFieldVariations > GravityFieldVariationsSet::getDirectTi // Retrieve variation object. It is the return object if no namesOfBodiesCausingDeformation are given gravityFieldVariation = variationObjects_.at( ( std::distance( variationType_.begin( ), std::find( variationType_.begin( ), variationType_.end( ), - basic_solid_body ) ) ) ); - std::shared_ptr< BasicSolidBodyTideGravityFieldVariations > tidalGravityFieldVariation = - std::dynamic_pointer_cast< BasicSolidBodyTideGravityFieldVariations >( gravityFieldVariation ); + tideType ) ) ) ); + std::shared_ptr< SolidBodyTideGravityFieldVariations > tidalGravityFieldVariation = + std::dynamic_pointer_cast< SolidBodyTideGravityFieldVariations >( gravityFieldVariation ); // Check consistency if( tidalGravityFieldVariation == nullptr ) @@ -323,11 +324,11 @@ std::shared_ptr< GravityFieldVariations > GravityFieldVariationsSet::getDirectTi // Iterate over all objects and check consistency with input for( unsigned int i = 0; i < variationType_.size( ); i++ ) { - if( std::dynamic_pointer_cast< gravitation::BasicSolidBodyTideGravityFieldVariations >( + if( std::dynamic_pointer_cast< gravitation::SolidBodyTideGravityFieldVariations >( variationObjects_.at( i ) ) != nullptr ) { bool doBodiesMatch = utilities::doStlVectorContentsMatch( - std::dynamic_pointer_cast< gravitation::BasicSolidBodyTideGravityFieldVariations >( + std::dynamic_pointer_cast< gravitation::SolidBodyTideGravityFieldVariations >( variationObjects_.at( i ) )->getDeformingBodies( ), namesOfBodiesCausingDeformation ); if( doBodiesMatch ) diff --git a/src/astro/low_thrust/shape_based/sphericalShapingLeg.cpp b/src/astro/low_thrust/shape_based/sphericalShapingLeg.cpp index 60e19f43ce..c20e4c4a9d 100644 --- a/src/astro/low_thrust/shape_based/sphericalShapingLeg.cpp +++ b/src/astro/low_thrust/shape_based/sphericalShapingLeg.cpp @@ -49,8 +49,8 @@ SphericalShapingLeg::SphericalShapingLeg(const std::shared_ptr departureBodyEphemeris, @@ -217,7 +217,7 @@ double SphericalShapingLeg::convertAzimuthToTime( const double currentAzimuthAng } // Define the derivative of the time function w.r.t the currentAzimuthAngle angle theta. - std::function< double( const double ) > derivativeTimeFunction = [ = ] ( const double currentAzimuthAngle ){ + std::function< double( const double ) > derivativeTimeFunction = [&] ( const double currentAzimuthAngle ){ double scalarFunctionTimeEquation = computeScalarFunctionD(currentAzimuthAngle); @@ -687,7 +687,7 @@ double SphericalShapingLeg::computeDeltaV( ) { // Define function to integrate: time derivative of the deltaV multiplied by a factor which changes the variable of // integration from the time to the azimuth - std::function< double( const double ) > derivativeFunctionDeltaV = [ = ] ( const double currentAzimuthAngle ) + std::function< double( const double ) > derivativeFunctionDeltaV = [&] ( const double currentAzimuthAngle ) { double thrustAcceleration = computeNormalizedThrustAccelerationInSphericalCoordinates(currentAzimuthAngle).norm(); double derivativeOfTimeWithRespectToAzimuth = std::sqrt(computeScalarFunctionD(currentAzimuthAngle) diff --git a/src/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.cpp b/src/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.cpp index 6d410e342c..fed061a1cd 100644 --- a/src/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.cpp +++ b/src/astro/orbit_determination/acceleration_partials/sphericalHarmonicAccelerationPartial.cpp @@ -196,45 +196,92 @@ std::pair< std::function< void( Eigen::MatrixXd& ) >, int > SphericalHarmonicsGr // Check if partial is a tidal property of body exerting acceleration. else if( estimatable_parameters::isParameterTidalProperty( parameter->getParameterName( ).first ) ) { - // Check input consistency - std::shared_ptr< estimatable_parameters::TidalLoveNumber< Eigen::VectorXd > > tidalLoveNumber = - std::dynamic_pointer_cast< estimatable_parameters::TidalLoveNumber< Eigen::VectorXd > >( parameter ); - if( tidalLoveNumber == nullptr ) + if( parameter->getParameterName( ).first != mode_coupled_tidal_love_numbers ) { - throw std::runtime_error( "Error when getting tidal Love number vector parameter, object is nullptr" ); - } + // Check input consistency + std::shared_ptr< estimatable_parameters::TidalLoveNumber< Eigen::VectorXd > > tidalLoveNumber = + std::dynamic_pointer_cast< estimatable_parameters::TidalLoveNumber< Eigen::VectorXd > >( parameter ); + if( tidalLoveNumber == nullptr ) + { + throw std::runtime_error( "Error when getting tidal Love number vector parameter, object is nullptr" ); + } - // Get degree and order(s) of tidal variations - int degree = tidalLoveNumber->getDegree( ); - std::vector< int > orders = tidalLoveNumber->getOrders( ); - int sumOrders = tidalLoveNumber->getSumOrders( ); + // Get degree and order(s) of tidal variations + int degree = tidalLoveNumber->getDegree( ); + std::vector< int > orders = tidalLoveNumber->getOrders( ); + int sumOrders = tidalLoveNumber->getSumOrders( ); - std::pair< int, std::pair< int, int > > currentTidalPartialOutput; - for( unsigned int i = 0; i < tidalLoveNumberPartialInterfaces_.size( ); i++ ) + std::pair< int, std::pair< int, int > > currentTidalPartialOutput; + for( unsigned int i = 0; i < tidalLoveNumberPartialInterfaces_.size( ); i++ ) + { + // Check dependency on current partial object + currentTidalPartialOutput = tidalLoveNumberPartialInterfaces_.at( i )->setParameterPartialFunction( + parameter, maximumDegree_, maximumOrder_ ); + if( numberOfRows != 0 && currentTidalPartialOutput.first > 0 ) + { + throw std::runtime_error( "Error when getting vector tidal parameter partial, inconsistent output" + + std::to_string( numberOfRows ) + ", " + + std::to_string( currentTidalPartialOutput.first ) ); + } + else + { + // If tidal dependency esists, set partial function + if( currentTidalPartialOutput.first > 0 ) + { + std::function< std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > >( ) > coefficientPartialFunction = + std::bind( &orbit_determination::TidalLoveNumberPartialInterface::getCurrentVectorParameterPartial, + tidalLoveNumberPartialInterfaces_.at( i ), parameter, currentTidalPartialOutput.second ); + partialFunction = std::bind( + &SphericalHarmonicsGravityPartial::wrtTidalModelParameter, this, coefficientPartialFunction, + degree, orders, sumOrders, parameter->getParameterSize( ), std::placeholders::_1 ); + + numberOfRows = currentTidalPartialOutput.first; + + } + } + } + } + else { - // Check dependency on current partial object - currentTidalPartialOutput = tidalLoveNumberPartialInterfaces_.at( i )->setParameterPartialFunction( - parameter, maximumDegree_, maximumOrder_ ); - if( numberOfRows != 0 && currentTidalPartialOutput.first > 0 ) + // Check input consistency + std::shared_ptr< estimatable_parameters::ModeCoupledTidalLoveNumber > tidalLoveNumber = + std::dynamic_pointer_cast< estimatable_parameters::ModeCoupledTidalLoveNumber >( parameter ); + if( tidalLoveNumber == nullptr ) { - throw std::runtime_error( "Error when getting vector tidal parameter partial, inconsistent output" + - std::to_string( numberOfRows ) + ", " + - std::to_string( currentTidalPartialOutput.first ) ); + throw std::runtime_error( "Error when getting mode coupled tidal Love number vector parameter, object is nullptr" ); } - else + + + std::pair< int, std::pair< int, int > > currentTidalPartialOutput; + for( unsigned int i = 0; i < tidalLoveNumberPartialInterfaces_.size( ); i++ ) { - // If tidal dependency esists, set partial function - if( currentTidalPartialOutput.first > 0 ) + // Check dependency on current partial object + currentTidalPartialOutput = tidalLoveNumberPartialInterfaces_.at( i )->setParameterPartialFunction( + parameter, maximumDegree_, maximumOrder_ ); + if( numberOfRows != 0 && currentTidalPartialOutput.first > 0 ) { - std::function< std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > >( ) > coefficientPartialFunction = + throw std::runtime_error( "Error when getting vector tidal parameter partial, inconsistent output" + + std::to_string( numberOfRows ) + ", " + + std::to_string( currentTidalPartialOutput.first ) ); + } + else + { + // If tidal dependency esists, set partial function + if( currentTidalPartialOutput.first > 0 ) + { + std::function< std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > >( ) > coefficientPartialFunction = std::bind( &orbit_determination::TidalLoveNumberPartialInterface::getCurrentVectorParameterPartial, tidalLoveNumberPartialInterfaces_.at( i ), parameter, currentTidalPartialOutput.second ); - partialFunction = std::bind( - &SphericalHarmonicsGravityPartial::wrtTidalModelParameter, this, coefficientPartialFunction, - degree, orders, sumOrders, parameter->getParameterSize( ), std::placeholders::_1 ); + partialFunction = std::bind( + &SphericalHarmonicsGravityPartial::wrtModeCoupledLoveNumbers, this, coefficientPartialFunction, + tidalLoveNumber->getResponseIndices( ), + tidalLoveNumber->getResponseDegreeOrders( ), + tidalLoveNumber->getParameterSize( ), + std::placeholders::_1 ); - numberOfRows = currentTidalPartialOutput.first; + numberOfRows = currentTidalPartialOutput.first; + } } } } @@ -687,6 +734,51 @@ void SphericalHarmonicsGravityPartial::wrtTidalModelParameter( } } +//! Function to calculate an acceleration partial wrt a tidal parameter. +void SphericalHarmonicsGravityPartial::wrtModeCoupledLoveNumbers( + const std::function< std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > >( ) > coefficientPartialFunctions, + const std::vector< int >& responseIndices, + const std::vector< std::pair< int, int > >& responseDegreeOrders, + const int parameterSize, + Eigen::MatrixXd& partialMatrix ) +{ + // Initialize partial matrix to zero values. + partialMatrix = Eigen::Matrix< double, 3, Eigen::Dynamic >::Zero( 3, parameterSize ); + + std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > > coefficientPartialsPerForcingDegreeOrder_ = coefficientPartialFunctions( ); + + if( coefficientPartialsPerForcingDegreeOrder_.size( ) != responseIndices.size( ) ) + { + std::cout< > TidalLoveNumberPartialInterface:: +calculateSphericalHarmonicCoefficientsPartialWrtModeCoupledTidalLoveNumbers( + const std::vector< std::pair< int, int > > parameterDegreeAndOrderIndices, + const std::map< int, std::vector< int > >& ordersPerDegree, + const std::vector< int >& deformingBodyIndices, + const int maximumDegree, + const int maximumOrder ) +{ + std::vector< Eigen::Matrix< double, 2, Eigen::Dynamic > > partials; + partials.reserve( parameterDegreeAndOrderIndices.size( ) ); + deformedBodyGravitationalParameter_ = deformedBodyGravitationalParameterFunction_( ); + + // Set and calculate states and rotation needed for calculation of partial. + updateCurrentTidalBodyStates( deformingBodyIndices ); + + // Compute list of partials for all orders at requested degree + std::map< int, std::vector > partialsPerDegree; + for( auto it : ordersPerDegree ) + { + partialsPerDegree[ it.first ] = calculateCoefficientPartialWrtRealTidalLoveNumber( + it.first, it.second, deformingBodyIndices, maximumDegree, maximumOrder ); + } + + // Add all partials into single list (ordered as per parameterDegreeAndOrderIndices) + std::cout<<"Number of indices "< > TidalLoveNumberPartialInterface::setParameterPartialFunction( const std::shared_ptr< estimatable_parameters::EstimatableParameter< double > > parameter, @@ -253,6 +285,65 @@ std::pair< int, std::pair< int, int > > TidalLoveNumberPartialInterface::setPara break; } + case mode_coupled_tidal_love_numbers: + { + // Cast parameter object to required type. + std::shared_ptr< ModeCoupledTidalLoveNumber > coefficientsParameter = + std::dynamic_pointer_cast< ModeCoupledTidalLoveNumber >( parameter ); + if( coefficientsParameter == nullptr ) + { + throw std::runtime_error( + "Error when setting partil function of mode_coupled_tidal_love_numbers in TidalLoveNumberPartialInterface, input is inconsistent" ); + } + + // Retrieve the maximum degree and order that are to be used for this model + getMaximumUsedDegreeAndOrder( + maximumDegree, maximumOrder, coefficientsParameter->getMaximumForcingDegree( ), maximumUsedDegree, maximumUsedOrder ); + + // Set parameter partial function if relevant terms are found + if( maximumUsedDegree > 0 ) + { + // Check if deforming bodies correspond to bodies in model. + std::vector< int > selectedDeformingBodies = getSelectedDeformingBodyIds( + coefficientsParameter->getDeformingBodies( ) ); + if( selectedDeformingBodies.size( ) == coefficientsParameter->getDeformingBodies( ).size( ) && + coefficientsParameter->getDeformingBodies( ).size( ) == deformingBodies_.size( ) ) + { + // Add partial function if it is not yet set. + if( parameterVectorPartialFunctions_.count( + std::make_pair( parameter, std::make_pair( maximumUsedDegree, maximumUsedOrder ) ) ) == 0 ) + { +// if( coefficientsParameter->useComplexComponents( ) ) +// { +// // Calculate partials for complex love number +// parameterVectorPartialFunctions_[ std::make_pair( +// parameter, std::make_pair( maximumUsedDegree, maximumUsedOrder ) ) ] = +// std::bind( &TidalLoveNumberPartialInterface:: +// calculateSphericalHarmonicCoefficientsPartialWrtComplexTidalLoveNumbers, this, +// coefficientsParameter->getDegree( ), coefficientsParameter->getOrders( ), +// selectedDeformingBodies, maximumUsedDegree, maximumUsedOrder ); +// } +// else +// { + // Calculate partial for real love number + parameterVectorPartialFunctions_[ std::make_pair( + parameter, std::make_pair( maximumUsedDegree, maximumUsedOrder ) ) ] = + std::bind( &TidalLoveNumberPartialInterface:: + calculateSphericalHarmonicCoefficientsPartialWrtModeCoupledTidalLoveNumbers, this, + coefficientsParameter->getParameterForcingDegreeAndOrderIndices( ), + coefficientsParameter->getForcingOrdersPerDegree( ), + selectedDeformingBodies, maximumUsedDegree, maximumUsedOrder ); +// } + } + + + + numberOfRows = coefficientsParameter->getParameterSize( ); + } + } + + break; + } default: break; } diff --git a/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp b/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp index cb3cee00c8..7c29159b6b 100644 --- a/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp +++ b/src/astro/orbit_determination/estimatable_parameters/estimatableParameter.cpp @@ -159,6 +159,9 @@ std::string getParameterTypeString( const EstimatebleParametersEnum parameterTyp case source_perpendicular_direction_radiation_pressure_scaling_factor: parameterDescription = " Radiation pressure acceleration scaling factor perpendicular to source "; break; + case mode_coupled_tidal_love_numbers: + parameterDescription = " Mode-coupled tidal Love numbers"; + break; default: std::string errorMessage = "Error when getting parameter string, did not recognize parameter " + std::to_string( parameterType ); @@ -327,6 +330,9 @@ bool isDoubleParameter( const EstimatebleParametersEnum parameterType ) case source_perpendicular_direction_radiation_pressure_scaling_factor: isDoubleParameter = true; break; + case mode_coupled_tidal_love_numbers: + isDoubleParameter = false; + break; default: throw std::runtime_error( "Error, parameter type " + std::to_string( parameterType ) + " not found when getting parameter type" ); @@ -439,6 +445,9 @@ bool isParameterTidalProperty( const EstimatebleParametersEnum parameterType ) case single_degree_variable_tidal_love_number: flag = true; break; + case mode_coupled_tidal_love_numbers: + flag = true; + break; default: flag = false; break; diff --git a/src/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.cpp b/src/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.cpp index f4194b566f..2db6b82dac 100755 --- a/src/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.cpp +++ b/src/astro/orbit_determination/estimatable_parameters/tidalLoveNumber.cpp @@ -124,6 +124,95 @@ void SingleDegreeVariableTidalLoveNumber::setParameterValue( Eigen::VectorXd par gravityFieldVariationModel_->resetLoveNumbersOfDegree( fullLoveNumbers, degree_ ); } +ModeCoupledTidalLoveNumber::ModeCoupledTidalLoveNumber( + const std::shared_ptr< gravitation::ModeCoupledSolidBodyTideGravityFieldVariations > gravityFieldVariationModel, + const std::string& associatedBody, + const std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices, + const bool useComplexComponents ): + EstimatableParameter< Eigen::VectorXd >( mode_coupled_tidal_love_numbers, associatedBody ), + gravityFieldVariationModel_( gravityFieldVariationModel ), + loveNumberIndices_( loveNumberIndices ) +{ + if( useComplexComponents ) + { + throw std::runtime_error( "Error, complex mode-coupled Love numbers not yet supported" ); + } + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers = gravityFieldVariationModel->getLoveNumbers( ); + + parameterSize_ = 0.0; + maximumForcingDegree_ = 0; + + int currentResponseIndex = 0; + + std::map< int, std::map< int, int > > orderIndexPerDegree; + for( auto it: loveNumberIndices ) + { + if( it.first.first > maximumForcingDegree_ ) + { + maximumForcingDegree_ = it.first.first; + } + if( loveNumbers.count( it.first ) == 0 ) + { + throw std::runtime_error( "Error when estimating mode-coupled Love number, no number at forcing D/O " + + std::to_string( it.first.first ) + "/" + std::to_string( it.first.second ) + " found "); + } + for( unsigned int i = 0; i < it.second.size( ); i++ ) + { + if( loveNumbers.at( it.first ).count( it.second.at( i ) ) == 0 ) + { + throw std::runtime_error( "Error when estimating mode-coupled Love number, no number at forcing D/O " + + std::to_string( it.first.first ) + "/" + std::to_string( it.first.second ) + + " and response D/O " + std::to_string( it.second.at( i ).first ) + "/" + std::to_string( it.second.at( i ).second ) +" found "); + } + std::pair< int, int > currentForcingDegreeOrder = std::make_pair( + it.second.at( i ).first, it.second.at( i ).second ); + if( std::find(responseDegreeOrders_.begin(), responseDegreeOrders_.end(), currentForcingDegreeOrder) == responseDegreeOrders_.end( ) ) + { + responseDegreeOrders_.push_back( currentForcingDegreeOrder ); + currentResponseIndex = responseDegreeOrders_.size( ) - 1; + } + else + { + auto findIterator = std::find(responseDegreeOrders_.begin(), responseDegreeOrders_.end(), currentForcingDegreeOrder ); + currentResponseIndex = std::distance(responseDegreeOrders_.begin(), findIterator); + } + responseIndices_.push_back( currentResponseIndex ); + } + + int forcingDegree = it.first.first; + int forcingOrder = it.first.second; + if( forcingOrdersPerDegree_.count( forcingDegree ) == 0 ) + { + forcingOrdersPerDegree_[ forcingDegree ].push_back( forcingOrder ); + } + else + { + std::vector ordersInCurrentDegree = forcingOrdersPerDegree_.at( forcingDegree ); + if(std::find(ordersInCurrentDegree.begin(), ordersInCurrentDegree.end(), forcingOrder) == ordersInCurrentDegree.end( ) ) + { + ordersInCurrentDegree.push_back( forcingOrder ); + forcingOrdersPerDegree_[ forcingDegree ] = ordersInCurrentDegree; + } + } + std::vector ordersInCurrentDegree = forcingOrdersPerDegree_.at( forcingDegree ); + + auto findIterator = std::find(ordersInCurrentDegree.begin(), ordersInCurrentDegree.end(), forcingOrder ); + int index = std::distance(ordersInCurrentDegree.begin(), findIterator); + for( unsigned int i = 0; i < it.second.size( ); i++ ) + { + parameterForcingDegreeAndOrderIndices_.push_back( std::make_pair( forcingDegree, index )); + } + parameterSize_ += it.second.size( ); + } + + if( useComplexComponents ) + { + parameterSize_ *= 2; + } +} + + + } } diff --git a/src/astro/propagators/propagateCovariance.cpp b/src/astro/propagators/propagateCovariance.cpp index bee4edbb0a..ee72937f89 100644 --- a/src/astro/propagators/propagateCovariance.cpp +++ b/src/astro/propagators/propagateCovariance.cpp @@ -104,70 +104,70 @@ void propagateCovariance( } -//Eigen::MatrixXd convertCovarianceToFrame( -// const Eigen::MatrixXd inputCovariance, -// const Eigen::VectorXd inertialCartesianRelativeState, -// const reference_frames::SatelliteReferenceFrames inputFrame, -// const reference_frames::SatelliteReferenceFrames outputFrame ) -//{ -// Eigen::MatrixXd outputCovariance; -// if( inertialCartesianRelativeState.rows( ) % 6 != 0 ) -// { -// throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame; input size is not 6N" ); -// } -// else if( ( inputCovariance.rows( ) != inertialCartesianRelativeState.rows( ) ) || -// ( inputCovariance.cols( ) != inertialCartesianRelativeState.rows( ) ) ) -// { -// throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame, state and covariance size don't match" ); -// } -// else -// { -// int numberOfBodes = inertialCartesianRelativeState.rows( ) / 6; -// Eigen::MatrixXd covarianceTransformation = -// Eigen::MatrixXd::Zero( 6 * numberOfBodes, 6 * numberOfBodes ); -// for( int i = 0; i < numberOfBodes; i++ ) -// { -// Eigen::Matrix3d rotationMatrix = reference_frames::getRotationBetweenSatelliteFrames( -// inertialCartesianRelativeState.segment( i * 6, 6 ), inputFrame, outputFrame ); -// covarianceTransformation.block( 6 * i, 6 * i, 3, 3 ) = rotationMatrix; -// covarianceTransformation.block( 6 * i + 3, 6 * i + 3, 3, 3 ) = rotationMatrix; -// } -// outputCovariance = covarianceTransformation * inputCovariance * covarianceTransformation.transpose( ); - -// } -// return outputCovariance; -//} - -//std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame( -// const std::map< double, Eigen::MatrixXd > inputCovariances, -// const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates, -// const reference_frames::SatelliteReferenceFrames inputFrame, -// const reference_frames::SatelliteReferenceFrames outputFrame ) -//{ -// std::map< double, Eigen::MatrixXd > outputCovariances; - -// auto covarianceIterator = inputCovariances.begin( ); -// auto stateIterator = inertialCartesianRelativeStates.begin( ); - -// while( covarianceIterator != inputCovariances.end( ) ) -// { -// if( stateIterator == inertialCartesianRelativeStates.end( ) ) -// { -// throw std::runtime_error( "Error when converting covariance history, state history has ended before covariance history" ); -// } -// if( covarianceIterator->first != stateIterator->first ) -// { -// throw std::runtime_error( "Error when converting covariance history, input times are not consistent for state and covariance" ); -// } -// outputCovariances[ covarianceIterator->first ] = convertCovarianceToFrame( -// covarianceIterator->second, stateIterator->second, -// inputFrame, outputFrame ); -// covarianceIterator++; -// stateIterator++; -// } - -// return outputCovariances; -//} +Eigen::MatrixXd convertCovarianceToFrame( + const Eigen::MatrixXd inputCovariance, + const Eigen::VectorXd inertialCartesianRelativeState, + const reference_frames::SatelliteReferenceFrames inputFrame, + const reference_frames::SatelliteReferenceFrames outputFrame ) +{ + Eigen::MatrixXd outputCovariance; + if( inertialCartesianRelativeState.rows( ) % 6 != 0 ) + { + throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame; input size is not 6N" ); + } + else if( ( inputCovariance.rows( ) != inertialCartesianRelativeState.rows( ) ) || + ( inputCovariance.cols( ) != inertialCartesianRelativeState.rows( ) ) ) + { + throw std::runtime_error( "Error when converting Cartesian state covariance to alternative frame, state and covariance size don't match" ); + } + else + { + int numberOfBodes = inertialCartesianRelativeState.rows( ) / 6; + Eigen::MatrixXd covarianceTransformation = + Eigen::MatrixXd::Zero( 6 * numberOfBodes, 6 * numberOfBodes ); + for( int i = 0; i < numberOfBodes; i++ ) + { + Eigen::Matrix3d rotationMatrix = reference_frames::getRotationBetweenSatelliteFrames( + inertialCartesianRelativeState.segment( i * 6, 6 ), inputFrame, outputFrame ); + covarianceTransformation.block( 6 * i, 6 * i, 3, 3 ) = rotationMatrix; + covarianceTransformation.block( 6 * i + 3, 6 * i + 3, 3, 3 ) = rotationMatrix; + } + outputCovariance = covarianceTransformation * inputCovariance * covarianceTransformation.transpose( ); + + } + return outputCovariance; +} + +std::map< double, Eigen::MatrixXd > convertCovarianceHistoryToFrame( + const std::map< double, Eigen::MatrixXd > inputCovariances, + const std::map< double, Eigen::VectorXd > inertialCartesianRelativeStates, + const reference_frames::SatelliteReferenceFrames inputFrame, + const reference_frames::SatelliteReferenceFrames outputFrame ) +{ + std::map< double, Eigen::MatrixXd > outputCovariances; + + auto covarianceIterator = inputCovariances.begin( ); + auto stateIterator = inertialCartesianRelativeStates.begin( ); + + while( covarianceIterator != inputCovariances.end( ) ) + { + if( stateIterator == inertialCartesianRelativeStates.end( ) ) + { + throw std::runtime_error( "Error when converting covariance history, state history has ended before covariance history" ); + } + if( covarianceIterator->first != stateIterator->first ) + { + throw std::runtime_error( "Error when converting covariance history, input times are not consistent for state and covariance" ); + } + outputCovariances[ covarianceIterator->first ] = convertCovarianceToFrame( + covarianceIterator->second, stateIterator->second, + inputFrame, outputFrame ); + covarianceIterator++; + stateIterator++; + } + + return outputCovariances; +} void convertCovarianceHistoryToFormalErrorHistory( @@ -181,6 +181,7 @@ void convertCovarianceHistoryToFormalErrorHistory( Eigen::VectorXd( covarianceIterator.second.diagonal( ).array( ).sqrt( ) ); } } + //! Function to propagate full covariance at the initial time to state formal errors at later times void propagateFormalErrors( std::map< double, Eigen::VectorXd >& propagatedFormalErrors, diff --git a/src/astro/reference_frames/referenceFrameTransformations.cpp b/src/astro/reference_frames/referenceFrameTransformations.cpp index 57ff06acd3..644ad3ebb5 100644 --- a/src/astro/reference_frames/referenceFrameTransformations.cpp +++ b/src/astro/reference_frames/referenceFrameTransformations.cpp @@ -597,12 +597,12 @@ Eigen::Matrix3d getJ2000toECLIPJ2000TransformationMatrix () //! Get transformation matrix from ECLIPJ2000 to J2000 Eigen::Matrix3d getECLIPJ2000toJ2000TransformationMatrix () - { - return (Eigen::Matrix3d() << - 1, 0, 0, - 0, 0.9174820620691818, -0.3977771559319137, - 0, 0.3977771559319137, 0.9174820620691818).finished() ; - } +{ + return (Eigen::Matrix3d() << + 1, 0, 0, + 0, 0.9174820620691818, -0.3977771559319137, + 0, 0.3977771559319137, 0.9174820620691818).finished() ; +} //! Function to compute the derivative of a rotation about the x-axis w.r.t. the rotation angle Eigen::Matrix3d getDerivativeOfXAxisRotationWrtAngle( const double angle ) @@ -672,6 +672,51 @@ Eigen::Vector3d getBodyFixedSphericalPosition( return sphericalPosition; } +Eigen::Matrix3d getRotationBetweenSatelliteFrames( + const Eigen::Vector6d relativeInertialCartesianState, + const SatelliteReferenceFrames originalFrame, + const SatelliteReferenceFrames targetFrame ) +{ + Eigen::Matrix3d rotationMatrix; + if ( !( originalFrame == global_reference_frame || targetFrame == global_reference_frame )) + { + rotationMatrix = getRotationBetweenSatelliteFrames( + relativeInertialCartesianState, originalFrame, global_reference_frame ) * + getRotationBetweenSatelliteFrames( + relativeInertialCartesianState, global_reference_frame, targetFrame ); + + } + else if ( targetFrame == global_reference_frame ) + { + rotationMatrix = getRotationBetweenSatelliteFrames( + relativeInertialCartesianState, targetFrame, originalFrame ).transpose( ); + } + else if ( originalFrame == global_reference_frame ) + { + switch ( targetFrame ) + { + case global_reference_frame: + rotationMatrix = Eigen::Matrix3d::Identity( ); + break; + case rsw_reference_frame: + rotationMatrix = getInertialToRswSatelliteCenteredFrameRotationMatrix( + relativeInertialCartesianState ); + break; + case tnw_reference_frame: + rotationMatrix = getInertialToTnwRotation( + relativeInertialCartesianState, true ); + break; + default: + throw std::runtime_error( "Error when converting between satellite frames, target frame not recognized" ); + } + } + else + { + throw std::runtime_error( "Error when converting between satellite frames, could not parse frames." ); + } + return rotationMatrix; +} + Eigen::Matrix3d getItrf2014ToArbitraryItrfRotationMatrix( const std::string& targetFrame ) { double d = 0.0, r1 = 0.0, r2 = 0.0, r3 = 0.0; diff --git a/src/io/extractSolarActivityData.cpp b/src/io/extractSolarActivityData.cpp index cca3edac8e..4681734579 100644 --- a/src/io/extractSolarActivityData.cpp +++ b/src/io/extractSolarActivityData.cpp @@ -55,13 +55,16 @@ std::shared_ptr< SolarActivityData > ExtractSolarActivityData::extract( centered81DaySolarRadioFlux107Observed, last81DaySolarRadioFlux107Observed ); - // Create the resulting solar activity data object (will be returned at the end) - std::shared_ptr< SolarActivityData > solarActivityContainer( new SolarActivityData( ) ); // Convert string data and append to solar activity data object - solarActivityContainer->year = getField< unsigned int >( data, year ); - solarActivityContainer->month = getField< unsigned int >( data, month ); - solarActivityContainer->day = getField< unsigned int >( data, day ); + int currentYear = getField< unsigned int >( data, year ); + int currentMonth = getField< unsigned int >( data, month ); + int currentDay = getField< unsigned int >( data, day ); + + // Create the resulting solar activity data object (will be returned at the end) + std::shared_ptr< SolarActivityData > solarActivityContainer = + std::make_shared< SolarActivityData >( currentYear, currentMonth, currentDay ); + solarActivityContainer->bartelsSolarRotationNumber = getField< unsigned int >( data, bartelsSolarRotationNumber ); solarActivityContainer->dayOfBartelsCycle = getField< unsigned int >( data, dayOfBartelsCycle ); diff --git a/src/io/solarActivityData.cpp b/src/io/solarActivityData.cpp index df898991e1..4fae04e3eb 100644 --- a/src/io/solarActivityData.cpp +++ b/src/io/solarActivityData.cpp @@ -36,7 +36,8 @@ namespace solar_activity { //! Default constructor. -SolarActivityData::SolarActivityData( ) : year( 0 ), month( 0 ), day( 0 ), +SolarActivityData::SolarActivityData( const int yearInput, const int monthInput, const int dayInput ): + year( yearInput ), month( monthInput ), day( dayInput ), bartelsSolarRotationNumber( 0 ), dayOfBartelsCycle( 0 ), planetaryRangeIndexSum( 0 ), planetaryEquivalentAmplitudeAverage( 0 ), planetaryDailyCharacterFigure( -0.0 ), planetaryDailyCharacterFigureConverted( 0 ), internationalSunspotNumber( 0 ), @@ -119,12 +120,14 @@ SolarActivityDataMap readSolarActivityData( std::string filePath ) double julianDate = TUDAT_NAN; // Save each line to datamap - for(int i = 0 ; i < numberOfLines ; i++ ){ + for(int i = 0 ; i < numberOfLines ; i++ ) + { julianDate = tudat::basic_astrodynamics::convertCalendarDateToJulianDay( solarActivityExtractor.extract( parsedDataVector->at( i ) )->year, solarActivityExtractor.extract( parsedDataVector->at( i ) )->month, solarActivityExtractor.extract( parsedDataVector->at( i ) )->day, 0, 0, 0.0 ) ; + dataMap[ julianDate ] = solarActivityExtractor.extract( parsedDataVector->at( i ) ) ; } diff --git a/src/simulation/environment_setup/createGravityFieldVariations.cpp b/src/simulation/environment_setup/createGravityFieldVariations.cpp index 784d3eee52..6b3baca5f0 100644 --- a/src/simulation/environment_setup/createGravityFieldVariations.cpp +++ b/src/simulation/environment_setup/createGravityFieldVariations.cpp @@ -127,6 +127,7 @@ std::shared_ptr< gravitation::GravityFieldVariations > createGravityFieldVariati switch( gravityFieldVariationSettings->getBodyDeformationType( ) ) { case basic_solid_body: + case mode_coupled_solid_body: { std::shared_ptr< TimeDependentSphericalHarmonicsGravityField > gravityField = std::dynamic_pointer_cast< TimeDependentSphericalHarmonicsGravityField >( @@ -138,100 +139,149 @@ std::shared_ptr< gravitation::GravityFieldVariations > createGravityFieldVariati } // Check consistency - std::shared_ptr< BasicSolidBodyGravityFieldVariationSettings > - basicSolidBodyGravityVariationSettings = - std::dynamic_pointer_cast< BasicSolidBodyGravityFieldVariationSettings >( - gravityFieldVariationSettings ); - if( basicSolidBodyGravityVariationSettings == nullptr ) + std::vector< std::string > deformingBodies; + if( gravityFieldVariationSettings->getBodyDeformationType( ) == basic_solid_body ) { - throw std::runtime_error( "Error, expected basic solid body gravity field settings for " + body ); + std::shared_ptr< BasicSolidBodyGravityFieldVariationSettings > + basicSolidBodyGravityVariationSettings = + std::dynamic_pointer_cast< BasicSolidBodyGravityFieldVariationSettings >( + gravityFieldVariationSettings ); + if( basicSolidBodyGravityVariationSettings == nullptr ) + { + throw std::runtime_error( "Error, expected basic solid body gravity field settings for " + body ); + } + else + { + deformingBodies = basicSolidBodyGravityVariationSettings->getDeformingBodies( ); + } + } + else if( gravityFieldVariationSettings->getBodyDeformationType( ) == mode_coupled_solid_body ) + { + std::shared_ptr< ModeCoupledSolidBodyGravityFieldVariationSettings > + modeCoupledSolidBodyGravityVariationSettings = + std::dynamic_pointer_cast< ModeCoupledSolidBodyGravityFieldVariationSettings >( + gravityFieldVariationSettings ); + if( modeCoupledSolidBodyGravityVariationSettings == nullptr ) + { + throw std::runtime_error( "Error, expected mode-coupled solid body gravity field settings for " + body ); + } + else + { + deformingBodies = modeCoupledSolidBodyGravityVariationSettings->getDeformingBodies( ); + } } else { - // Define list of required input. - std::vector< std::string > deformingBodies - = basicSolidBodyGravityVariationSettings->getDeformingBodies( ); - std::function< Eigen::Vector6d( const double ) > deformedBodyStateFunction; - std::function< Eigen::Quaterniond( const double ) > deformedBodyOrientationFunction; - std::vector< std::function< Eigen::Vector6d( const double ) > > - deformingBodyStateFunctions; - std::vector< std::function< double( ) > > gravitionalParametersOfDeformingBodies; - - // Iterate over all bodies causing tidal perturbation. - for( unsigned int i = 0; i < deformingBodies.size( ); i++ ) + throw std::runtime_error( "Error, incorrect variation type when making tidal gravity field variation" ); + } + + // Define list of required input. + std::function< Eigen::Vector6d( const double ) > deformedBodyStateFunction; + std::function< Eigen::Quaterniond( const double ) > deformedBodyOrientationFunction; + std::vector< std::function< Eigen::Vector6d( const double ) > > + deformingBodyStateFunctions; + std::vector< std::function< double( ) > > gravitionalParametersOfDeformingBodies; + + // Iterate over all bodies causing tidal perturbation. + for( unsigned int i = 0; i < deformingBodies.size( ); i++ ) + { + // Check if perturbing body exists. + if( bodies.count( deformingBodies[ i ] ) == 0 ) { - // Check if perturbing body exists. - if( bodies.count( deformingBodies[ i ] ) == 0 ) - { - throw std::runtime_error( "Error when making basic solid body gravity field variation, " + - deformingBodies[ i ] + " deforming body not found." ); - } - - // Create body state functions (depending on whether the variation is calculated - // directly during propagation, or a priori by an interpolator - if( gravityFieldVariationSettings->getInterpolatorSettings( ) != nullptr ) - { - deformingBodyStateFunctions.push_back( - std::bind( - &Body::getStateInBaseFrameFromEphemeris< double, double >, - bodies.at( deformingBodies[ i ] ), std::placeholders::_1 ) ); - } - else - { - deformingBodyStateFunctions.push_back( - std::bind( &Body::getState, bodies.at( deformingBodies[ i ] ) ) ); - } - - // Get gravitational parameter of perturbing bodies. - if( bodies.at( deformingBodies[ i ] )->getGravityFieldModel( ) == nullptr ) - { - throw std::runtime_error( - "Error, could not find gravity field model in body " + deformingBodies[ i ] + - " when making basic sh variation for body " + body ); - } - else - { - gravitionalParametersOfDeformingBodies.push_back( - std::bind( &GravityFieldModel::getGravitationalParameter, - bodies.at( deformingBodies[ i ] ) - ->getGravityFieldModel( ) ) ); - } + throw std::runtime_error( "Error when making basic solid body gravity field variation, " + + deformingBodies[ i ] + " deforming body not found." ); } - // Set state and orientation functions of perturbed body. + // Create body state functions (depending on whether the variation is calculated + // directly during propagation, or a priori by an interpolator if( gravityFieldVariationSettings->getInterpolatorSettings( ) != nullptr ) { - deformedBodyStateFunction = std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, - bodies.at( body ), std::placeholders::_1 ); - deformedBodyOrientationFunction = std::bind( - &ephemerides::RotationalEphemeris::getRotationToTargetFrame, - bodies.at( body )->getRotationalEphemeris( ), std::placeholders::_1 ); + deformingBodyStateFunctions.push_back( + std::bind( + &Body::getStateInBaseFrameFromEphemeris< double, double >, + bodies.at( deformingBodies[ i ] ), std::placeholders::_1 ) ); } else { - deformedBodyStateFunction = std::bind( &Body::getState, bodies.at( body ) ); - deformedBodyOrientationFunction = std::bind( &Body::getCurrentRotationToLocalFrame, - bodies.at( body ) ); + deformingBodyStateFunctions.push_back( + std::bind( &Body::getState, bodies.at( deformingBodies[ i ] ) ) ); + } - + // Get gravitational parameter of perturbing bodies. + if( bodies.at( deformingBodies[ i ] )->getGravityFieldModel( ) == nullptr ) + { + throw std::runtime_error( + "Error, could not find gravity field model in body " + deformingBodies[ i ] + + " when making basic sh variation for body " + body ); } + else + { + gravitionalParametersOfDeformingBodies.push_back( + std::bind( &GravityFieldModel::getGravitationalParameter, + bodies.at( deformingBodies[ i ] ) + ->getGravityFieldModel( ) ) ); + } + } + + // Set state and orientation functions of perturbed body. + if( gravityFieldVariationSettings->getInterpolatorSettings( ) != nullptr ) + { + deformedBodyStateFunction = std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, + bodies.at( body ), std::placeholders::_1 ); + deformedBodyOrientationFunction = std::bind( + &ephemerides::RotationalEphemeris::getRotationToTargetFrame, + bodies.at( body )->getRotationalEphemeris( ), std::placeholders::_1 ); + } + else + { + deformedBodyStateFunction = std::bind( &Body::getState, bodies.at( body ) ); + deformedBodyOrientationFunction = std::bind( &Body::getCurrentRotationToLocalFrame, + bodies.at( body ) ); + + + } - std::function< double( ) > gravitionalParameterOfDeformedBody = - std::bind( &GravityFieldModel::getGravitationalParameter, - bodies.at( body )->getGravityFieldModel( ) ); - - // Create basic tidal variation object. + std::function< double( ) > gravitionalParameterOfDeformedBody = + std::bind( &GravityFieldModel::getGravitationalParameter, + bodies.at( body )->getGravityFieldModel( ) ); + + + // Create basic tidal variation object. + if( gravityFieldVariationSettings->getBodyDeformationType( ) == basic_solid_body ) + { + std::shared_ptr< BasicSolidBodyGravityFieldVariationSettings > + basicSolidBodyGravityVariationSettings = + std::dynamic_pointer_cast< BasicSolidBodyGravityFieldVariationSettings >( + gravityFieldVariationSettings ); gravityFieldVariationModel - = std::make_shared< BasicSolidBodyTideGravityFieldVariations >( - deformedBodyStateFunction, - deformedBodyOrientationFunction, - deformingBodyStateFunctions, - gravityField->getReferenceRadius( ), - gravitionalParameterOfDeformedBody, - gravitionalParametersOfDeformingBodies, - basicSolidBodyGravityVariationSettings->getLoveNumbers( ), - deformingBodies ); + = std::make_shared( + deformedBodyStateFunction, + deformedBodyOrientationFunction, + deformingBodyStateFunctions, + gravityField->getReferenceRadius( ), + gravitionalParameterOfDeformedBody, + gravitionalParametersOfDeformingBodies, + basicSolidBodyGravityVariationSettings->getLoveNumbers( ), + deformingBodies ); } + else if( gravityFieldVariationSettings->getBodyDeformationType( ) == mode_coupled_solid_body ) + { + std::shared_ptr< ModeCoupledSolidBodyGravityFieldVariationSettings > + modeCoupledSolidBodyGravityVariationSettings = + std::dynamic_pointer_cast< ModeCoupledSolidBodyGravityFieldVariationSettings >( + gravityFieldVariationSettings ); + gravityFieldVariationModel + = std::make_shared( + deformedBodyStateFunction, + deformedBodyOrientationFunction, + deformingBodyStateFunctions, + gravityField->getReferenceRadius( ), + gravitionalParameterOfDeformedBody, + gravitionalParametersOfDeformingBodies, + modeCoupledSolidBodyGravityVariationSettings->getLoveNumbers( ), + deformingBodies ); + } + break; } case tabulated_variation: diff --git a/src/simulation/estimation_setup/createAccelerationPartials.cpp b/src/simulation/estimation_setup/createAccelerationPartials.cpp index 6775814242..74a3debf8a 100644 --- a/src/simulation/estimation_setup/createAccelerationPartials.cpp +++ b/src/simulation/estimation_setup/createAccelerationPartials.cpp @@ -30,9 +30,9 @@ std::vector< std::shared_ptr< orbit_determination::TidalLoveNumberPartialInterfa if( bodies.at( acceleratingBodyName )->getGravityFieldVariationSet( ) != nullptr ) { // Get list of tidal gravity field variations. - std::vector< std::shared_ptr< gravitation::BasicSolidBodyTideGravityFieldVariations > > variationObjectList = + std::vector< std::shared_ptr< gravitation::SolidBodyTideGravityFieldVariations > > variationObjectList = utilities::dynamicCastSVectorToTVector< gravitation::GravityFieldVariations, - gravitation::BasicSolidBodyTideGravityFieldVariations >( + gravitation::SolidBodyTideGravityFieldVariations >( bodies.at( acceleratingBodyName )->getGravityFieldVariationSet( )-> getDirectTidalGravityFieldVariations( ) ); diff --git a/src/simulation/estimation_setup/createLightTimeCorrection.cpp b/src/simulation/estimation_setup/createLightTimeCorrection.cpp index 15444e3791..d011a4ffc9 100644 --- a/src/simulation/estimation_setup/createLightTimeCorrection.cpp +++ b/src/simulation/estimation_setup/createLightTimeCorrection.cpp @@ -460,7 +460,7 @@ std::shared_ptr< LightTimeCorrection > createLightTimeCorrections( "Error when creating inverse power series solar corona correction: incompatible settings type." ); } - std::string sunBodyName = coronaCorrectionSettings->getSunBodyName( ); + std::string sunBodyName = "Sun";//coronaCorrectionSettings->getSunBodyName( ); std::function< Eigen::Vector6d( const double ) > sunStateFunction = std::bind( &simulation_setup::Body::getStateInBaseFrameFromEphemeris< double, double >, @@ -606,6 +606,11 @@ std::function< double ( std::vector< FrequencyBands >, double ) > createLinkFreq { double frequency = transmittedFrequencyCalculator->getTemplatedCurrentFrequency< double, double >( time ); + if( frequencyBands.size( ) + 1 < turnaroundRatioFunctions.size( ) ) + { + throw std::runtime_error( "Error when computing link frequency, found incompatible number of frequency bands: " + + std::to_string( frequencyBands.size( ) ) + " and turnaround ratios " + std::to_string( turnaroundRatioFunctions.size( ) ) ); + } for ( unsigned int i = 0; i < turnaroundRatioFunctions.size( ); ++i ) { frequency *= turnaroundRatioFunctions.at( i )( frequencyBands.at( i ), frequencyBands.at( i + 1 ) ); diff --git a/src/simulation/propagation_setup/createEnvironmentUpdater.cpp b/src/simulation/propagation_setup/createEnvironmentUpdater.cpp index 84ee6248b7..ad8c6f4b3a 100644 --- a/src/simulation/propagation_setup/createEnvironmentUpdater.cpp +++ b/src/simulation/propagation_setup/createEnvironmentUpdater.cpp @@ -1131,6 +1131,9 @@ std::vector< std::string > > createEnvironmentUpdaterSettingsForDependentVariabl break; case paneled_radiation_source_geometry: break; + case nrlmsise_input_data: + variablesToUpdate[ vehicle_flight_conditions_update ].push_back( dependentVariableSaveSettings->associatedBody_ ); + break; default: throw std::runtime_error( "Error when getting environment updates for dependent variables, parameter " + std::to_string( dependentVariableSaveSettings->dependentVariableType_ ) + " not found." ); diff --git a/src/simulation/propagation_setup/propagationOutput.cpp b/src/simulation/propagation_setup/propagationOutput.cpp index 74873e7ba3..5475079d2b 100644 --- a/src/simulation/propagation_setup/propagationOutput.cpp +++ b/src/simulation/propagation_setup/propagationOutput.cpp @@ -558,6 +558,9 @@ int getDependentVariableSize( } break; } + case nrlmsise_input_data: + variableSize = 17; + break; default: std::string errorMessage = "Error, did not recognize dependent variable size of type: " + std::to_string( dependentVariableSettings->dependentVariableType_ ); diff --git a/src/simulation/propagation_setup/propagationOutputSettings.cpp b/src/simulation/propagation_setup/propagationOutputSettings.cpp index cbd40dc8b1..371dcbea29 100644 --- a/src/simulation/propagation_setup/propagationOutputSettings.cpp +++ b/src/simulation/propagation_setup/propagationOutputSettings.cpp @@ -322,6 +322,9 @@ std::string getDependentVariableName( case paneled_radiation_source_geometry: variableName = "Per-source panel radiation pressure geometry"; break; + case nrlmsise_input_data: + variableName = "NRLMSISE00 input data vector"; + break; default: std::string errorMessage = "Error, dependent variable " + std::to_string( propagationDependentVariables ) + diff --git a/tests/src/astro/aerodynamics/CMakeLists.txt b/tests/src/astro/aerodynamics/CMakeLists.txt index b1378948b2..9c346a251f 100644 --- a/tests/src/astro/aerodynamics/CMakeLists.txt +++ b/tests/src/astro/aerodynamics/CMakeLists.txt @@ -77,9 +77,7 @@ TUDAT_ADD_TEST_CASE(WindModel ${Tudat_PROPAGATION_LIBRARIES} ) -if (TUDAT_BUILD_WITH_NRLMSISE00) - TUDAT_ADD_TEST_CASE(NRLMSISE00Atmosphere - PRIVATE_LINKS - ${Tudat_PROPAGATION_LIBRARIES} - ) -endif () +TUDAT_ADD_TEST_CASE(NRLMSISE00Atmosphere + PRIVATE_LINKS + ${Tudat_PROPAGATION_LIBRARIES} + ) diff --git a/tests/src/astro/aerodynamics/unitTestNRLMSISE00Atmosphere.cpp b/tests/src/astro/aerodynamics/unitTestNRLMSISE00Atmosphere.cpp index 65f61a220d..8988d01e0b 100644 --- a/tests/src/astro/aerodynamics/unitTestNRLMSISE00Atmosphere.cpp +++ b/tests/src/astro/aerodynamics/unitTestNRLMSISE00Atmosphere.cpp @@ -33,6 +33,7 @@ #include "tudat/astro/aerodynamics/nrlmsise00InputFunctions.h" #include "tudat/math/basic/mathematicalConstants.h" +#include "tudat/simulation/simulation.h" namespace tudat { @@ -43,9 +44,9 @@ namespace unit_tests //! Test NRLMSISE00 atmosphere model. BOOST_AUTO_TEST_SUITE( test_nrlmsise00_atmosphere ) +using namespace tudat; using tudat::aerodynamics::NRLMSISE00Input; using tudat::aerodynamics::NRLMSISE00Atmosphere; - using tudat::mathematical_constants::PI; // Global variable to be changed by tests and function. @@ -488,7 +489,7 @@ BOOST_AUTO_TEST_CASE( testNRLMSISE00AtmosphereTest8 ) 1.759874640391E+05, 5.501648779570E+02, 1.571888739255E-15, 8.896775722935E+04, 1.979740836233E+06, 9.121814875991E+03, 1.031247440715E+03, 1.024848492213E+03 }; - + // Create the model NRLMSISE00Atmosphere model( std::bind( &nrlmsiseTestFunction, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, false, false ) ); @@ -620,7 +621,7 @@ BOOST_AUTO_TEST_CASE( testNRLMSISE00AtmosphereTest11 ) 5.498695433719E+18, 2.451733158028E+17, 1.261065661119E-03, 0.000000000000E+00, 0.000000000000E+00, 0.000000000000E+00, 1.027318464900E+03, 2.814647576632E+02 }; - + // Create the model NRLMSISE00Atmosphere model( std::bind( &nrlmsiseTestFunction, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, false, false ) ); @@ -752,7 +753,7 @@ BOOST_AUTO_TEST_CASE( testNRLMSISE00AtmosphereTest14 ) 5.645392443377E+15, 2.517141749411E+14, 1.294709015929E-06, 0.000000000000E+00, 0.000000000000E+00, 0.000000000000E+00, 1.027318464900E+03, 2.795551129541E+02 }; - + // Create the model NRLMSISE00Atmosphere model( std::bind( &nrlmsiseTestFunction, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, false, false ) ); @@ -1137,8 +1138,173 @@ BOOST_AUTO_TEST_CASE( test_nrlmise_FullFileLoad ) } +BOOST_AUTO_TEST_CASE( testNRLMSISEInPropagation ) +//int main( ) +{ + using namespace aerodynamics; + using namespace numerical_integrators; + using namespace simulation_setup; + using namespace propagators; + using namespace basic_mathematics; + using namespace basic_astrodynamics; + using namespace orbital_element_conversions; + using namespace spice_interface; + + // Load Spice kernels. + loadStandardSpiceKernels( ); + + // Set simulation start epoch. + const double simulationStartEpoch = 0.0; + const double simulationEndEpoch = 300.0; + const double fixedStepSize = 1.0; + + // Set Keplerian elements for Capsule. + Eigen::Vector6d apolloInitialStateInKeplerianElements; + apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3; + apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005; + apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + // Convert apollo state from Keplerian elements to Cartesian elements. + const double earthGravitationalParameter = getBodyGravitationalParameter( "Earth" ); + const Eigen::Vector6d apolloInitialState = convertKeplerianToCartesianElements( + apolloInitialStateInKeplerianElements, earthGravitationalParameter ); + + + // Define simulation body settings. + BodyListSettings bodySettings = + getDefaultBodySettings( { "Earth", "Moon" }, "Earth", "ECLIPJ2000" ); + bodySettings.at( "Earth" )->gravityFieldSettings = + std::make_shared< simulation_setup::GravityFieldSettings >( central_spice ); + bodySettings.at( "Earth" )->atmosphereSettings = + std::make_shared< simulation_setup::AtmosphereSettings >( nrlmsise00 ); + + + // Create Earth object + simulation_setup::SystemOfBodies bodies = simulation_setup::createSystemOfBodies( bodySettings ); + + // Create vehicle objects. + bodies.createEmptyBody( "Apollo" ); + bodies.at( "Apollo" )->setConstantBodyMass( 2000.0 ); + + // Create vehicle aerodynamic coefficients + double referenceArea = 4.0; + double aerodynamicCoefficient = 1.2; + std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings = + std::make_shared< ConstantAerodynamicCoefficientSettings >( + referenceArea, aerodynamicCoefficient * Eigen::Vector3d::UnitX( ), negative_aerodynamic_frame_coefficients ); + + // Create and set aerodynamic coefficients object + bodies.at( "Apollo" )->setAerodynamicCoefficientInterface( + createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Apollo", bodies ) ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define acceleration model settings. + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfApollo; + accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); + accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) ); + accelerationsOfApollo[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); + accelerationMap[ "Apollo" ] = accelerationsOfApollo; + + bodiesToPropagate.push_back( "Apollo" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + Eigen::Vector6d systemInitialState = apolloInitialState; + + // Define list of dependent variables to save. + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + altitude_dependent_variable, "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + local_density_dependent_variable, "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + body_fixed_relative_spherical_position, "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( nrlmsise_input_data, "Apollo", "Earth" ) ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToPropagate, centralBodies ); + + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, simulationStartEpoch, fixedStepSize ); + + std::shared_ptr< TranslationalStatePropagatorSettings < double > > propagatorSettings = + std::make_shared< TranslationalStatePropagatorSettings< double > >( + centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationStartEpoch, integratorSettings, + std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), cowell, + dependentVariables ); + + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodies, propagatorSettings ); + + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > dependentVariableOutput = + dynamicsSimulator.getDependentVariableHistory( ); + + nrlmsise_flags flags; + nrlmsise_input input; + nrlmsise_output output; + auto nrlmsiseInputFunction = + std::dynamic_pointer_cast< NRLMSISE00Atmosphere >( bodies.at( "Earth" )->getAtmosphereModel( ) )->getNrlmsise00InputFunction( ); + + for( auto it : dependentVariableOutput ) + { + double altitude = it.second( 0 ); + double density = it.second( 1 ); + Eigen::Vector3d sphericalPosition = it.second.segment( 2, 3 ); + Eigen::VectorXd nrlmsiseInputVector = it.second.segment( 5, 17 ); + + NRLMSISE00Input inputData = nrlmsiseInputFunction( + altitude, sphericalPosition( 2 ), sphericalPosition( 1 ), it.first ); + + BOOST_CHECK_EQUAL( nrlmsiseInputVector( 0 ), inputData.year ); + BOOST_CHECK_EQUAL( nrlmsiseInputVector( 1 ), inputData.dayOfTheYear ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 2 ), inputData.secondOfTheDay, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 3 ) * 1000.0, altitude, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 4 ), sphericalPosition( 1 ) * 180.0 / mathematical_constants::PI, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 5 ), sphericalPosition( 2 ) * 180.0 / mathematical_constants::PI, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 6 ), inputData.localSolarTime, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 7 ), inputData.f107, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 8 ), inputData.f107a, 1.0E-14 ); + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 9 ), inputData.apDaily, 1.0E-14 ); + for( int i = 0; i < 7; i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( nrlmsiseInputVector( 10 + i ), inputData.apVector.at( i ), 1.0E-14 ); + } + + double manualDensity = std::dynamic_pointer_cast< NRLMSISE00Atmosphere >( bodies.at( "Earth" )->getAtmosphereModel( ) )->getDensity( + altitude, sphericalPosition( 2 ), sphericalPosition( 1 ), it.first ); + BOOST_CHECK_CLOSE_FRACTION( density, manualDensity, 1.0E-14 ); + + +// std::copy( inputData_.switches.begin( ), inputData_.switches.end( ), flags_.switches ); +// +// +// gtd7(&input_, &flags, &output ); +// +// // Retrieve density and temperature +// double reconstructeDensity_ = output_.d[ 5 ] * 1000.0; + } } +BOOST_AUTO_TEST_SUITE_END( ) + + } // namespace unit_tests } // namespace tudat diff --git a/tests/src/astro/electromagnetism/unitTestRadiationPressureAcceleration.cpp b/tests/src/astro/electromagnetism/unitTestRadiationPressureAcceleration.cpp index ae9ae39e00..fbe0bf5013 100644 --- a/tests/src/astro/electromagnetism/unitTestRadiationPressureAcceleration.cpp +++ b/tests/src/astro/electromagnetism/unitTestRadiationPressureAcceleration.cpp @@ -46,7 +46,7 @@ namespace tudat { namespace unit_tests { - +using namespace tudat; using namespace tudat::electromagnetism; BOOST_AUTO_TEST_SUITE(test_radiation_pressure_acceleration) @@ -549,45 +549,49 @@ BOOST_AUTO_TEST_CASE( testRadiationPressureAcceleration_IsotropicPointSource_Pan // Calculated acceleration if Sun-Vehicle vector aligned with +X-axis (acceleration generated by panels whose normals are // along +X-axis only) - double cosPanelInclinationPositiveXaxis = expectedVehicleToSunVectorNormalized.dot(Eigen::Vector3d::UnitX( ) ); - Eigen::Vector3d accelerationPositiveXaxisOrientedPanels = - cosPanelInclinationPositiveXaxis - * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() - * ( areas[5] * (( 1.0 - specularReflectivities[5] ) * expectedVehicleToSunVectorNormalized - + 2.0 / 3.0 * diffuseReflectivities[5] * Eigen::Vector3d::UnitX( ) ) - + areas[6] * (( 1.0 - specularReflectivities[6] ) * expectedVehicleToSunVectorNormalized - + ( 2.0 / 3.0 * diffuseReflectivities[6] + 2.0 * cosPanelInclinationPositiveXaxis * specularReflectivities[6] ) - * Eigen::Vector3d::UnitX( ) ) ); - - - // Calculated acceleration generated by the panels whose normals are along -X-axis. - double cosPanelInclinationNegativeXaxis = expectedVehicleToSunVectorNormalized.dot(- Eigen::Vector3d::UnitX() ); - Eigen::Vector3d accelerationNegativeXaxisOrientedPanels = - cosPanelInclinationNegativeXaxis - * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() - * ( areas[3] * (( 1.0 - specularReflectivities[3] ) * expectedVehicleToSunVectorNormalized - + 2.0 / 3.0 * diffuseReflectivities[3] * - Eigen::Vector3d::UnitX( ) ) - + areas[4] * (( 1.0 - specularReflectivities[4] ) * expectedVehicleToSunVectorNormalized - + ( 2.0 / 3.0 * diffuseReflectivities[4] + 2.0 * specularReflectivities[4] * cosPanelInclinationNegativeXaxis ) - * - Eigen::Vector3d::UnitX( ) ) ); - - // Calculated acceleration generated by the panels whose normals are along +Y-axis. - double cosPanelInclinationPositiveYaxis = expectedVehicleToSunVectorNormalized.dot(Eigen::Vector3d::UnitY() ); - Eigen::Vector3d accelerationPositiveYaxisOrientedPanels = - cosPanelInclinationPositiveYaxis + Eigen::Vector3d accelerationPositiveXaxisOrientedPanels, accelerationNegativeXaxisOrientedPanels, accelerationPositiveYaxisOrientedPanels, accelerationNegativeYaxisOrientedPanels; + if( testCase < 3 ) + { + double cosPanelInclinationPositiveXaxis = expectedVehicleToSunVectorNormalized.dot(Eigen::Vector3d::UnitX( ) ); + accelerationPositiveXaxisOrientedPanels = - cosPanelInclinationPositiveXaxis * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() - * ( areas[7] * (( 1.0 - specularReflectivities[7] ) * expectedVehicleToSunVectorNormalized - + 2.0 / 3.0 * diffuseReflectivities[7] * Eigen::Vector3d::UnitY( ) ) - + areas[8] * (( 1.0 - specularReflectivities[8] ) * expectedVehicleToSunVectorNormalized - + ( 2.0 / 3.0 * diffuseReflectivities[8] + 2.0 * specularReflectivities[8] * cosPanelInclinationPositiveYaxis ) - * Eigen::Vector3d::UnitY( ) ) ); - - // Calculated acceleration generated by the panels whose normals are along -Y-axis. - double cosPanelInclinationNegativeYaxis = expectedVehicleToSunVectorNormalized.dot(- Eigen::Vector3d::UnitY() ); - Eigen::Vector3d accelerationNegativeYaxisOrientedPanels = - cosPanelInclinationNegativeYaxis + * ( areas[5] * (( 1.0 - specularReflectivities[5] ) * expectedVehicleToSunVectorNormalized + + 2.0 / 3.0 * diffuseReflectivities[5] * Eigen::Vector3d::UnitX( ) ) + + areas[6] * (( 1.0 - specularReflectivities[6] ) * expectedVehicleToSunVectorNormalized + + ( 2.0 / 3.0 * diffuseReflectivities[6] + 2.0 * cosPanelInclinationPositiveXaxis * specularReflectivities[6] ) + * Eigen::Vector3d::UnitX( ) ) ); + + + // Calculated acceleration generated by the panels whose normals are along -X-axis. + double cosPanelInclinationNegativeXaxis = expectedVehicleToSunVectorNormalized.dot(- Eigen::Vector3d::UnitX() ); + accelerationNegativeXaxisOrientedPanels = - cosPanelInclinationNegativeXaxis * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() - * ( areas[9] * (( 1.0 - specularReflectivities[9] ) * expectedVehicleToSunVectorNormalized - + 2.0 / 3.0 * diffuseReflectivities[9] * - Eigen::Vector3d::UnitY( ) ) - + areas[10] * (( 1.0 - specularReflectivities[10] ) * expectedVehicleToSunVectorNormalized - + ( 2.0 / 3.0 * diffuseReflectivities[10] + 2.0 * specularReflectivities[10] * cosPanelInclinationNegativeYaxis ) - * - Eigen::Vector3d::UnitY( ) ) ); + * ( areas[3] * (( 1.0 - specularReflectivities[3] ) * expectedVehicleToSunVectorNormalized + + 2.0 / 3.0 * diffuseReflectivities[3] * - Eigen::Vector3d::UnitX( ) ) + + areas[4] * (( 1.0 - specularReflectivities[4] ) * expectedVehicleToSunVectorNormalized + + ( 2.0 / 3.0 * diffuseReflectivities[4] + 2.0 * specularReflectivities[4] * cosPanelInclinationNegativeXaxis ) + * - Eigen::Vector3d::UnitX( ) ) ); + + // Calculated acceleration generated by the panels whose normals are along +Y-axis. + double cosPanelInclinationPositiveYaxis = expectedVehicleToSunVectorNormalized.dot(Eigen::Vector3d::UnitY() ); + accelerationPositiveYaxisOrientedPanels = - cosPanelInclinationPositiveYaxis + * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() + * ( areas[7] * (( 1.0 - specularReflectivities[7] ) * expectedVehicleToSunVectorNormalized + + 2.0 / 3.0 * diffuseReflectivities[7] * Eigen::Vector3d::UnitY( ) ) + + areas[8] * (( 1.0 - specularReflectivities[8] ) * expectedVehicleToSunVectorNormalized + + ( 2.0 / 3.0 * diffuseReflectivities[8] + 2.0 * specularReflectivities[8] * cosPanelInclinationPositiveYaxis ) + * Eigen::Vector3d::UnitY( ) ) ); + + // Calculated acceleration generated by the panels whose normals are along -Y-axis. + double cosPanelInclinationNegativeYaxis = expectedVehicleToSunVectorNormalized.dot(- Eigen::Vector3d::UnitY() ); + accelerationNegativeYaxisOrientedPanels = - cosPanelInclinationNegativeYaxis + * radiationPressure / bodies.at( "Vehicle" )->getBodyMass() + * ( areas[9] * (( 1.0 - specularReflectivities[9] ) * expectedVehicleToSunVectorNormalized + + 2.0 / 3.0 * diffuseReflectivities[9] * - Eigen::Vector3d::UnitY( ) ) + + areas[10] * (( 1.0 - specularReflectivities[10] ) * expectedVehicleToSunVectorNormalized + + ( 2.0 / 3.0 * diffuseReflectivities[10] + 2.0 * specularReflectivities[10] * cosPanelInclinationNegativeYaxis ) + * - Eigen::Vector3d::UnitY( ) ) ); + } // Calculated acceleration generated by the panels whose normals are along +Z-axis. double cosPanelInclinationPositiveZaxis = expectedVehicleToSunVectorNormalized.dot(Eigen::Vector3d::UnitZ() ); diff --git a/tests/src/astro/gravitation/unitTestGravityFieldVariations.cpp b/tests/src/astro/gravitation/unitTestGravityFieldVariations.cpp index e0f887a61e..2a38dfe0d9 100644 --- a/tests/src/astro/gravitation/unitTestGravityFieldVariations.cpp +++ b/tests/src/astro/gravitation/unitTestGravityFieldVariations.cpp @@ -315,6 +315,135 @@ BOOST_AUTO_TEST_CASE( testGravityFieldVariations ) } } +std::shared_ptr< BasicSolidBodyTideGravityFieldVariations > getBasicGravityFieldVariation( ) +{ + // Define bodies raising rides. + std::vector< std::string > deformingBodies; + deformingBodies.push_back( "Io" ); + deformingBodies.push_back( "Europa" ); + + // Retrieve required data of bodies raising tides. + std::vector< std::function< Eigen::Vector6d( const double ) > > + deformingBodyStateFunctions; + std::vector< std::function< double( ) > > deformingBodyMasses; + for( unsigned int i = 0; i < deformingBodies.size( ); i++ ) + { + deformingBodyStateFunctions.push_back( + std::bind( &getBodyCartesianStateAtEpoch, deformingBodies.at( i ), "SSB", "J2000", "None", std::placeholders::_1 ) ); + deformingBodyMasses.push_back( std::bind( &getBodyGravitationalParameter, deformingBodies.at( i ) ) ); + } + + // Define Love numbers ( constant for degree 2 only) + std::vector< std::complex< double > > degreeTwoLoveNumber = + { std::complex< double >( 0.1, 0.0 ), std::complex< double >( 0.2, 0.0 ), std::complex< double >( 0.3, 0.0 ) }; + std::map< int, std::vector< std::complex< double > > > loveNumbers; + loveNumbers[ 2 ] = degreeTwoLoveNumber; + + // Set up gravity field variation of Jupiter due to Galilean moons. + return std::make_shared< BasicSolidBodyTideGravityFieldVariations >( + std::bind( &getBodyCartesianStateAtEpoch, "Jupiter", "SSB", "J2000", "None", std::placeholders::_1 ), + std::bind( &computeRotationQuaternionBetweenFrames, "J2000", "IAU_Jupiter", std::placeholders::_1 ), + deformingBodyStateFunctions, getAverageRadius( "Jupiter" ), + std::bind( &getBodyGravitationalParameter, "Jupiter" ), + deformingBodyMasses, loveNumbers, deformingBodies ); +} + +std::shared_ptr< ModeCoupledSolidBodyTideGravityFieldVariations > getModeCoupledGravityFieldVariation( + const int index ) +{ + // Define bodies raising rides. + std::vector< std::string > deformingBodies; + deformingBodies.push_back( "Io" ); + deformingBodies.push_back( "Europa" ); + + // Retrieve required data of bodies raising tides. + std::vector< std::function< Eigen::Vector6d( const double ) > > + deformingBodyStateFunctions; + std::vector< std::function< double( ) > > deformingBodyMasses; + for( unsigned int i = 0; i < deformingBodies.size( ); i++ ) + { + deformingBodyStateFunctions.push_back( + std::bind( &getBodyCartesianStateAtEpoch, deformingBodies.at( i ), "SSB", "J2000", "None", std::placeholders::_1 ) ); + deformingBodyMasses.push_back( std::bind( &getBodyGravitationalParameter, deformingBodies.at( i ) ) ); + } + + // Define Love numbers ( constant for degree 2 only) + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > loveNumbers; + if( index == 0 ) + { + loveNumbers[ std::make_pair( 2, 0 ) ][ std::make_pair( 2, 0 ) ] = 0.1; + loveNumbers[ std::make_pair( 2, 1 ) ][ std::make_pair( 2, 1 ) ] = 0.2; + loveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 2, 2 ) ] = 0.3; + } + else if( index == 1 ) + { + loveNumbers[ std::make_pair( 2, 0 ) ][ std::make_pair( 2, 2 ) ] = 0.1; + loveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 2, 1 ) ] = 0.3; + loveNumbers[ std::make_pair( 2, 1 ) ][ std::make_pair( 2, 0 ) ] = 0.2; + } + else if( index == 2 ) + { + loveNumbers[ std::make_pair( 2, 0 ) ][ std::make_pair( 3, 3 ) ] = 0.1; + loveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 3, 2 ) ] = 0.3; + loveNumbers[ std::make_pair( 2, 1 ) ][ std::make_pair( 3, 1 ) ] = 0.2; + } + + // Set up gravity field variation of Jupiter due to Galilean moons. + return std::make_shared< ModeCoupledSolidBodyTideGravityFieldVariations >( + std::bind( &getBodyCartesianStateAtEpoch, "Jupiter", "SSB", "J2000", "None", std::placeholders::_1 ), + std::bind( &computeRotationQuaternionBetweenFrames, "J2000", "IAU_Jupiter", std::placeholders::_1 ), + deformingBodyStateFunctions, getAverageRadius( "Jupiter" ), + std::bind( &getBodyGravitationalParameter, "Jupiter" ), + deformingBodyMasses, loveNumbers, deformingBodies ); +} + +BOOST_AUTO_TEST_CASE( testModeCoupledGravityFieldVariations ) +{ + std::cout< modeCoupledControlVariationModel = + getModeCoupledGravityFieldVariation( 0 ); + std::shared_ptr< ModeCoupledSolidBodyTideGravityFieldVariations > modeCoupledOrderOffsetVariationModel = + getModeCoupledGravityFieldVariation( 1 ); + std::shared_ptr< ModeCoupledSolidBodyTideGravityFieldVariations > modeCoupledDegreeOrderOffsetVariationModel = + getModeCoupledGravityFieldVariation( 2 ); + std::shared_ptr< BasicSolidBodyTideGravityFieldVariations > basicVariationModel = + getBasicGravityFieldVariation( ); + + double testTime = 1.0E7; + std::pair< Eigen::MatrixXd, Eigen::MatrixXd > modeCoupledControlVariations = + modeCoupledControlVariationModel->calculateBasicSphericalHarmonicsCorrections( testTime ); + std::pair< Eigen::MatrixXd, Eigen::MatrixXd > basicVariations = + basicVariationModel->calculateBasicSphericalHarmonicsCorrections( testTime ); + std::pair< Eigen::MatrixXd, Eigen::MatrixXd > modeCoupledOrderOffsetVariations = + modeCoupledOrderOffsetVariationModel->calculateBasicSphericalHarmonicsCorrections( testTime ); + std::pair< Eigen::MatrixXd, Eigen::MatrixXd > modeCoupledDegreeOrderOffsetVariations = + modeCoupledDegreeOrderOffsetVariationModel->calculateBasicSphericalHarmonicsCorrections( testTime ); + + std::cout<& cosineShAmplitudesCosineTime, diff --git a/tests/src/astro/observation_models/CMakeLists.txt b/tests/src/astro/observation_models/CMakeLists.txt index d394698260..f447ac2bdb 100644 --- a/tests/src/astro/observation_models/CMakeLists.txt +++ b/tests/src/astro/observation_models/CMakeLists.txt @@ -9,11 +9,7 @@ TUDAT_ADD_TEST_CASE(LightTimeSolution PRIVATE_LINKS - tudat_observation_models - tudat_ephemerides - tudat_spice_interface - ) - + ${Tudat_ESTIMATION_LIBRARIES}) TUDAT_ADD_TEST_CASE(AngularPositionModel PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) @@ -60,14 +56,14 @@ TUDAT_ADD_TEST_CASE(TimeBias PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) TUDAT_ADD_TEST_CASE(AtmosphereCorrection PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -TUDAT_ADD_TEST_CASE(SolarCoronaCorrection PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) +TUDAT_ADD_TEST_CASE(SolarPlasmaCorrection PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) TUDAT_ADD_TEST_CASE(ObservationRoundingErrors PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -TUDAT_ADD_TEST_CASE(ObservationDependentVariables PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) +#TUDAT_ADD_TEST_CASE(ObservationDependentVariables PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -TUDAT_ADD_TEST_CASE(DsnOdfGrail PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) +#TUDAT_ADD_TEST_CASE(DsnOdfGrail PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -TUDAT_ADD_TEST_CASE(DsnOdfGrailStatistics PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) +#TUDAT_ADD_TEST_CASE(DsnOdfGrailStatistics PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -TUDAT_ADD_TEST_CASE(DsnNWayDopplerObservationModel PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) \ No newline at end of file +#TUDAT_ADD_TEST_CASE(DsnNWayDopplerObservationModel PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) diff --git a/tests/src/astro/observation_models/unitTestNWayRangeObservationModel.cpp b/tests/src/astro/observation_models/unitTestNWayRangeObservationModel.cpp index 3b64524200..556d63d4ab 100644 --- a/tests/src/astro/observation_models/unitTestNWayRangeObservationModel.cpp +++ b/tests/src/astro/observation_models/unitTestNWayRangeObservationModel.cpp @@ -511,9 +511,98 @@ BOOST_AUTO_TEST_CASE( testNWayRangeModel ) } } } +} + + +BOOST_AUTO_TEST_CASE( testTwoWayRangeModelTimeScaleBias ) +{ + spice_interface::loadStandardSpiceKernels( ); + + // Define bodies to use. + std::vector< std::string > bodiesToCreate; + bodiesToCreate.push_back( "Earth" ); + bodiesToCreate.push_back( "Mars" ); + + // Specify initial time + double initialEphemerisTime = 0.0; + double finalEphemerisTime = initialEphemerisTime + 7.0 * 86400.0; + double maximumTimeStep = 3600.0; + double buffer = 10.0 * maximumTimeStep; + + // Create bodies settings needed in simulation + BodyListSettings defaultBodySettings = + getDefaultBodySettings( + bodiesToCreate ); + + // Create bodies + SystemOfBodies bodies = createSystemOfBodies( defaultBodySettings ); + + // Create ground stations + std::pair< std::string, std::string > earthStationStation = std::pair< std::string, std::string >( "Earth", "EarthStation" ); + std::pair< std::string, std::string > earthStationStation2 = std::pair< std::string, std::string >( "Earth", "EarthStation2" ); + std::pair< std::string, std::string > mslStation = std::pair< std::string, std::string >( "Mars", "MarsStation" ); + createGroundStation( bodies.at( "Mars" ), "MarsStation", ( Eigen::Vector3d( ) << 100.0, 0.5, 2.1 ).finished( ), + coordinate_conversions::geodetic_position ); + createGroundStation( bodies.at( "Earth" ), "EarthStation", ( Eigen::Vector3d( ) << 1.0, 0.1, -1.4 ).finished( ), + coordinate_conversions::geodetic_position ); + createGroundStation( bodies.at( "Earth" ), "EarthStation2", ( Eigen::Vector3d( ) << -30.0, 1.2, 2.1 ).finished( ), + coordinate_conversions::geodetic_position ); + std::vector< std::pair< std::string, std::string > > groundStations; + groundStations.push_back( earthStationStation ); + groundStations.push_back( earthStationStation2 ); + groundStations.push_back( mslStation ); + + // Define list of observation times for which to check model + std::vector< double > observationTimes; + observationTimes.push_back( 1.0E8 ); + observationTimes.push_back( 2.0E8 ); + observationTimes.push_back( 3.0E8 ); + + // Define link ends for observations. + for( unsigned int observationTimeNumber = 0; observationTimeNumber < observationTimes.size( ); observationTimeNumber++ ) + { + // Define link ends for 2-way model and constituent one-way models + LinkEnds twoWayLinkEnds; + twoWayLinkEnds[ transmitter ] = std::make_pair< std::string, std::string >( "Earth" , "EarthStation" ); + twoWayLinkEnds[ reflector1 ] = std::make_pair< std::string, std::string >( "Mars" , "MarsStation" ); + twoWayLinkEnds[ receiver ] = std::make_pair< std::string, std::string >( "Earth" , "EarthStation2" ); + + // Create observation models + std::shared_ptr< NWayRangeObservationSettings > twoWayObservableSettings = std::make_shared< NWayRangeObservationSettings > + ( twoWayLinkEnds, std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ) ); + std::shared_ptr< ObservationModel< 1, double, double > > twoWayObservationModel = + ObservationModelCreator< 1, double, double >::createObservationModel( + twoWayObservableSettings, bodies ); + + std::vector< double > linkEndTimes; + std::vector< Eigen::Matrix< double, 6, 1 > > linkEndStates; + double unbiasedTwoWayRange = twoWayObservationModel->computeObservationsWithLinkEndData( + observationTimes.at( observationTimeNumber ), receiver, + linkEndTimes, linkEndStates )( 0 ); + + std::shared_ptr< ObservationBiasSettings > biasSettings = twoWayTimeScaleRangeBias( ); + std::shared_ptr< NWayRangeObservationSettings > twoWayObservableSettingsWithBias = std::make_shared< NWayRangeObservationSettings > + ( twoWayLinkEnds, std::vector< std::shared_ptr< LightTimeCorrectionSettings > >( ), biasSettings ); + std::shared_ptr< ObservationModel< 1, double, double > > twoWayObservationModelWithBias = + ObservationModelCreator< 1, double, double >::createObservationModel( + twoWayObservableSettingsWithBias, bodies ); + + double biasedTwoWayRange = twoWayObservationModelWithBias->computeObservationsWithLinkEndData( + observationTimes.at( observationTimeNumber ), receiver, + linkEndTimes, linkEndStates )( 0 ); + + std::cout< timeScaleConverter = earth_orientation::defaultTimeConverter; + double transmissionTimeDifference = timeScaleConverter->getCurrentTime< Time >( basic_astrodynamics::tdb_scale, basic_astrodynamics::utc_scale, linkEndTimes.at( 0 ), + bodies.at( "Earth" )->getGroundStationMap( ).at( "EarthStation" )->getNominalStationState( )->getNominalCartesianPosition( ) ) - Time( linkEndTimes.at( 0 ) ); + double receptionTimeDifference = timeScaleConverter->getCurrentTime< Time >( basic_astrodynamics::tdb_scale, basic_astrodynamics::utc_scale, linkEndTimes.at( 3 ), + bodies.at( "Earth" )->getGroundStationMap( ).at( "EarthStation2" )->getNominalStationState( )->getNominalCartesianPosition( ) ) - Time( linkEndTimes.at( 3 ) ); + BOOST_CHECK_SMALL( ( receptionTimeDifference -transmissionTimeDifference ) * physical_constants::SPEED_OF_LIGHT - (biasedTwoWayRange - unbiasedTwoWayRange), 1.0E-4 ); + } } + BOOST_AUTO_TEST_SUITE_END( ) } diff --git a/tests/src/astro/observation_models/unitTestSolarCoronaCorrection.cpp b/tests/src/astro/observation_models/unitTestSolarPlasmaCorrection.cpp similarity index 100% rename from tests/src/astro/observation_models/unitTestSolarCoronaCorrection.cpp rename to tests/src/astro/observation_models/unitTestSolarPlasmaCorrection.cpp diff --git a/tests/src/astro/orbit_determination/CMakeLists.txt b/tests/src/astro/orbit_determination/CMakeLists.txt index c99ee5ef6f..af19d4863a 100644 --- a/tests/src/astro/orbit_determination/CMakeLists.txt +++ b/tests/src/astro/orbit_determination/CMakeLists.txt @@ -131,11 +131,17 @@ TUDAT_ADD_TEST_CASE(NonSequentialStateEstimation ${Tudat_ESTIMATION_LIBRARIES} ) + TUDAT_ADD_TEST_CASE(ObservationsProcessing PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES} ) + TUDAT_ADD_TEST_CASE(CovariancePropagation + PRIVATE_LINKS + ${Tudat_ESTIMATION_LIBRARIES} + ) + if( TUDAT_BUILD_WITH_EXTENDED_PRECISION_PROPAGATION_TOOLS ) diff --git a/tests/src/astro/orbit_determination/acceleration_partials/unitTestSphericalHarmonicPartials.cpp b/tests/src/astro/orbit_determination/acceleration_partials/unitTestSphericalHarmonicPartials.cpp index a6dd32a1af..0b5fd84140 100644 --- a/tests/src/astro/orbit_determination/acceleration_partials/unitTestSphericalHarmonicPartials.cpp +++ b/tests/src/astro/orbit_determination/acceleration_partials/unitTestSphericalHarmonicPartials.cpp @@ -428,6 +428,19 @@ std::vector< std::shared_ptr< GravityFieldVariationSettings > > getEarthGravityF std::make_shared< BasicSolidBodyGravityFieldVariationSettings >( deformingBodies, loveNumbers ); gravityFieldVariations.push_back( singleGravityFieldVariation ); + // Define Love numbers ( constant for degree 2 only) + std::map< std::pair< int, int >, std::map< std::pair< int, int >, double > > modeCoupledLoveNumbers; + modeCoupledLoveNumbers[ std::make_pair( 2, 0 ) ][ std::make_pair( 2, 1 ) ] = 0.1; + modeCoupledLoveNumbers[ std::make_pair( 2, 0 ) ][ std::make_pair( 2, 2 ) ] = 0.3; + modeCoupledLoveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 2, 2 ) ] = 0.2; + modeCoupledLoveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 2, 1 ) ] = 0.2; + modeCoupledLoveNumbers[ std::make_pair( 2, 2 ) ][ std::make_pair( 3, 2 ) ] = 0.2; + modeCoupledLoveNumbers[ std::make_pair( 2, 1 ) ][ std::make_pair( 3, 1 ) ] = 0.2; + modeCoupledLoveNumbers[ std::make_pair( 3, 3 ) ][ std::make_pair( 3, 1 ) ] = 0.2; + + gravityFieldVariations.push_back( std::make_shared< ModeCoupledSolidBodyGravityFieldVariationSettings >( + deformingBodies, modeCoupledLoveNumbers ) ); + std::map cosineAmplitudes; cosineAmplitudes[ 1 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); cosineAmplitudes[ 2 ] = Eigen::Matrix< double, 3, 5 >::Zero( ); @@ -498,6 +511,7 @@ std::vector< std::shared_ptr< GravityFieldVariationSettings > > getEarthGravityF return gravityFieldVariations; } + BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartial ) { //Load spice kernels. @@ -647,6 +661,18 @@ BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartial ) parameterNames.push_back( std::make_shared< SingleDegreeVariableTidalLoveNumberEstimatableParameterSettings >( "Earth", 3, std::vector< int >{ 0, 3 }, "", true ) ); + + std::map< std::pair< int, int >, std::vector< std::pair< int, int > > > loveNumberIndices; + loveNumberIndices[{2,0}].push_back({2,1}); + loveNumberIndices[{2,0}].push_back({2,2}); + loveNumberIndices[{2,2}].push_back({2,2}); + loveNumberIndices[{2,2}].push_back({2,1}); + loveNumberIndices[{2,2}].push_back({3,2}); + loveNumberIndices[{2,1}].push_back({3,1}); + loveNumberIndices[{3,3}].push_back({3,1}); + + parameterNames.push_back( std::make_shared< ModeCoupledTidalLoveNumberEstimatableParameterSettings >( + "Earth", loveNumberIndices, std::vector({"Moon"}) ) ); std::map > > cosineBlockIndicesPerPower; cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 2, 0 ) ); cosineBlockIndicesPerPower[ 1 ].push_back( std::make_pair( 3, 2 ) ); @@ -856,6 +882,12 @@ BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartial ) vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( 4, 10.0 ), sphericalHarmonicFieldUpdate ); vectorParametersIterator++; + Eigen::MatrixXd partialWrtModeCoupledLoveNumbers = accelerationPartial->wrtParameter( + vectorParametersIterator->second ); + Eigen::MatrixXd testPartialWrtModeCoupledLoveNumbers = calculateAccelerationWrtParameterPartials( + vectorParametersIterator->second, gravitationalAcceleration, Eigen::VectorXd::Constant( + vectorParametersIterator->second->getParameterSize( ), 10.0 ), sphericalHarmonicFieldUpdate ); + vectorParametersIterator++; Eigen::MatrixXd partialWrtPolynomialVariations = accelerationPartial->wrtParameter( vectorParametersIterator->second ); @@ -901,6 +933,7 @@ BOOST_AUTO_TEST_CASE( testSphericalHarmonicAccelerationPartial ) TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtDegreeThreeLoveNumber, testPartialWrtDegreeThreeLoveNumber, 1.0E-6 ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtComplexDegreeThreeLoveNumberAtSeparateOrder, testPartialWrtComplexDegreeThreeLoveNumberAtSeparateOrder, 1.0E-6 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtModeCoupledLoveNumbers, testPartialWrtModeCoupledLoveNumbers, 1.0E-6 ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPolynomialVariations, testPartialWrtPolynomialVariations, 1.0E-12 ); TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtPeriodicVariations, testPartialWrtPeriodicVariations, 1.0E-12 ); diff --git a/tests/src/astro/orbit_determination/observation_partials/unitTestTimeBiasPartials.cpp b/tests/src/astro/orbit_determination/observation_partials/unitTestTimeBiasPartials.cpp index 3f87d6c900..955cf0b0f4 100644 --- a/tests/src/astro/orbit_determination/observation_partials/unitTestTimeBiasPartials.cpp +++ b/tests/src/astro/orbit_determination/observation_partials/unitTestTimeBiasPartials.cpp @@ -166,7 +166,7 @@ BOOST_AUTO_TEST_CASE( testTimeBiasPartials ) differencedTransmitterLinkEndsInverse[ transmitter2 ] = LinkEndId( "Vehicle", "" ); differencedTransmitterLinkEndsInverse[ transmitter ] = LinkEndId( "Vehicle2", "" ); - for( unsigned int observableCase = 9; observableCase < 10; observableCase++ ) + for( unsigned int observableCase = 0; observableCase < 11; observableCase++ ) { std::cout<<"OBSERVABLE CASE "< + +#include + +#include "tudat/basics/testMacros.h" + +#include "tudat/simulation/estimation_setup/orbitDeterminationTestCases.h" +#include "tudat/simulation/estimation_setup/podProcessing.h" +#include "tudat/astro/propagators/propagateCovariance.h" + +namespace tudat +{ +namespace unit_tests +{ +BOOST_AUTO_TEST_SUITE( test_covariance_propagation ) + +BOOST_AUTO_TEST_CASE( test_EstimationInputAndOutput ) +{ + const double startTime = 1.0E7; + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + for( int test = 0; test < 2; test++ ) + { + // Define bodies in simulation + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + bodyNames.push_back( "Moon" ); + bodyNames.push_back( "Mars" ); + + // Specify initial time + double initialEphemerisTime = startTime; + double finalEphemerisTime = initialEphemerisTime + 4.0 * 3600.0; + + // Create bodies needed in simulation + BodyListSettings bodySettings = + getDefaultBodySettings( bodyNames, "Earth", "ECLIPJ2000" ); + bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >( + "ECLIPJ2000", "IAU_Earth", + spice_interface::computeRotationQuaternionBetweenFrames( + "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ), + initialEphemerisTime, 2.0 * mathematical_constants::PI / + ( physical_constants::JULIAN_DAY ) ); + + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + bodies.createEmptyBody( "Vehicle" ); + bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 ); + + // Create aerodynamic coefficient interface settings. + double referenceArea = 4.0; + double aerodynamicCoefficient = 1.2; + std::shared_ptr< AerodynamicCoefficientSettings > aerodynamicCoefficientSettings = + std::make_shared< ConstantAerodynamicCoefficientSettings >( + referenceArea, aerodynamicCoefficient * ( Eigen::Vector3d( ) << 1.2, -0.1, -0.4 ).finished( ), + negative_aerodynamic_frame_coefficients ); + + // Create and set aerodynamic coefficnumberOfDaysOfDataients object + bodies.at( "Vehicle" )->setAerodynamicCoefficientInterface( + createAerodynamicCoefficientInterface( aerodynamicCoefficientSettings, "Vehicle", bodies ) ); + + // Create radiation pressure settings + double referenceAreaRadiation = 4.0; + double radiationPressureCoefficient = 1.2; + std::vector< std::string > occultingBodies; + occultingBodies.push_back( "Earth" ); + std::shared_ptr< RadiationPressureInterfaceSettings > asterixRadiationPressureSettings = + std::make_shared< CannonBallRadiationPressureInterfaceSettings >( + "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + // Create and set radiation pressure settings + bodies.at( "Vehicle" )->setRadiationPressureInterface( + "Sun", createRadiationPressureInterface( + asterixRadiationPressureSettings, "Vehicle", bodies ) ); + + // bodies.at( "Vehicle" )->setEphemeris( std::make_shared< TabulatedCartesianEphemeris< > >( + // std::shared_ptr< interpolators::OneDimensionalInterpolator + // < double, Eigen::Vector6d > >( ), "Earth", "ECLIPJ2000" ) ); + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 2, 2 ) ); + accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( + basic_astrodynamics::point_mass_gravity ) ); + accelerationsOfVehicle[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( + basic_astrodynamics::point_mass_gravity ) ); + accelerationsOfVehicle[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( + basic_astrodynamics::point_mass_gravity ) ); + accelerationsOfVehicle[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( + basic_astrodynamics::cannon_ball_radiation_pressure ) ); + accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( + basic_astrodynamics::aerodynamic ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::vector< std::string > centralBodies; + bodiesToIntegrate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + + // Set Keplerian elements for Asterix. + Eigen::Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 1.0E-5; + asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + + // Set (perturbed) initial state. + Eigen::Matrix< double, 6, 1 > systemInitialState = convertKeplerianToCartesianElements( + asterixInitialStateInKeplerianElements, earthGravitationalParameter ); + + // Create propagator settings + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + rungeKuttaFixedStepSettings( 400.0, CoefficientSets::rungeKuttaFehlberg78 ); + std::shared_ptr< TranslationalStatePropagatorSettings< double, double > > propagatorSettings = + std::make_shared< TranslationalStatePropagatorSettings< double, double > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, initialEphemerisTime, + integratorSettings, propagationTimeTerminationSettings( finalEphemerisTime ), cowell ); + + + // Define parameters. + LinkEnds linkEnds; + linkEnds[ observed_body ] = LinkEndId( "Vehicle", "" ); + + std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable; + linkEndsPerObservable[ position_observable ].push_back( linkEnds ); + + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( + std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >( + "Vehicle", systemInitialState, "Earth" ) ); + + if( test > 0 ) + { + parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", radiation_pressure_coefficient ) ); + parameterNames.push_back( std::make_shared< EstimatableParameterSettings >( "Vehicle", constant_drag_coefficient ) ); + } + + // Create parameters + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate = + createParametersToEstimate< double, double >( parameterNames, bodies ); + + printEstimatableParameterEntries( parametersToEstimate ); + + std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; + observationSettingsList.push_back( std::make_shared< ObservationModelSettings >( + position_observable, linkEnds ) ); + + // Create orbit determination object. + OrbitDeterminationManager< double, double > orbitDeterminationManager = + OrbitDeterminationManager< double, double >( + bodies, parametersToEstimate, observationSettingsList, propagatorSettings ); + + std::vector< double > baseTimeList; + double observationTimeStart = initialEphemerisTime + 100.0; + double observationInterval = 20.0; + double currentObservationTime = observationTimeStart; + while( currentObservationTime < finalEphemerisTime - 100.0 ) + { + baseTimeList.push_back( currentObservationTime ); + currentObservationTime += observationInterval; + } + + std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput = + getObservationSimulationSettings< double >( + linkEndsPerObservable, baseTimeList, observed_body ); + + // Simulate observations + std::shared_ptr< ObservationCollection< double, double > > simulatedObservations = + simulateObservations< double, double >( + measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies ); + + + + // Define estimation input + std::shared_ptr< CovarianceAnalysisInput< double, double > > estimationInput = + std::make_shared< CovarianceAnalysisInput< double, double > >( + simulatedObservations ); + + std::map< observation_models::ObservableType, double > weightPerObservable; + weightPerObservable[ position_observable ] = 1.0; + estimationInput->setConstantPerObservableWeightsMatrix( weightPerObservable ); + + // Perform estimation + std::shared_ptr< CovarianceAnalysisOutput< double > > estimationOutput = orbitDeterminationManager.computeCovariance( + estimationInput ); + auto stateHistory = std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( ) + )->getDynamicsSimulator( )->getEquationsOfMotionNumericalSolution( ); + auto stateTransitionMatrixHistory = + std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( ) + )->getStateTransitionMatrixSolution( ); + auto sensitivityMatrixHistory = + std::dynamic_pointer_cast< SingleArcVariationalEquationsSolver< > >( orbitDeterminationManager.getVariationalEquationsSolver( ) + )->getSensitivityMatrixSolution( ); + + std::map< double, Eigen::MatrixXd > propagatedCovariance; + propagateCovariance( + propagatedCovariance, + estimationOutput->getUnnormalizedCovarianceMatrix( ), + orbitDeterminationManager.getStateTransitionAndSensitivityMatrixInterface( ), + utilities::createVectorFromMapKeys( stateTransitionMatrixHistory ) ); + + std::map< double, Eigen::MatrixXd > rswPropagatedCovariance = convertCovarianceHistoryToFrame( + propagatedCovariance, + stateHistory, + reference_frames::global_reference_frame, + reference_frames::rsw_reference_frame ); + std::map< double, Eigen::MatrixXd > tnwPropagatedCovariance = convertCovarianceHistoryToFrame( + propagatedCovariance, + stateHistory, + reference_frames::global_reference_frame, + reference_frames::tnw_reference_frame ); + + for( auto it : propagatedCovariance ) + { + Eigen::MatrixXd currentStateTransition = stateTransitionMatrixHistory.at( it.first ); + if( test == 0 ) + { + Eigen::MatrixXd manualCovariance = currentStateTransition * estimationOutput->getUnnormalizedCovarianceMatrix( ) * currentStateTransition.transpose( ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( it.second, manualCovariance, 1.0E-10 ); + + Eigen::Matrix3d rotationToRsw = reference_frames::getInertialToRswSatelliteCenteredFrameRotationMatrix( stateHistory.at( it.first ) ); + Eigen::Matrix6d rotationToRswFull = Eigen::Matrix6d::Zero( ); + rotationToRswFull.block( 0, 0, 3, 3 ) = rotationToRsw; + rotationToRswFull.block( 3, 3, 3, 3 ) = rotationToRsw; + Eigen::MatrixXd manualRswCovariance = rotationToRswFull * it.second * rotationToRswFull.transpose( ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( rswPropagatedCovariance.at( it.first ), manualRswCovariance, 1.0E-9 ); + + Eigen::Matrix3d rotationToTnw = reference_frames::getInertialToTnwRotation( stateHistory.at( it.first ) ); + Eigen::Matrix6d rotationToTnwFull = Eigen::Matrix6d::Zero( ); + rotationToTnwFull.block( 0, 0, 3, 3 ) = rotationToTnw; + rotationToTnwFull.block( 3, 3, 3, 3 ) = rotationToTnw; + Eigen::MatrixXd manualTnwCovariance = rotationToTnwFull * it.second * rotationToTnwFull.transpose( ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( tnwPropagatedCovariance.at( it.first ), manualTnwCovariance, 1.0E-9 ); + + Eigen::VectorXd rswFormalError = rswPropagatedCovariance.at( it.first ).diagonal( ).cwiseSqrt( ); + Eigen::VectorXd tnwFormalError = tnwPropagatedCovariance.at( it.first ).diagonal( ).cwiseSqrt( ); + + // Weak check on radial and along-track errors. + BOOST_CHECK_SMALL( rswFormalError( 0 ) / rswFormalError( 1 ), 0.6 ); + BOOST_CHECK_SMALL( rswFormalError( 4 ) / rswFormalError( 3 ), 0.6 ); + + // Check if in-plane RSW and TNW are similar + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 0 ), tnwFormalError( 1 ), 1.0E-3 ); + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 1 ), tnwFormalError( 0 ), 1.0E-3 ); + + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 3 ), tnwFormalError( 4 ), 1.0E-3 ); + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 4 ), tnwFormalError( 3 ), 1.0E-3 ); + + // Check if cross-plane RSW and TNW are equal + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 2 ), tnwFormalError( 2 ), std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( rswFormalError( 5 ), tnwFormalError( 5 ), std::numeric_limits< double >::epsilon( ) ); + } + else + { + Eigen::MatrixXd currentSensitivity = sensitivityMatrixHistory.at( it.first ); + Eigen::MatrixXd fullStateTransition = Eigen::MatrixXd::Zero( 6, 8 ); + fullStateTransition.block( 0, 0, 6, 6 ) = currentStateTransition; + fullStateTransition.block( 0, 6, 6, 2 ) = currentSensitivity; + Eigen::MatrixXd manualCovariance = fullStateTransition * estimationOutput->getUnnormalizedCovarianceMatrix( ) * fullStateTransition.transpose( ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( it.second, manualCovariance, 1.0E-12 ); + + } + } +// std::cout <<"formal error: "<< ( estimationOutput->getFormalErrorVector( ) ).transpose( ) << std::endl; + } + +} + +BOOST_AUTO_TEST_SUITE_END( ) + +} + +} + + + diff --git a/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp b/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp index da683e4689..05c7c7a8b6 100644 --- a/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp +++ b/tests/src/astro/reference_frames/unitTestReferenceFrameTransformations.cpp @@ -37,7 +37,8 @@ namespace tudat namespace unit_tests { -using namespace unit_conversions; +using namespace tudat::unit_conversions; +using namespace tudat::reference_frames; BOOST_AUTO_TEST_SUITE( test_reference_frame_transformations ) @@ -815,7 +816,7 @@ BOOST_AUTO_TEST_CASE( testVelocityBasedtnwFrameTransformations ) // Compute angle between positive T axis and negative y axis double angle = std::atan2( ( vehicleStateCartesian( 3 ) - centralBodyStateCartesian( 3 ) ), - ( vehicleStateCartesian( 4 ) - centralBodyStateCartesian( 4 ) ) ); + ( vehicleStateCartesian( 4 ) - centralBodyStateCartesian( 4 ) ) ); // Declare the expected thrust vector in the inertial (I) reference frame. Eigen::Vector3d expectedThrustVector; @@ -884,7 +885,44 @@ BOOST_AUTO_TEST_CASE( testVelocityBasedtnwFrameTransformations ) } } - +BOOST_AUTO_TEST_CASE( testAutomaticConversionFunctions ) +{ + Eigen::Vector6d inertialCartesianState; + inertialCartesianState << 4.0E6, 5.0E6, -300.0, -2.0E3, 6.0E3, 20.0; + + Eigen::Matrix3d manualInertialToRsw = getInertialToRswSatelliteCenteredFrameRotationMatrix( + inertialCartesianState ); + Eigen::Matrix3d automaticInertialToRsw = getRotationBetweenSatelliteFrames( + inertialCartesianState, global_reference_frame, rsw_reference_frame ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualInertialToRsw, automaticInertialToRsw, std::numeric_limits< double >::epsilon( ) ); + + Eigen::Matrix3d manualRswToInertial = getRswSatelliteCenteredToInertialFrameRotationMatrix( + inertialCartesianState ); + Eigen::Matrix3d automaticRswToInertial = getRotationBetweenSatelliteFrames( + inertialCartesianState, rsw_reference_frame, global_reference_frame ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualRswToInertial, automaticRswToInertial, std::numeric_limits< double >::epsilon( ) ); + + Eigen::Matrix3d manualInertialToTnw = getInertialToTnwRotation( + inertialCartesianState ); + Eigen::Matrix3d automaticInertialToTnw = getRotationBetweenSatelliteFrames( + inertialCartesianState, global_reference_frame, tnw_reference_frame ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualInertialToTnw, automaticInertialToTnw, std::numeric_limits< double >::epsilon( ) ); + + Eigen::Matrix3d manualTnwToInertial = getTnwToInertialRotation( + inertialCartesianState ); + Eigen::Matrix3d automaticTnwToInertial = getRotationBetweenSatelliteFrames( + inertialCartesianState, tnw_reference_frame, global_reference_frame ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( manualTnwToInertial, automaticTnwToInertial, std::numeric_limits< double >::epsilon( ) ); + + + Eigen::Matrix3d automaticTnwToRsw = getRotationBetweenSatelliteFrames( + inertialCartesianState, tnw_reference_frame, rsw_reference_frame ); + Eigen::Matrix3d automaticRswToTnw = getRotationBetweenSatelliteFrames( + inertialCartesianState, rsw_reference_frame, tnw_reference_frame ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( automaticTnwToRsw, ( automaticRswToTnw.transpose( ) ), ( 5.0 * std::numeric_limits< double >::epsilon( ) ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( automaticTnwToRsw, Eigen::Matrix3d( manualTnwToInertial * manualInertialToRsw ), std::numeric_limits< double >::epsilon( ) ); +} BOOST_AUTO_TEST_SUITE_END( ) diff --git a/tests/src/astro/relativity/CMakeLists.txt b/tests/src/astro/relativity/CMakeLists.txt index 3595bb16c9..8b508cb966 100644 --- a/tests/src/astro/relativity/CMakeLists.txt +++ b/tests/src/astro/relativity/CMakeLists.txt @@ -12,9 +12,5 @@ TUDAT_ADD_TEST_CASE(RelativisticAccelerationCorrection PRIVATE_LINKS ${Tudat_PRO TUDAT_ADD_TEST_CASE(EihAcceleration PRIVATE_LINKS ${Tudat_PROPAGATION_LIBRARIES}) +TUDAT_ADD_TEST_CASE(ShapiroTimeDelay PRIVATE_LINKS ${Tudat_ESTIMATION_LIBRARIES}) -if( TUDAT_BUILD_WITH_ESTIMATION_TOOLS ) - - TUDAT_ADD_TEST_CASE(ShapiroTimeDelay PRIVATE_LINKS tudat_relativity tudat_gravitation tudat_observation_models tudat_ephemerides) - -endif( ) diff --git a/version b/version index b50403a1e9..3fb62ffef6 100644 --- a/version +++ b/version @@ -1 +1 @@ -2.13.0.dev11 +2.13.0.dev17