From 13ce1f7f40e75fc35d08552a8097919720282cb0 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 25 Oct 2017 10:31:49 +0200 Subject: [PATCH 01/10] Working on Bulirsch-Stoer integrator --- .../NumericalIntegrators/CMakeLists.txt | 8 + .../numericalIntegratorTestFunctions.h | 9 + ...ulirschStoerVariableStepSizeIntegrator.cpp | 280 +++++++++++++ ...bulirschStoerRationalFunctionSequences.cpp | 65 +++ .../bulirschStoerRationalFunctionSequences.h | 71 ++++ .../bulirschStoerVariableStepsizeIntegrator.h | 378 ++++++++++++++++++ 6 files changed, 811 insertions(+) create mode 100644 Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.cpp create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h diff --git a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt index 3ac61a1d87..992af5188e 100755 --- a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt +++ b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt @@ -13,6 +13,7 @@ set(NUMERICALINTEGRATORS_SOURCES "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/rungeKuttaCoefficients.cpp" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/numericalIntegrator.cpp" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/UnitTests/numericalIntegratorTests.cpp" + "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerRationalFunctionSequences.cpp" ) # Add header files. @@ -20,6 +21,8 @@ set(NUMERICALINTEGRATORS_HEADERS "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h" "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/rungeKuttaCoefficients.h" "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/UnitTests/burdenAndFairesNumericalIntegratorTest.h" + "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" + "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/euler.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/numericalIntegrator.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/reinitializableNumericalIntegrator.h" @@ -69,3 +72,8 @@ target_link_libraries(test_RungeKuttaFehlberg78Integrator tudat_numerical_integr add_executable(test_RungeKutta87DormandPrinceIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/UnitTests/unitTestRungeKutta87DormandPrinceIntegrator.cpp") setup_custom_test_program(test_RungeKutta87DormandPrinceIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators") target_link_libraries(test_RungeKutta87DormandPrinceIntegrator tudat_numerical_integrators tudat_input_output ${Boost_LIBRARIES}) + +add_executable(test_BulirschStoerVariableStepSizeIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp") + setup_custom_test_program(test_BulirschStoerVariableStepSizeIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators") +target_link_libraries(test_BulirschStoerVariableStepSizeIntegrator tudat_numerical_integrators tudat_input_output ${Boost_LIBRARIES}) + diff --git a/Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h b/Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h index 58d89340be..4da5e2e46c 100644 --- a/Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h +++ b/Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h @@ -37,6 +37,15 @@ static inline Eigen::VectorXd computeZeroStateDerivative( const double time, return Eigen::VectorXd::Zero( state.rows( ) ); } +static inline Eigen::VectorXd computeConstantStateDerivative( const double time, + const Eigen::VectorXd& state ) +{ + Eigen::VectorXd stateDerivative = Eigen::VectorXd::Zero( state.rows( ) ); + stateDerivative( 0 ) = 1.0; + //std::cout<<"State der: "< + +#include + +#include + +#include +#include + +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" + +namespace tudat +{ +namespace unit_tests +{ + +const double TUDAT_MACHINE_PRECISION = std::numeric_limits< double >::epsilon( ); + +BOOST_AUTO_TEST_SUITE( test_bulirsch_stoer_variable_step_size_integrator ) + +using numerical_integrators::BulirschStoerVariableStepSizeIntegratorXd; +using numerical_integrators::BulirschStoerRationalFunctionSequences; + +//! Test the validity of the BurlischStoerVariableStepSize integrator. +/*! + * Tests the validity of the RungeKuttaVariableStepSize integrator. + * \param sequence The rational function sequence used for the Bulirsch-Stoer integrator being + * tested. + * \param stateDerivativeFunction Function pointer to the state derivative function. + * \param intervalStart The start of the integration interval. + * \param intervalEnd The end of the integration interval. + * \param stepSize The step size to take. + * \param initialState The initial state. + * \param expectedState Expected final state. + * \param tolerance Tolerance when comparing. + * \return True if actual final state equals the expected final state within the specified + * tolerance. + */ +bool testValidityOfBulirschStoerVariableStepSizeIntegrator( + const BulirschStoerRationalFunctionSequences sequence, + const BulirschStoerVariableStepSizeIntegratorXd::StateDerivativeFunction& + stateDerivativeFunction, + const double intervalStart, const double intervalEnd, const double stepSize, + const Eigen::VectorXd& initialState, const Eigen::VectorXd expectedState, + const double tolerance ) +{ + // Create forward BulirschStoerVariableStepSize integrator. + { + std::cout<<"T1"< + 10.0 * TUDAT_MACHINE_PRECISION ) + { + return false; + } + + // Compute differences between computed and expected results and generate + // cerr statement if test fails. + if ( !expectedState.isApprox( finalState, tolerance ) ) + { + return false; + } + } + std::cout<<"T1"< TUDAT_MACHINE_PRECISION ) + { + return false; + } + + std::cout<<"T2"< + 10.0 * TUDAT_MACHINE_PRECISION ) + { + return false; + } + + // Compute differences between computed and expected results and generate + // cerr statement if test fails. + if ( !expectedState.isApprox( finalState, tolerance ) ) + { + return false; + } + + integrator.performIntegrationStep( stepSize ); + if ( !integrator.rollbackToPreviousState( ) ) + { + return false; + } + + // No need to check machine precision, because this interval is stored exact. + if ( std::fabs( integrator.getCurrentIndependentVariable( ) - intervalEnd ) / intervalEnd > + 10.0 * TUDAT_MACHINE_PRECISION ) + { + return false; + } + + // This result should be exactly the same. + if ( integrator.getCurrentState( ) != finalState ) + { + return false; + } + + if ( integrator.rollbackToPreviousState( ) ) + { + return false; + } + } + + return true; +} + +//! Test different types of states and state derivatives. +/*! + * Tests if different types of states and state derivatives work. If something + * is broken, then a compile time error will be generated. + * \return Unconditionally true + */ +bool testDifferentStateAndStateDerivativeTypes( ) +{ + using tudat::unit_tests::numerical_integrator_test_functions::computeZeroStateDerivative; + tudat::numerical_integrators::BulirschStoerVariableStepSizeIntegrator + < double, Eigen::Vector3d, Eigen::VectorXd > integrator( + BulirschStoerRationalFunctionSequences( ), &computeZeroStateDerivative, + 0.0, Eigen::Vector3d::Zero( ) ); + + integrator.integrateTo( 0.0, 0.1 ); + + // No need to test anything, this is just to check compile time errors. + return true; +} + +//! Test the validity of the BulirschStoerVariableStepSize integrator. +/*! + * Tests the validity of the BulirschStoerVariableStepSize integrator. + * \param sequence Bulirsch-Stoer, variable-step size rational function sequence. + * \return True if actual final state equals the expected final state within the specified + * tolerance. + */ +bool testValidityOfBulirschStoerVariableStepSizeIntegrator( + const BulirschStoerRationalFunctionSequences& sequence ) +{ + // Test result initialised to false. + bool testBulirschStoerVariableStepSizeIsOk = true; + + using namespace tudat::unit_tests; +// std::map< BenchmarkFunctions, BenchmarkFunction >& benchmarkFunctions = +// getBenchmarkFunctions( ); + +// // Case 1: test with x_dot = 0, which results in x_f = x_0. +// { +// testBulirschStoerVariableStepSizeIsOk +// &= testValidityOfBulirschStoerVariableStepSizeIntegrator( +// sequence, +// &unit_tests::numerical_integrator_test_functions::computeZeroStateDerivative, +// 0.0, +// 100.0, 0.2, +// Eigen::Vector3d::Zero( ), +// Eigen::Vector3d::Zero( ), +// TUDAT_MACHINE_PRECISION ); +// } + + // Case 2: test with x_dot = 1, which results in x_f = x_0 + t_f. + { + std::cout<<"test 2"< + +#include + +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" + +namespace tudat +{ + +namespace numerical_integrators +{ + +//! Get rational function sequence. +const BulirschStoerRationalFunctionSequences& +BulirschStoerRationalFunctionSequences::get( RationalFunctionSequences sequence, + const unsigned int lengthOfSequence ) +{ + static BulirschStoerRationalFunctionSequences bulirschStoerSequence_, deufelhardSequence_; + + switch ( sequence ) + { + case bulirschStoer: + using namespace boost::assign; + bulirschStoerSequence_.rationalFunctionSequence += 2, 4, 6; + for ( unsigned int i = 3; i < lengthOfSequence; i++ ) + { + bulirschStoerSequence_.rationalFunctionSequence.push_back( + 2 * bulirschStoerSequence_.rationalFunctionSequence.at( i - 2 ) ); + } + return bulirschStoerSequence_; + + case deufelhard: + for ( unsigned int i = 0; i < lengthOfSequence; i++ ) + { + deufelhardSequence_.rationalFunctionSequence.push_back( 2 * ( i + 1 ) ); + } + return deufelhardSequence_; + + default: // The default case will never occur because sequence is an enum + throw BulirschStoerRationalFunctionSequences( ); + } +} + +} // namespace integrators +} // namespace tudat diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h new file mode 100644 index 0000000000..fa8e4cb849 --- /dev/null +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h @@ -0,0 +1,71 @@ +/* Copyright (c) 2010-2017, Delft University of Technology + * All rigths reserved + * + * This file is part of the Tudat. Redistribution and use in source and + * binary forms, with or without modification, are permitted exclusively + * under the terms of the Modified BSD license. You should have received + * a copy of the license with this file. If not, please or visit: + * http://tudat.tudelft.nl/LICENSE. + * + */ + + +#ifndef TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H +#define TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H + +#include + +namespace tudat +{ + +namespace numerical_integrators +{ + +//! Struct that defines the rational function sequence for a Bulirsch-Stoer integrator. +/*! + * Struct that defines the rational function sequence for a Bulirsch-Stoer integrator. + */ +struct BulirschStoerRationalFunctionSequences +{ + + //! Default constructor. + /*! + * Default constructor that initializes without setting the rational function sequence. + */ + BulirschStoerRationalFunctionSequences( ) { } + + //! Constructor. + /*! + * Constructor that sets the rational function sequence. + * \param sequence Rational function sequence. + */ + BulirschStoerRationalFunctionSequences( const std::vector< unsigned int > sequence ) : + rationalFunctionSequence( sequence ) { } + + //! Rational function sequence. + /*! + * Rational function sequence. + */ + std::vector< unsigned int > rationalFunctionSequence; + + //! Enum of predefined rational function sequences. + enum RationalFunctionSequences + { + bulirschStoer, + deufelhard + }; + + //! Get rational function sequence. + /*! + * Returns requested rational function sequence. + * \param rationalFunctionSequence The rational function sequence. + * \return The requested rational function sequence. + */ + static const BulirschStoerRationalFunctionSequences& get( + RationalFunctionSequences sequence, const unsigned int lengthOfSequence = 100 ); +}; + +} // namespace integrators +} // namespace tudat + +#endif // TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h new file mode 100644 index 0000000000..75ed7487fa --- /dev/null +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -0,0 +1,378 @@ +/* Copyright (c) 2010-2012 Delft University of Technology. + * + * This software is protected by national and international copyright. + * Any unauthorized use, reproduction or modification is unlawful and + * will be prosecuted. Commercial and non-private application of the + * software in any form is strictly prohibited unless otherwise granted + * by the authors. + * + * The code is provided without any warranty; without even the implied + * warranty of merchantibility or fitness for a particular purpose. + * + * Changelog + * YYMMDD Author Comment + * 120316 K. Kumar File created. + * + * References + * Press W.H., et al. Numerical Recipes in C++: The Art of + * Scientific Computing. Cambridge University Press, February 2002. + * + */ + +#ifndef TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H +#define TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H + +#include + +#include + +#include +#include + +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" + +namespace tudat +{ +namespace numerical_integrators +{ + +//! Class that implements the Bulirsch-Stoer variable stepsize integrator. +/*! + * Class that implements the Bulirsch-Stoer variable step size integrator. + * \tparam StateType The type of the state. This type should be an Eigen::Matrix derived type. + * \tparam StateDerivativeType The type of the state derivative. This type should be an + * Eigen::Matrix derived type. + * \tparam IndependentVariableType The type of the independent variable. This type should be + * either a float or double. + * \sa NumericalIntegrator. + */ +template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, + typename StateDerivativeType = Eigen::VectorXd > +class BulirschStoerVariableStepSizeIntegrator : + public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > +{ +public: + + //! Typedef of the base class. + /*! + * Typedef of the base class with all template parameters filled in. + */ + typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; + + //! Typedef to the state derivative function. + /*! + * Typedef to the state derivative function inherited from the base class. + * \sa NumericalIntegrator::StateDerivativeFunction. + */ + typedef typename Base::StateDerivativeFunction StateDerivativeFunction; + + //! Default constructor. + /*! + * Default constructor, taking sequence, a state derivative function, initial conditions, + * minimum step size and relative error tolerance per item in the state vector as argument. + * \param sequence Rational function sequence used by algorithm. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, for each individual state + * vector element. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const BulirschStoerRationalFunctionSequences& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize, + const StateType& relativeErrorTolerance ) : + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ) + { } + + //! Default constructor. + /*! + * Default constructor, taking coefficients, a state derivative function, initial conditions, + * minimum step size and relative error tolerance for all items in the state vector as argument. + * \param coefficients Coefficients to use with this integrator. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, equal for all individual state + * vector elements. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const BulirschStoerRationalFunctionSequences& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize = 1.0e-15, + const typename StateType::Scalar relativeErrorTolerance = 1.0e-12 ) : + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + relativeErrorTolerance ) ), + isMinimumStepSizeViolated_( false ) + { + maximumNumberOfAttempts_ = sequence_.rationalFunctionSequence.size( ); + + subSteps_.resize( maximumNumberOfAttempts_ ); + + integratedStates_.resize( maximumNumberOfAttempts_ ); + for( unsigned int i = 0; i < maximumNumberOfAttempts_; i++ ) + { + integratedStates_[ i ].resize( maximumNumberOfAttempts_ ); + } + } + + //! Get step size of the next step. + /*! + * Returns the step size of the next step. + * \return Step size to be used for the next step. + */ + virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } + + //! Get current state. + /*! + * Returns the current state of the integrator. + * \return Current integrated state. + */ + virtual StateType getCurrentState( ) const { return currentState_; } + + //! Returns the current independent variable. + /*! + * Returns the current value of the independent variable of the integrator. + * \return Current independent variable. + */ + virtual IndependentVariableType getCurrentIndependentVariable( ) const + { + return currentIndependentVariable_; + } + + //! Perform a single integration step. + /*! + * Perform a single integration step and compute a new step size. + * \param stepSize The step size to take. If the time step is too large to satisfy the error + * constraints, the step is redone until the error constraint is satisfied. + * \return The state at the end of the interval. + */ + virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); + + //! Rollback internal state to the last state. + /*! + * Performs rollback of the internal state to the last state. This function can only be called + * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by + * implementations, and can not be called before any of these functions have been called. Will + * return true if the rollback was successful, and false otherwise. + * \return True if the rollback was successful. + */ + virtual bool rollbackToPreviousState( ) + { + if ( currentIndependentVariable_ == lastIndependentVariable_ ) + { + return false; + } + + currentIndependentVariable_ = lastIndependentVariable_; + currentState_ = lastState_; + return true; + } + + //! Check if minimum step size constraint was violated. + /*! + * Returns true if the minimum step size constraint has been violated since this integrator + * was constructed. + * \return True if the minimum step size constraint was violated. + */ + bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } + +protected: + + //! Last used step size. + /*! + * Last used step size, passed to either integrateTo( ) or performIntegrationStep( ). + */ + IndependentVariableType stepSize_; + + //! Current independent variable. + /*! + * Current independent variable as computed by performIntegrationStep(). + */ + IndependentVariableType currentIndependentVariable_; + + //! Current state. + /*! + * Current state as computed by performIntegrationStep( ). + */ + StateType currentState_; + + //! Last independent variable. + /*! + * Last independent variable value as computed by performIntegrationStep(). + */ + IndependentVariableType lastIndependentVariable_; + + //! Last state. + /*! + * Last state as computed by performIntegrationStep( ). + */ + StateType lastState_; + + //! Sequence for the integrator. + /*! + * Rational function sequence for the integrator. + */ + BulirschStoerRationalFunctionSequences sequence_; + + //! Minimum step size. + /*! + * Minimum step size. + */ + IndependentVariableType minimumStepSize_; + + //! Relative error tolerance. + /*! + * Relative error tolerance per element in the state. + */ + StateType relativeErrorTolerance_; + + //! Flag to indicate whether the minimum step size constraint has been violated. + /*! + * Flag to indicate whether the minimum step size constraint has been violated. + */ + bool isMinimumStepSizeViolated_; + + //! Execute mid-point method. + /*! + * Executes mid-point method, given a known state and state derivative. + * \param stateAtFirstPoint State at first point. + * \param stateAtCenterPoint State at center point. + * \param independentVariableAtFirstPoint Independent variable at first point. + * \param subStepSize Sub step size between successive states used by mid-point method. + * \param stateAtLastPoint State at last point. + */ + StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ); + + std::vector< std::vector< StateType > > integratedStates_; + + int maximumNumberOfAttempts_; + + std::vector< double > subSteps_; + + +private: +}; + +//! Perform a single integration step. +template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::performIntegrationStep( const IndependentVariableType stepSize ) +{ + + StateType stateAtFirstPoint_; + StateType stateAtCenterPoint_; + StateType stateAtLastPoint_; + + + // Compute sub steps to take. + for ( unsigned int p = 0; p < subSteps_.size( ); p++ ) + { + subSteps_.at( p ) = stepSize / static_cast< double >( + sequence_.rationalFunctionSequence.at( p ) ); + } + + for ( unsigned int i = 0; i < sequence_.rationalFunctionSequence.size( ); i++ ) + { + // Compute Euler step and set as state at center point for use with mid-point method. + stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) + * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); + + // Apply modified mid-point rule. + stateAtFirstPoint_ = currentState_; + IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; + for ( unsigned int j = 0; j < sequence_.rationalFunctionSequence.at( i ) - 1; j++ ) + { + stateAtLastPoint_ = executeMidPointMethod( + stateAtFirstPoint_, stateAtCenterPoint_, + independentVariableAtFirstPoint_, subSteps_.at( i ) ); + + if ( j < sequence_.rationalFunctionSequence.at( i ) - 2 ) + { + stateAtFirstPoint_ = stateAtCenterPoint_; + stateAtCenterPoint_ = stateAtLastPoint_; + independentVariableAtFirstPoint_ += subSteps_.at( i ); + } + } + + // Apply end-point correction. + std::vector< StateType > integratedStatesRow_( i + 1 ); + integratedStatesRow_.front( ) + = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( + currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); + + for ( unsigned int k = 1; k < integratedStatesRow_.size( ); k++ ) + { + integratedStatesRow_.at( k ) = + integratedStatesRow_.at( k - 1 ) + 1.0 / + ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) + * ( integratedStatesRow_.at( k - 1 ) - integratedStates_.at( i - 1 ).at( k - 1 ) ); + } + + integratedStates_.at( i ) = integratedStatesRow_; + + if ( i == sequence_.rationalFunctionSequence.size( ) - 1 ) + { + std::cout << "Problem in BS integrator. " << std::endl; + } + + if ( ( i > 1 ) && + ( ( integratedStatesRow_.back( ) - integratedStates_.at( i - 1 ).back( ) ).array( ).abs( ).maxCoeff( ) + < relativeErrorTolerance_.array( ).maxCoeff( ) ) ) + { + // Accept the current step. + lastIndependentVariable_ = currentIndependentVariable_; + lastState_ = currentState_; + currentIndependentVariable_ += stepSize; + currentState_ = integratedStatesRow_.back( ); + stepSize_ = stepSize; + + break; + } + } + + return currentState_; +} + +//! Execute mid-point method. +template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ) +{ + return stateAtFirstPoint + 2.0 * subStepSize + * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, + stateAtCenterPoint ); +} + +//! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, +//! independent variable = double). +/*! + * Typedef of a variable-step size Bulirsch-Stoer integrator with VectorXds as state and state + * derivative and double as independent variable. + */ +typedef BulirschStoerVariableStepSizeIntegrator< > BulirschStoerVariableStepSizeIntegratorXd; + +} // namespace numerical_integrators +} // namespace tudat + +#endif // TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H From 14076280a7673ab2cc5db0a182a01f730bcf09b7 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 25 Oct 2017 13:24:55 +0200 Subject: [PATCH 02/10] Incorporated BS integrator into framework --- .../NumericalIntegrators/CMakeLists.txt | 3 +- ...ulirschStoerVariableStepSizeIntegrator.cpp | 49 ++++---- ...bulirschStoerRationalFunctionSequences.cpp | 65 ---------- .../bulirschStoerRationalFunctionSequences.h | 71 ----------- ...ulirschStoerVariableStepsizeIntegrator.cpp | 61 ++++++++++ .../bulirschStoerVariableStepsizeIntegrator.h | 108 ++++++++++++----- .../createNumericalIntegrator.h | 111 +++++++++++++++++- 7 files changed, 276 insertions(+), 192 deletions(-) delete mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.cpp delete mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp diff --git a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt index 992af5188e..4ba309cfb2 100755 --- a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt +++ b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt @@ -11,9 +11,9 @@ # Add source files. set(NUMERICALINTEGRATORS_SOURCES "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/rungeKuttaCoefficients.cpp" + "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/numericalIntegrator.cpp" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/UnitTests/numericalIntegratorTests.cpp" - "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerRationalFunctionSequences.cpp" ) # Add header files. @@ -22,7 +22,6 @@ set(NUMERICALINTEGRATORS_HEADERS "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/rungeKuttaCoefficients.h" "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/UnitTests/burdenAndFairesNumericalIntegratorTest.h" "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" - "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/euler.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/numericalIntegrator.h" "${SRCROOT}${NUMERICALINTEGRATORSDIR}/reinitializableNumericalIntegrator.h" diff --git a/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp b/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp index 73ade4ff0e..0f2f81afff 100644 --- a/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp +++ b/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp @@ -25,7 +25,6 @@ #include #include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" -#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" namespace tudat { @@ -36,8 +35,7 @@ const double TUDAT_MACHINE_PRECISION = std::numeric_limits< double >::epsilon( ) BOOST_AUTO_TEST_SUITE( test_bulirsch_stoer_variable_step_size_integrator ) -using numerical_integrators::BulirschStoerVariableStepSizeIntegratorXd; -using numerical_integrators::BulirschStoerRationalFunctionSequences; +using namespace tudat::numerical_integrators; //! Test the validity of the BurlischStoerVariableStepSize integrator. /*! @@ -55,7 +53,7 @@ using numerical_integrators::BulirschStoerRationalFunctionSequences; * tolerance. */ bool testValidityOfBulirschStoerVariableStepSizeIntegrator( - const BulirschStoerRationalFunctionSequences sequence, + const std::vector< unsigned int > sequence, const BulirschStoerVariableStepSizeIntegratorXd::StateDerivativeFunction& stateDerivativeFunction, const double intervalStart, const double intervalEnd, const double stepSize, @@ -171,7 +169,7 @@ bool testDifferentStateAndStateDerivativeTypes( ) using tudat::unit_tests::numerical_integrator_test_functions::computeZeroStateDerivative; tudat::numerical_integrators::BulirschStoerVariableStepSizeIntegrator < double, Eigen::Vector3d, Eigen::VectorXd > integrator( - BulirschStoerRationalFunctionSequences( ), &computeZeroStateDerivative, + getBulirschStoerStepSequence( ), &computeZeroStateDerivative, 0.0, Eigen::Vector3d::Zero( ) ); integrator.integrateTo( 0.0, 0.1 ); @@ -188,7 +186,7 @@ bool testDifferentStateAndStateDerivativeTypes( ) * tolerance. */ bool testValidityOfBulirschStoerVariableStepSizeIntegrator( - const BulirschStoerRationalFunctionSequences& sequence ) + const std::vector< unsigned int >& sequence ) { // Test result initialised to false. bool testBulirschStoerVariableStepSizeIsOk = true; @@ -210,21 +208,21 @@ bool testValidityOfBulirschStoerVariableStepSizeIntegrator( // TUDAT_MACHINE_PRECISION ); // } - // Case 2: test with x_dot = 1, which results in x_f = x_0 + t_f. - { - std::cout<<"test 2"< - -#include - -#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" - -namespace tudat -{ - -namespace numerical_integrators -{ - -//! Get rational function sequence. -const BulirschStoerRationalFunctionSequences& -BulirschStoerRationalFunctionSequences::get( RationalFunctionSequences sequence, - const unsigned int lengthOfSequence ) -{ - static BulirschStoerRationalFunctionSequences bulirschStoerSequence_, deufelhardSequence_; - - switch ( sequence ) - { - case bulirschStoer: - using namespace boost::assign; - bulirschStoerSequence_.rationalFunctionSequence += 2, 4, 6; - for ( unsigned int i = 3; i < lengthOfSequence; i++ ) - { - bulirschStoerSequence_.rationalFunctionSequence.push_back( - 2 * bulirschStoerSequence_.rationalFunctionSequence.at( i - 2 ) ); - } - return bulirschStoerSequence_; - - case deufelhard: - for ( unsigned int i = 0; i < lengthOfSequence; i++ ) - { - deufelhardSequence_.rationalFunctionSequence.push_back( 2 * ( i + 1 ) ); - } - return deufelhardSequence_; - - default: // The default case will never occur because sequence is an enum - throw BulirschStoerRationalFunctionSequences( ); - } -} - -} // namespace integrators -} // namespace tudat diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h deleted file mode 100644 index fa8e4cb849..0000000000 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h +++ /dev/null @@ -1,71 +0,0 @@ -/* Copyright (c) 2010-2017, Delft University of Technology - * All rigths reserved - * - * This file is part of the Tudat. Redistribution and use in source and - * binary forms, with or without modification, are permitted exclusively - * under the terms of the Modified BSD license. You should have received - * a copy of the license with this file. If not, please or visit: - * http://tudat.tudelft.nl/LICENSE. - * - */ - - -#ifndef TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H -#define TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H - -#include - -namespace tudat -{ - -namespace numerical_integrators -{ - -//! Struct that defines the rational function sequence for a Bulirsch-Stoer integrator. -/*! - * Struct that defines the rational function sequence for a Bulirsch-Stoer integrator. - */ -struct BulirschStoerRationalFunctionSequences -{ - - //! Default constructor. - /*! - * Default constructor that initializes without setting the rational function sequence. - */ - BulirschStoerRationalFunctionSequences( ) { } - - //! Constructor. - /*! - * Constructor that sets the rational function sequence. - * \param sequence Rational function sequence. - */ - BulirschStoerRationalFunctionSequences( const std::vector< unsigned int > sequence ) : - rationalFunctionSequence( sequence ) { } - - //! Rational function sequence. - /*! - * Rational function sequence. - */ - std::vector< unsigned int > rationalFunctionSequence; - - //! Enum of predefined rational function sequences. - enum RationalFunctionSequences - { - bulirschStoer, - deufelhard - }; - - //! Get rational function sequence. - /*! - * Returns requested rational function sequence. - * \param rationalFunctionSequence The rational function sequence. - * \return The requested rational function sequence. - */ - static const BulirschStoerRationalFunctionSequences& get( - RationalFunctionSequences sequence, const unsigned int lengthOfSequence = 100 ); -}; - -} // namespace integrators -} // namespace tudat - -#endif // TUDAT_BULIRSCH_STOER_RATIONAL_FUNCTION_SEQUENCES_H diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp new file mode 100644 index 0000000000..56e76a7c85 --- /dev/null +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp @@ -0,0 +1,61 @@ +/* Copyright (c) 2010-2012 Delft University of Technology. + * + * This software is protected by national and international copyright. + * Any unauthorized use, reproduction or modification is unlawful and + * will be prosecuted. Commercial and non-private application of the + * software in any form is strictly prohibited unless otherwise granted + * by the authors. + * + * The code is provided without any warranty; without even the implied + * warranty of merchantibility or fitness for a particular purpose. + * + * Changelog + * YYMMDD Author Comment + * 120316 K. Kumar File created. + * + * References + * Press W.H., et al. Numerical Recipes in C++: The Art of + * Scientific Computing. Cambridge University Press, February 2002. + * + */ + +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" + +namespace tudat +{ +namespace numerical_integrators +{ + + +std::vector< unsigned int > getBulirschStoerStepSequence( + const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType, + const unsigned int lengthOfSequence ) +{ + std::vector< unsigned int > stepSequence; + switch ( extrapolationMethodStepSequenceType ) + { + case bulirsch_stoer_sequence: + using namespace boost::assign; + stepSequence += 2, 4, 6; + for ( unsigned int i = 3; i < lengthOfSequence; i++ ) + { + stepSequence.push_back( + 2 * stepSequence.at( i - 2 ) ); + } + break; + case deufelhard_sequence: + for ( unsigned int i = 0; i < lengthOfSequence; i++ ) + { + stepSequence.push_back( 2 * ( i + 1 ) ); + } + break; + default: // The default case will never occur because sequence is an enum + throw std::runtime_error( "Error, did not recognize step sequence" ); + } + + return stepSequence; +} + +} // namespace numerical_integrators + +} // namespace tudat diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index 75ed7487fa..b666e5f4bb 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -24,18 +24,30 @@ #include +#include + #include #include #include -#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerRationalFunctionSequences.h" namespace tudat { namespace numerical_integrators { +enum ExtrapolationMethodStepSequences +{ + bulirsch_stoer_sequence, + deufelhard_sequence +}; + + +std::vector< unsigned int > getBulirschStoerStepSequence( + const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, + const unsigned int lengthOfSequence = 12 ); + //! Class that implements the Bulirsch-Stoer variable stepsize integrator. /*! * Class that implements the Bulirsch-Stoer variable step size integrator. @@ -81,7 +93,7 @@ class BulirschStoerVariableStepSizeIntegrator : * \sa NumericalIntegrator::NumericalIntegrator. */ BulirschStoerVariableStepSizeIntegrator( - const BulirschStoerRationalFunctionSequences& sequence, + const std::vector< unsigned int >& sequence, const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, const IndependentVariableType minimumStepSize, @@ -90,7 +102,16 @@ class BulirschStoerVariableStepSizeIntegrator : currentState_( initialState ), lastIndependentVariable_( intervalStart ), sequence_( sequence ), minimumStepSize_( minimumStepSize ), relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ) - { } + { + maximumNumberOfAttempts_ = sequence_.size( ); + subSteps_.resize( maximumNumberOfAttempts_ ); + + integratedStates_.resize( maximumNumberOfAttempts_ ); + for( unsigned int i = 0; i < maximumNumberOfAttempts_; i++ ) + { + integratedStates_[ i ].resize( maximumNumberOfAttempts_ ); + } + } //! Default constructor. /*! @@ -107,20 +128,22 @@ class BulirschStoerVariableStepSizeIntegrator : * \sa NumericalIntegrator::NumericalIntegrator. */ BulirschStoerVariableStepSizeIntegrator( - const BulirschStoerRationalFunctionSequences& sequence, + const std::vector< unsigned int >& sequence, const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, const IndependentVariableType minimumStepSize = 1.0e-15, - const typename StateType::Scalar relativeErrorTolerance = 1.0e-12 ) : + const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, + const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12) : Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), currentState_( initialState ), lastIndependentVariable_( intervalStart ), sequence_( sequence ), minimumStepSize_( minimumStepSize ), relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), relativeErrorTolerance ) ), + absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + absoluteErrorTolerance ) ), isMinimumStepSizeViolated_( false ) { - maximumNumberOfAttempts_ = sequence_.rationalFunctionSequence.size( ); - + maximumNumberOfAttempts_ = sequence_.size( ); subSteps_.resize( maximumNumberOfAttempts_ ); integratedStates_.resize( maximumNumberOfAttempts_ ); @@ -227,7 +250,7 @@ class BulirschStoerVariableStepSizeIntegrator : /*! * Rational function sequence for the integrator. */ - BulirschStoerRationalFunctionSequences sequence_; + std::vector< unsigned int > sequence_; //! Minimum step size. /*! @@ -241,6 +264,8 @@ class BulirschStoerVariableStepSizeIntegrator : */ StateType relativeErrorTolerance_; + StateType absoluteErrorTolerance_; + //! Flag to indicate whether the minimum step size constraint has been violated. /*! * Flag to indicate whether the minimum step size constraint has been violated. @@ -262,7 +287,7 @@ class BulirschStoerVariableStepSizeIntegrator : std::vector< std::vector< StateType > > integratedStates_; - int maximumNumberOfAttempts_; + unsigned int maximumNumberOfAttempts_; std::vector< double > subSteps_; @@ -277,6 +302,9 @@ BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, Sta ::performIntegrationStep( const IndependentVariableType stepSize ) { + // std::cout<currentIndependentVariable_<( - sequence_.rationalFunctionSequence.at( p ) ); + sequence_.at( p ) ); } - for ( unsigned int i = 0; i < sequence_.rationalFunctionSequence.size( ); i++ ) + for ( unsigned int i = 0; i < sequence_.size( ); i++ ) { // Compute Euler step and set as state at center point for use with mid-point method. stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) @@ -298,13 +326,13 @@ ::performIntegrationStep( const IndependentVariableType stepSize ) // Apply modified mid-point rule. stateAtFirstPoint_ = currentState_; IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; - for ( unsigned int j = 0; j < sequence_.rationalFunctionSequence.at( i ) - 1; j++ ) + for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) { stateAtLastPoint_ = executeMidPointMethod( stateAtFirstPoint_, stateAtCenterPoint_, independentVariableAtFirstPoint_, subSteps_.at( i ) ); - if ( j < sequence_.rationalFunctionSequence.at( i ) - 2 ) + if ( j < sequence_.at( i ) - 2 ) { stateAtFirstPoint_ = stateAtCenterPoint_; stateAtCenterPoint_ = stateAtLastPoint_; @@ -313,41 +341,67 @@ ::performIntegrationStep( const IndependentVariableType stepSize ) } // Apply end-point correction. - std::vector< StateType > integratedStatesRow_( i + 1 ); - integratedStatesRow_.front( ) + integratedStates_[ i ][ 0 ] = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); - for ( unsigned int k = 1; k < integratedStatesRow_.size( ); k++ ) + // std::cout<<"Integrated state: "< 1 ) +// { +// std::cout<<"i: "< 1 ) && - ( ( integratedStatesRow_.back( ) - integratedStates_.at( i - 1 ).back( ) ).array( ).abs( ).maxCoeff( ) - < relativeErrorTolerance_.array( ).maxCoeff( ) ) ) + ( ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ) + < relativeErrorTolerance_.array( ).maxCoeff( ) ) ) { // Accept the current step. lastIndependentVariable_ = currentIndependentVariable_; lastState_ = currentState_; currentIndependentVariable_ += stepSize; - currentState_ = integratedStatesRow_.back( ); + currentState_ = integratedStates_[ i ][ i ]; stepSize_ = stepSize; +// std::cout<<"Accepting: "<stepSize_ = 0.7 * stepSize * std::pow( relativeErrorTolerance_.array( ).maxCoeff( ) / maximumErrorValue, + 1.0 / ( 2.0 * i - 1.0 ) ); + +// std::cout<<"Resetting: "<stepSize_<<" "<stepSize_ ); + } } + // std::cout<<"End"< #include +#include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" #include "Tudat/Mathematics/NumericalIntegrators/numericalIntegrator.h" #include "Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h" #include "Tudat/Mathematics/NumericalIntegrators/euler.h" @@ -35,7 +36,8 @@ enum AvailableIntegrators { rungeKutta4, euler, - rungeKuttaVariableStepSize + rungeKuttaVariableStepSize, + bulirsch_stoer, }; //! Class to define settings of numerical integrator @@ -202,6 +204,90 @@ class RungeKuttaVariableStepSizeSettings: public IntegratorSettings< TimeType > const TimeType minimumFactorDecreaseForNextStepSize_; }; +template< typename TimeType = double > +class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > +{ +public: + + //! Constructor + /*! + * Constructor for variable step RK integrator settings. + * \param integratorType Type of numerical integrator (must be an RK variable step type) + * \param initialTime Start time (independent variable) of numerical integration. + * \param initialTimeStep Initial time (independent variable) step used in numerical integration. + * Adapted during integration + * \param coefficientSet Coefficient set (butcher tableau) to use in integration. + * \param minimumStepSize Minimum step size for integration. Integration stops (exception thrown) if time step + * comes below this value. + * \param maximumStepSize Maximum step size for integration. + * \param relativeErrorTolerance Relative error tolerance for step size control + * \param absoluteErrorTolerance Absolute error tolerance for step size control + * \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration + * time steps, with n = saveFrequency). + * \param assessPropagationTerminationConditionDuringIntegrationSubsteps Whether the propagation termination + * conditions should be evaluated during the intermediate sub-steps of the integrator (`true`) or only at the end of + * each integration step (`false`). + * \param safetyFactorForNextStepSize Safety factor for step size control + * \param maximumFactorIncreaseForNextStepSize Maximum increase factor in time step in subsequent iterations. + * \param minimumFactorDecreaseForNextStepSize Maximum decrease factor in time step in subsequent iterations. + */ + BulirschStoerIntegratorSettings( + const TimeType initialTime, + const TimeType initialTimeStep, + const ExtrapolationMethodStepSequences extrapolationSequence, + const unsigned int maximumNumberOfSteps, + const TimeType minimumStepSize, const TimeType maximumStepSize, + const TimeType relativeErrorTolerance = 1.0E-12, + const TimeType absoluteErrorTolerance = 1.0E-12, + const int saveFrequency = 1, + const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false, + const TimeType safetyFactorForNextStepSize = 0.8, + const TimeType maximumFactorIncreaseForNextStepSize = 4.0, + const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ): + IntegratorSettings< TimeType >( bulirsch_stoer, initialTime, initialTimeStep, saveFrequency, + assessPropagationTerminationConditionDuringIntegrationSubsteps ), + extrapolationSequence_( extrapolationSequence ), maximumNumberOfSteps_( maximumNumberOfSteps ), + minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), + relativeErrorTolerance_( relativeErrorTolerance ), absoluteErrorTolerance_( absoluteErrorTolerance ), + safetyFactorForNextStepSize_( safetyFactorForNextStepSize ), + maximumFactorIncreaseForNextStepSize_( maximumFactorIncreaseForNextStepSize ), + minimumFactorDecreaseForNextStepSize_( minimumFactorDecreaseForNextStepSize ){ } + + //! Destructor + /*! + * Destructor + */ + ~BulirschStoerIntegratorSettings( ){ } + + ExtrapolationMethodStepSequences extrapolationSequence_; + + unsigned int maximumNumberOfSteps_; + + //! Minimum step size for integration. + /*! + * Minimum step size for integration. Integration stops (exception thrown) if time step comes below this value. + */ + const TimeType minimumStepSize_; + + //! Maximum step size for integration. + const TimeType maximumStepSize_; + + //! Relative error tolerance for step size control + const TimeType relativeErrorTolerance_; + + //! Absolute error tolerance for step size control + const TimeType absoluteErrorTolerance_; + + //! Safety factor for step size control + const TimeType safetyFactorForNextStepSize_; + + //! Maximum increase factor in time step in subsequent iterations. + const TimeType maximumFactorIncreaseForNextStepSize_; + + //! Maximum decrease factor in time step in subsequent iterations. + const TimeType minimumFactorDecreaseForNextStepSize_; +}; + //! Function to create a numerical integrator. /*! * Function to create a numerical integrator from given integrator settings, state derivative function and initial state. @@ -268,6 +354,29 @@ DependentVariableType, TimeStepType > > createIntegrator( } break; } + case bulirsch_stoer: + { + // Check input consistency + boost::shared_ptr< BulirschStoerIntegratorSettings< IndependentVariableType > > bulirschStoerIntegratorSettings = + boost::dynamic_pointer_cast< BulirschStoerIntegratorSettings< IndependentVariableType > >( + integratorSettings ); + if( bulirschStoerIntegratorSettings == NULL ) + { + std::runtime_error( "Error, type of integrator settings (rungeKuttaVariableStepSize) not compatible with selected integrator (derived class of IntegratorSettings must be RungeKuttaVariableStepSizeSettings for this type)" ); + } + else + { + integrator = boost::make_shared< + BulirschStoerVariableStepSizeIntegrator + < IndependentVariableType, DependentVariableType, DependentVariableType > > + ( getBulirschStoerStepSequence( bulirschStoerIntegratorSettings->extrapolationSequence_, + bulirschStoerIntegratorSettings->maximumNumberOfSteps_ ), + stateDerivativeFunction, integratorSettings->initialTime_, initialState, + static_cast< TimeStepType >( bulirschStoerIntegratorSettings->minimumStepSize_ ), + bulirschStoerIntegratorSettings->relativeErrorTolerance_ ); + } + break; + } default: std::runtime_error( "Error, integrator " + boost::lexical_cast< std::string >( integratorSettings->integratorType_ ) + From 9dfe512b3fcb8314a6b731ab63d08892592ec698 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 25 Oct 2017 15:59:42 +0200 Subject: [PATCH 03/10] Going to include step-size and order control --- .../bulirschStoerVariableStepsizeIntegrator.h | 37 +++++++------------ .../createNumericalIntegrator.h | 3 +- 2 files changed, 15 insertions(+), 25 deletions(-) diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index b666e5f4bb..b87049db1b 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -133,7 +133,7 @@ class BulirschStoerVariableStepSizeIntegrator : const IndependentVariableType intervalStart, const StateType& initialState, const IndependentVariableType minimumStepSize = 1.0e-15, const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, - const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12) : + const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), currentState_( initialState ), lastIndependentVariable_( intervalStart ), sequence_( sequence ), minimumStepSize_( minimumStepSize ), @@ -345,8 +345,6 @@ ::performIntegrationStep( const IndependentVariableType stepSize ) = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); - // std::cout<<"Integrated state: "< 1 ) -// { -// std::cout<<"i: "< 1 ) && ( ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ) - < relativeErrorTolerance_.array( ).maxCoeff( ) ) ) + < errorTolerance_.array( ).maxCoeff( ) ) ) { // Accept the current step. lastIndependentVariable_ = currentIndependentVariable_; @@ -378,22 +370,19 @@ ::performIntegrationStep( const IndependentVariableType stepSize ) currentState_ = integratedStates_[ i ][ i ]; stepSize_ = stepSize; -// std::cout<<"Accepting: "<stepSize_ = 0.7 * stepSize * std::pow( relativeErrorTolerance_.array( ).maxCoeff( ) / maximumErrorValue, - 1.0 / ( 2.0 * i - 1.0 ) ); -// std::cout<<"Resetting: "<stepSize_<<" "<stepSize_ = 0.7 * stepSize * std::pow( errorTolerance_.array( ).maxCoeff( ) / maximumErrorValue, + 1.0 / ( 2.0 * i - 1.0 ) ); return performIntegrationStep( this->stepSize_ ); } diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 834921e2b6..6d58040c75 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -373,7 +373,8 @@ DependentVariableType, TimeStepType > > createIntegrator( bulirschStoerIntegratorSettings->maximumNumberOfSteps_ ), stateDerivativeFunction, integratorSettings->initialTime_, initialState, static_cast< TimeStepType >( bulirschStoerIntegratorSettings->minimumStepSize_ ), - bulirschStoerIntegratorSettings->relativeErrorTolerance_ ); + bulirschStoerIntegratorSettings->relativeErrorTolerance_, + bulirschStoerIntegratorSettings->absoluteErrorTolerance_ ); } break; } From 6107a59b4e08d74a1ab3d9f7e6bc94378c878de6 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 25 Oct 2017 22:30:16 +0200 Subject: [PATCH 04/10] Included step-size control --- .../bulirschStoerVariableStepsizeIntegrator.h | 315 ++++++----- ...rVariableStepsizeVariableOrderIntegrator.h | 533 ++++++++++++++++++ ...iableStepsizeVariableOrderIntegrator_old.h | 526 +++++++++++++++++ .../createNumericalIntegrator.h | 8 +- 4 files changed, 1245 insertions(+), 137 deletions(-) create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h create mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index b87049db1b..d9b3688ff4 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -59,7 +59,7 @@ std::vector< unsigned int > getBulirschStoerStepSequence( * \sa NumericalIntegrator. */ template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, - typename StateDerivativeType = Eigen::VectorXd > + typename StateDerivativeType = Eigen::VectorXd, typename TimeStepType = double > class BulirschStoerVariableStepSizeIntegrator : public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > { @@ -97,19 +97,29 @@ class BulirschStoerVariableStepSizeIntegrator : const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, const IndependentVariableType minimumStepSize, - const StateType& relativeErrorTolerance ) : + const IndependentVariableType maximumStepSize, + const StateType& relativeErrorTolerance, + const StateType& absoluteErrorTolerance, + const TimeStepType safetyFactorForNextStepSize = 0.6, + const TimeStepType maximumFactorIncreaseForNextStepSize = 4.0, + const TimeStepType minimumFactorDecreaseForNextStepSize = 0.1 ): Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), - relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ) + sequence_( sequence ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), + relativeErrorTolerance_( relativeErrorTolerance ), + absoluteErrorTolerance_( absoluteErrorTolerance ), + safetyFactorForNextStepSize_( safetyFactorForNextStepSize ), + maximumFactorIncreaseForNextStepSize_( maximumFactorIncreaseForNextStepSize ), + minimumFactorDecreaseForNextStepSize_( minimumFactorDecreaseForNextStepSize ), + isMinimumStepSizeViolated_( false ) { - maximumNumberOfAttempts_ = sequence_.size( ); - subSteps_.resize( maximumNumberOfAttempts_ ); + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ ); - integratedStates_.resize( maximumNumberOfAttempts_ ); - for( unsigned int i = 0; i < maximumNumberOfAttempts_; i++ ) + integratedStates_.resize( maximumStepIndex_ ); + for( unsigned int i = 0; i < maximumStepIndex_; i++ ) { - integratedStates_[ i ].resize( maximumNumberOfAttempts_ ); + integratedStates_[ i ].resize( maximumStepIndex_ ); } } @@ -131,25 +141,32 @@ class BulirschStoerVariableStepSizeIntegrator : const std::vector< unsigned int >& sequence, const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize = 1.0e-15, + const IndependentVariableType minimumStepSize, + const IndependentVariableType maximumStepSize, const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, - const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): + const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12, + const TimeStepType safetyFactorForNextStepSize = 0.75, + const TimeStepType maximumFactorIncreaseForNextStepSize = 4.0, + const TimeStepType minimumFactorDecreaseForNextStepSize = 0.1 ): Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), relativeErrorTolerance ) ), absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), absoluteErrorTolerance ) ), + safetyFactorForNextStepSize_( safetyFactorForNextStepSize ), + maximumFactorIncreaseForNextStepSize_( maximumFactorIncreaseForNextStepSize ), + minimumFactorDecreaseForNextStepSize_( minimumFactorDecreaseForNextStepSize ), isMinimumStepSizeViolated_( false ) { - maximumNumberOfAttempts_ = sequence_.size( ); - subSteps_.resize( maximumNumberOfAttempts_ ); + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ + 1 ); - integratedStates_.resize( maximumNumberOfAttempts_ ); - for( unsigned int i = 0; i < maximumNumberOfAttempts_; i++ ) + integratedStates_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) { - integratedStates_[ i ].resize( maximumNumberOfAttempts_ ); + integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); } } @@ -184,7 +201,129 @@ class BulirschStoerVariableStepSizeIntegrator : * constraints, the step is redone until the error constraint is satisfied. * \return The state at the end of the interval. */ - virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); + virtual StateType performIntegrationStep( const IndependentVariableType stepSize ) + { + StateType stateAtFirstPoint_; + StateType stateAtCenterPoint_; + StateType stateAtLastPoint_; + + bool stepSuccessful = 0; + + // Compute sub steps to take. + for ( unsigned int p = 0; p < subSteps_.size( ); p++ ) + { + subSteps_.at( p ) = stepSize / static_cast< double >( + sequence_.at( p ) ); + } + + double errorScaleTerm = TUDAT_NAN; + for ( unsigned int i = 0; i <= maximumStepIndex_; i++ ) + { + // Compute Euler step and set as state at center point for use with mid-point method. + stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) + * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); + + // Apply modified mid-point rule. + stateAtFirstPoint_ = currentState_; + IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; + for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) + { + stateAtLastPoint_ = executeMidPointMethod( + stateAtFirstPoint_, stateAtCenterPoint_, + independentVariableAtFirstPoint_, subSteps_.at( i ) ); + + if ( j < sequence_.at( i ) - 2 ) + { + stateAtFirstPoint_ = stateAtCenterPoint_; + stateAtCenterPoint_ = stateAtLastPoint_; + independentVariableAtFirstPoint_ += subSteps_.at( i ); + } + } + + // Apply end-point correction. + integratedStates_[ i ][ 0 ] + = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( + currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); + + for ( unsigned int k = 1; k < i + 1; k++ ) + { + integratedStates_[ i ][ k ] = + integratedStates_[ i ][ k - 1 ] + 1.0 / + ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) + * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); + } + + if( i == maximumStepIndex_ ) + { + const StateType errorTolerance_ = + ( integratedStates_.at( i ).at( i ).array( ).abs( ) * relativeErrorTolerance_.array( ) ).matrix( ) + + absoluteErrorTolerance_; + + double maximumAllowableErrorValue = errorTolerance_.array( ).maxCoeff( ); + double maximumErrorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); + + errorScaleTerm = safetyFactorForNextStepSize_ * std::pow( maximumAllowableErrorValue / maximumErrorValue, + ( 1.0 / static_cast< double >( 2 * i - 1 ) ) ); + + if( maximumErrorValue < maximumAllowableErrorValue ) + { + // Accept the current step. + lastIndependentVariable_ = currentIndependentVariable_; + lastState_ = currentState_; + currentIndependentVariable_ += stepSize; + currentState_ = integratedStates_[ i ][ i ]; + stepSize_ = stepSize; + stepSuccessful = true; + } + else + { + stepSuccessful = false; + } + } + } + if( !stepSuccessful ) + { + if( errorScaleTerm < minimumFactorDecreaseForNextStepSize_ ) + { + this->stepSize_ = minimumFactorDecreaseForNextStepSize_ * errorScaleTerm; + } + else + { + this->stepSize_ = stepSize * safetyFactorForNextStepSize_ * errorScaleTerm; + } + + if( stepSize_ < minimumStepSize_ ) + { + isMinimumStepSizeViolated_ = true; + throw std::runtime_error( "Error in BS integrator, minimum step size exceeded" ); + } + performIntegrationStep( stepSize_ ); + } + else + { + if( errorScaleTerm > maximumFactorIncreaseForNextStepSize_ ) + { + this->stepSize_ = stepSize * maximumFactorIncreaseForNextStepSize_; + + if( stepSize < maximumStepSize_ && this->stepSize_ >= maximumStepSize_ ) + { + stepSize_ = maximumStepSize_; + performIntegrationStep( stepSize_ ); + } + else if( this->stepSize_ < maximumStepSize_ ) + { + performIntegrationStep( stepSize_ ); + } + } + else + { + this->stepSize_ = stepSize * errorScaleTerm; + } + } + + + return currentState_; + } //! Rollback internal state to the last state. /*! @@ -214,7 +353,7 @@ class BulirschStoerVariableStepSizeIntegrator : */ bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } -protected: +private: //! Last used step size. /*! @@ -258,14 +397,29 @@ class BulirschStoerVariableStepSizeIntegrator : */ IndependentVariableType minimumStepSize_; + IndependentVariableType maximumStepSize_; + //! Relative error tolerance. /*! * Relative error tolerance per element in the state. */ StateType relativeErrorTolerance_; + //! Absolute error tolerance. + /*! + * Absolute error tolerance per element in the state. + */ StateType absoluteErrorTolerance_; + + TimeStepType safetyFactorForNextStepSize_; + + + TimeStepType maximumFactorIncreaseForNextStepSize_; + + + TimeStepType minimumFactorDecreaseForNextStepSize_; + //! Flag to indicate whether the minimum step size constraint has been violated. /*! * Flag to indicate whether the minimum step size constraint has been violated. @@ -283,130 +437,21 @@ class BulirschStoerVariableStepSizeIntegrator : */ StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ); + const IndependentVariableType subStepSize ) + { + return stateAtFirstPoint + 2.0 * subStepSize + * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, + stateAtCenterPoint ); + } std::vector< std::vector< StateType > > integratedStates_; - unsigned int maximumNumberOfAttempts_; + unsigned int maximumStepIndex_; std::vector< double > subSteps_; - -private: }; -//! Perform a single integration step. -template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::performIntegrationStep( const IndependentVariableType stepSize ) -{ - - // std::cout<currentIndependentVariable_<( - sequence_.at( p ) ); - } - - for ( unsigned int i = 0; i < sequence_.size( ); i++ ) - { - // Compute Euler step and set as state at center point for use with mid-point method. - stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) - * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); - - // Apply modified mid-point rule. - stateAtFirstPoint_ = currentState_; - IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; - for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) - { - stateAtLastPoint_ = executeMidPointMethod( - stateAtFirstPoint_, stateAtCenterPoint_, - independentVariableAtFirstPoint_, subSteps_.at( i ) ); - - if ( j < sequence_.at( i ) - 2 ) - { - stateAtFirstPoint_ = stateAtCenterPoint_; - stateAtCenterPoint_ = stateAtLastPoint_; - independentVariableAtFirstPoint_ += subSteps_.at( i ); - } - } - - // Apply end-point correction. - integratedStates_[ i ][ 0 ] - = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( - currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); - - for ( unsigned int k = 1; k < i + 1; k++ ) - { - integratedStates_[ i ][ k ] = - integratedStates_[ i ][ k - 1 ] + 1.0 / - ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) - * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); - } - - const StateType errorTolerance_ = - ( integratedStates_.at( i ).at( i ).array( ).abs( ) * - relativeErrorTolerance_.array( ) ).matrix( ) - + absoluteErrorTolerance_; - - - if ( ( i > 1 ) && - ( ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ) - < errorTolerance_.array( ).maxCoeff( ) ) ) - { - // Accept the current step. - lastIndependentVariable_ = currentIndependentVariable_; - lastState_ = currentState_; - currentIndependentVariable_ += stepSize; - currentState_ = integratedStates_[ i ][ i ]; - stepSize_ = stepSize; - - if( i < ( sequence_.size( ) - 1 ) ) - { - stepSize_ *= 4.0; - } - - break; - } - else if( i == ( maximumNumberOfAttempts_ - 1 ) ) - { - double maximumErrorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); - - this->stepSize_ = 0.7 * stepSize * std::pow( errorTolerance_.array( ).maxCoeff( ) / maximumErrorValue, - 1.0 / ( 2.0 * i - 1.0 ) ); - - return performIntegrationStep( this->stepSize_ ); - } - } - - // std::cout<<"End"< -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, - const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ) -{ - return stateAtFirstPoint + 2.0 * subStepSize - * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, - stateAtCenterPoint ); -} - //! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, //! independent variable = double). /*! diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h new file mode 100644 index 0000000000..397445c34b --- /dev/null +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h @@ -0,0 +1,533 @@ +/* Copyright (c) 2010-2012 Delft University of Technology. + * + * This software is protected by national and international copyright. + * Any unauthorized use, reproduction or modification is unlawful and + * will be prosecuted. Commercial and non-private application of the + * software in any form is strictly prohibited unless otherwise granted + * by the authors. + * + * The code is provided without any warranty; without even the implied + * warranty of merchantibility or fitness for a particular purpose. + * + * Changelog + * YYMMDD Author Comment + * 120316 K. Kumar File created. + * + * References + * Press W.H., et al. Numerical Recipes in C++: The Art of + * Scientific Computing. Cambridge University Press, February 2002. + * + */ + +#ifndef TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H +#define TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H + +#include + +#include + +#include + +#include +#include + + +namespace tudat +{ +namespace numerical_integrators +{ + +enum ExtrapolationMethodStepSequences +{ + bulirsch_stoer_sequence, + deufelhard_sequence +}; + + +std::vector< unsigned int > getBulirschStoerStepSequence( + const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, + const unsigned int lengthOfSequence = 12 ); + +//! Class that implements the Bulirsch-Stoer variable stepsize integrator. +/*! + * Class that implements the Bulirsch-Stoer variable step size integrator. + * \tparam StateType The type of the state. This type should be an Eigen::Matrix derived type. + * \tparam StateDerivativeType The type of the state derivative. This type should be an + * Eigen::Matrix derived type. + * \tparam IndependentVariableType The type of the independent variable. This type should be + * either a float or double. + * \sa NumericalIntegrator. + */ +template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, + typename StateDerivativeType = Eigen::VectorXd > +class BulirschStoerVariableStepSizeIntegrator : + public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > +{ +public: + + //! Typedef of the base class. + /*! + * Typedef of the base class with all template parameters filled in. + */ + typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; + + //! Typedef to the state derivative function. + /*! + * Typedef to the state derivative function inherited from the base class. + * \sa NumericalIntegrator::StateDerivativeFunction. + */ + typedef typename Base::StateDerivativeFunction StateDerivativeFunction; + + //! Default constructor. + /*! + * Default constructor, taking sequence, a state derivative function, initial conditions, + * minimum step size and relative error tolerance per item in the state vector as argument. + * \param sequence Rational function sequence used by algorithm. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, for each individual state + * vector element. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const std::vector< unsigned int >& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize, + const StateType& relativeErrorTolerance ) : + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ) + { + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ ); + + integratedStates_.resize( maximumStepIndex_ ); + for( unsigned int i = 0; i < maximumStepIndex_; i++ ) + { + integratedStates_[ i ].resize( maximumStepIndex_ ); + } + + initializeWorkParameters( ); + } + + //! Default constructor. + /*! + * Default constructor, taking coefficients, a state derivative function, initial conditions, + * minimum step size and relative error tolerance for all items in the state vector as argument. + * \param coefficients Coefficients to use with this integrator. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, equal for all individual state + * vector elements. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const std::vector< unsigned int >& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize = 1.0e-15, + const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, + const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + relativeErrorTolerance ) ), + absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + absoluteErrorTolerance ) ), + isMinimumStepSizeViolated_( false ) + { + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ + 1 ); + + integratedStates_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) + { + integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); + } + + initializeWorkParameters( ); + } + + //! Get step size of the next step. + /*! + * Returns the step size of the next step. + * \return Step size to be used for the next step. + */ + virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } + + //! Get current state. + /*! + * Returns the current state of the integrator. + * \return Current integrated state. + */ + virtual StateType getCurrentState( ) const { return currentState_; } + + //! Returns the current independent variable. + /*! + * Returns the current value of the independent variable of the integrator. + * \return Current independent variable. + */ + virtual IndependentVariableType getCurrentIndependentVariable( ) const + { + return currentIndependentVariable_; + } + + //! Perform a single integration step. + /*! + * Perform a single integration step and compute a new step size. + * \param stepSize The step size to take. If the time step is too large to satisfy the error + * constraints, the step is redone until the error constraint is satisfied. + * \return The state at the end of the interval. + */ + virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); + + //! Rollback internal state to the last state. + /*! + * Performs rollback of the internal state to the last state. This function can only be called + * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by + * implementations, and can not be called before any of these functions have been called. Will + * return true if the rollback was successful, and false otherwise. + * \return True if the rollback was successful. + */ + virtual bool rollbackToPreviousState( ) + { + if ( currentIndependentVariable_ == lastIndependentVariable_ ) + { + return false; + } + + currentIndependentVariable_ = lastIndependentVariable_; + currentState_ = lastState_; + return true; + } + + //! Check if minimum step size constraint was violated. + /*! + * Returns true if the minimum step size constraint has been violated since this integrator + * was constructed. + * \return True if the minimum step size constraint was violated. + */ + bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } + +protected: + + void initializeWorkParameters( ) + { + + aTerms_.resize( maximumStepIndex_ + 1 ); + aTerms_[ 0 ] = sequence_.at( 0 ) + 1; + + alphaTerms_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i <= maximumStepIndex_; i++ ) + { + alphaTerms_[ i ].resize( maximumStepIndex_ + 1 ); + + if( i > 0 ) + { + aTerms_[ i ] = sequence_.at( i ) + aTerms_[ i - 1 ]; + } + } + + for( unsigned int i = 0; i < maximumStepIndex_ ; i++ ) + { + for( unsigned int j = 0; j < maximumStepIndex_ ; j++ ) + { + alphaTerms_[ i ][ j ] = std::pow( + relativeErrorTolerance_.array( ).maxCoeff( ), static_cast< double >( aTerms_[ i + 1 ] - aTerms_[ j + 1 ] ) / + static_cast< double >( ( 2 * i - 1 ) * ( aTerms_[ j + 1 ] - aTerms_[ 0 ] + 1 ) ) ); + //std::cout<<"Computing alpha: "< sequence_; + + //! Minimum step size. + /*! + * Minimum step size. + */ + IndependentVariableType minimumStepSize_; + + //! Relative error tolerance. + /*! + * Relative error tolerance per element in the state. + */ + StateType relativeErrorTolerance_; + + StateType absoluteErrorTolerance_; + + //! Flag to indicate whether the minimum step size constraint has been violated. + /*! + * Flag to indicate whether the minimum step size constraint has been violated. + */ + bool isMinimumStepSizeViolated_; + + //! Execute mid-point method. + /*! + * Executes mid-point method, given a known state and state derivative. + * \param stateAtFirstPoint State at first point. + * \param stateAtCenterPoint State at center point. + * \param independentVariableAtFirstPoint Independent variable at first point. + * \param subStepSize Sub step size between successive states used by mid-point method. + * \param stateAtLastPoint State at last point. + */ + StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ); + + std::vector< std::vector< StateType > > integratedStates_; + + unsigned int maximumStepIndex_; + + std::vector< double > subSteps_; + + int optimalIndex_; + + std::vector< std::vector< double > > alphaTerms_; + + std::vector< int > aTerms_; + +private: +}; + +//! Perform a single integration step. +template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::performIntegrationStep( const IndependentVariableType stepSize ) +{ + //std::cout<<"Starting: "<currentIndependentVariable_<( + sequence_.at( p ) ); + } + + double errorScaleTerm = TUDAT_NAN; + int breakIndex = -1; + + for ( unsigned int i = 0; i <= maximumStepIndex_; i++ ) + { + breakIndex = i; + // Compute Euler step and set as state at center point for use with mid-point method. + stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) + * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); + + // Apply modified mid-point rule. + stateAtFirstPoint_ = currentState_; + IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; + for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) + { + stateAtLastPoint_ = executeMidPointMethod( + stateAtFirstPoint_, stateAtCenterPoint_, + independentVariableAtFirstPoint_, subSteps_.at( i ) ); + + if ( j < sequence_.at( i ) - 2 ) + { + stateAtFirstPoint_ = stateAtCenterPoint_; + stateAtCenterPoint_ = stateAtLastPoint_; + independentVariableAtFirstPoint_ += subSteps_.at( i ); + } + } + + // Apply end-point correction. + integratedStates_[ i ][ 0 ] + = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( + currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); + + for ( unsigned int k = 1; k < i + 1; k++ ) + { + integratedStates_[ i ][ k ] = + integratedStates_[ i ][ k - 1 ] + 1.0 / + ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) + * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); + } + + if( i != 1 ) + { + const StateType errorTolerance_ = + ( integratedStates_.at( i ).at( i ).array( ).abs( ) * relativeErrorTolerance_.array( ) ).matrix( ) + + absoluteErrorTolerance_; + + double maximumAllowableErrorValue = errorTolerance_.array( ).maxCoeff( ); + double maximumErrorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); + + errorScaleTerm = 0.8 * std::pow( maximumAllowableErrorValue / maximumErrorValue, + ( 1.0 / static_cast< double >( 2 * i - 1 ) ) ); + double inverseErrorScaleTerm = 1.0 / errorScaleTerm; + inverseErrorScaleTerms[ i - 1 ] = inverseErrorScaleTerm; + } + + if( i == maximumStepIndex_ && ( i >= optimalIndex_ - 1 ) ) + { + + if( maximumErrorValue < maximumAllowableErrorValue ) + { + // Accept the current step. + lastIndependentVariable_ = currentIndependentVariable_; + lastState_ = currentState_; + currentIndependentVariable_ += stepSize; + currentState_ = integratedStates_[ i ][ i ]; + stepSize_ = stepSize; + stepSuccessful = true; + break; + } + + if( ( i == maximumStepIndex_ ) || ( i == optimalIndex_ + 1 ) ) + { + std::cout<<"In A"< 0.7 ) + { + stepSizeChangeFactor = 0.7; + } + + this->stepSize_ = stepSizeChangeFactor * stepSize; + } + + firstStepDone = false; + + { + double scale = 1.0; + for( unsigned int i = 1; k < breakIndex; i++ ) + { + double factor = std::max( inverseErrorScaleTerms[ i ], 0.1 ); + double work = factor * aTerms_[ k + 1 ]; + if( work < minimumWork ) + { + scale = factor; + minimumWork = work; + optimalIndex_ = i + 1; + } + } + this->stepSize_ = this->stepSize_ / scale; + } + + if( !stepSuccessful ) + { + + performIntegrationStep( stepSize_ ); + } + else + { + if( errorScaleTerm > 4.0 ) + { + this->stepSize_ = stepSize * 4.0; + performIntegrationStep( stepSize_ ); + } + else + { + this->stepSize_ = stepSize * errorScaleTerm; + } + } + + + return currentState_; +} + +//! Execute mid-point method. +template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ) +{ + return stateAtFirstPoint + 2.0 * subStepSize + * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, + stateAtCenterPoint ); +} + +//! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, +//! independent variable = double). +/*! + * Typedef of a variable-step size Bulirsch-Stoer integrator with VectorXds as state and state + * derivative and double as independent variable. + */ +typedef BulirschStoerVariableStepSizeIntegrator< > BulirschStoerVariableStepSizeIntegratorXd; + +} // namespace numerical_integrators +} // namespace tudat + +#endif // TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h new file mode 100644 index 0000000000..4d94560364 --- /dev/null +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h @@ -0,0 +1,526 @@ +/* Copyright (c) 2010-2012 Delft University of Technology. + * + * This software is protected by national and international copyright. + * Any unauthorized use, reproduction or modification is unlawful and + * will be prosecuted. Commercial and non-private application of the + * software in any form is strictly prohibited unless otherwise granted + * by the authors. + * + * The code is provided without any warranty; without even the implied + * warranty of merchantibility or fitness for a particular purpose. + * + * Changelog + * YYMMDD Author Comment + * 120316 K. Kumar File created. + * + * References + * Press W.H., et al. Numerical Recipes in C++: The Art of + * Scientific Computing. Cambridge University Press, February 2002. + * + */ + +#ifndef TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H +#define TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H + +#include + +#include + +#include + +#include +#include + + +namespace tudat +{ +namespace numerical_integrators +{ + +enum ExtrapolationMethodStepSequences +{ + bulirsch_stoer_sequence, + deufelhard_sequence +}; + + +std::vector< unsigned int > getBulirschStoerStepSequence( + const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, + const unsigned int lengthOfSequence = 12 ); + +//! Class that implements the Bulirsch-Stoer variable stepsize integrator. +/*! + * Class that implements the Bulirsch-Stoer variable step size integrator. + * \tparam StateType The type of the state. This type should be an Eigen::Matrix derived type. + * \tparam StateDerivativeType The type of the state derivative. This type should be an + * Eigen::Matrix derived type. + * \tparam IndependentVariableType The type of the independent variable. This type should be + * either a float or double. + * \sa NumericalIntegrator. + */ +template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, + typename StateDerivativeType = Eigen::VectorXd > +class BulirschStoerVariableStepSizeIntegrator : + public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > +{ +public: + + //! Typedef of the base class. + /*! + * Typedef of the base class with all template parameters filled in. + */ + typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; + + //! Typedef to the state derivative function. + /*! + * Typedef to the state derivative function inherited from the base class. + * \sa NumericalIntegrator::StateDerivativeFunction. + */ + typedef typename Base::StateDerivativeFunction StateDerivativeFunction; + + //! Default constructor. + /*! + * Default constructor, taking sequence, a state derivative function, initial conditions, + * minimum step size and relative error tolerance per item in the state vector as argument. + * \param sequence Rational function sequence used by algorithm. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, for each individual state + * vector element. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const std::vector< unsigned int >& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize, + const StateType& relativeErrorTolerance ) : + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ), + stepSizeChanged_( true ) + { + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ ); + + integratedStates_.resize( maximumStepIndex_ ); + for( unsigned int i = 0; i < maximumStepIndex_; i++ ) + { + integratedStates_[ i ].resize( maximumStepIndex_ ); + } + + initializeWorkParameters( ); + } + + //! Default constructor. + /*! + * Default constructor, taking coefficients, a state derivative function, initial conditions, + * minimum step size and relative error tolerance for all items in the state vector as argument. + * \param coefficients Coefficients to use with this integrator. + * \param stateDerivativeFunction State derivative function. + * \param intervalStart The start of the integration interval. + * \param initialState The initial state. + * \param minimumStepSize The minimum step size to take. If this constraint is violated, a + * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param relativeErrorTolerance The relative error tolerance, equal for all individual state + * vector elements. + * \sa NumericalIntegrator::NumericalIntegrator. + */ + BulirschStoerVariableStepSizeIntegrator( + const std::vector< unsigned int >& sequence, + const StateDerivativeFunction& stateDerivativeFunction, + const IndependentVariableType intervalStart, const StateType& initialState, + const IndependentVariableType minimumStepSize = 1.0e-15, + const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, + const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): + Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), + currentState_( initialState ), lastIndependentVariable_( intervalStart ), + sequence_( sequence ), minimumStepSize_( minimumStepSize ), + relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + relativeErrorTolerance ) ), + absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), + absoluteErrorTolerance ) ), + isMinimumStepSizeViolated_( false ), + stepSizeChanged_( true ) + { + maximumStepIndex_ = sequence_.size( ) - 1; + subSteps_.resize( maximumStepIndex_ + 1 ); + + integratedStates_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) + { + integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); + } + + initializeWorkParameters( ); + } + + //! Get step size of the next step. + /*! + * Returns the step size of the next step. + * \return Step size to be used for the next step. + */ + virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } + + //! Get current state. + /*! + * Returns the current state of the integrator. + * \return Current integrated state. + */ + virtual StateType getCurrentState( ) const { return currentState_; } + + //! Returns the current independent variable. + /*! + * Returns the current value of the independent variable of the integrator. + * \return Current independent variable. + */ + virtual IndependentVariableType getCurrentIndependentVariable( ) const + { + return currentIndependentVariable_; + } + + //! Perform a single integration step. + /*! + * Perform a single integration step and compute a new step size. + * \param stepSize The step size to take. If the time step is too large to satisfy the error + * constraints, the step is redone until the error constraint is satisfied. + * \return The state at the end of the interval. + */ + virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); + + //! Rollback internal state to the last state. + /*! + * Performs rollback of the internal state to the last state. This function can only be called + * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by + * implementations, and can not be called before any of these functions have been called. Will + * return true if the rollback was successful, and false otherwise. + * \return True if the rollback was successful. + */ + virtual bool rollbackToPreviousState( ) + { + if ( currentIndependentVariable_ == lastIndependentVariable_ ) + { + return false; + } + + currentIndependentVariable_ = lastIndependentVariable_; + currentState_ = lastState_; + return true; + } + + //! Check if minimum step size constraint was violated. + /*! + * Returns true if the minimum step size constraint has been violated since this integrator + * was constructed. + * \return True if the minimum step size constraint was violated. + */ + bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } + +protected: + + //! Last used step size. + /*! + * Last used step size, passed to either integrateTo( ) or performIntegrationStep( ). + */ + IndependentVariableType stepSize_; + + //! Current independent variable. + /*! + * Current independent variable as computed by performIntegrationStep(). + */ + IndependentVariableType currentIndependentVariable_; + + //! Current state. + /*! + * Current state as computed by performIntegrationStep( ). + */ + StateType currentState_; + + //! Last independent variable. + /*! + * Last independent variable value as computed by performIntegrationStep(). + */ + IndependentVariableType lastIndependentVariable_; + + //! Last state. + /*! + * Last state as computed by performIntegrationStep( ). + */ + StateType lastState_; + + //! Sequence for the integrator. + /*! + * Rational function sequence for the integrator. + */ + std::vector< unsigned int > sequence_; + + //! Minimum step size. + /*! + * Minimum step size. + */ + IndependentVariableType minimumStepSize_; + + //! Relative error tolerance. + /*! + * Relative error tolerance per element in the state. + */ + StateType relativeErrorTolerance_; + + StateType absoluteErrorTolerance_; + + //! Flag to indicate whether the minimum step size constraint has been violated. + /*! + * Flag to indicate whether the minimum step size constraint has been violated. + */ + bool isMinimumStepSizeViolated_; + + //! Execute mid-point method. + /*! + * Executes mid-point method, given a known state and state derivative. + * \param stateAtFirstPoint State at first point. + * \param stateAtCenterPoint State at center point. + * \param independentVariableAtFirstPoint Independent variable at first point. + * \param subStepSize Sub step size between successive states used by mid-point method. + * \param stateAtLastPoint State at last point. + */ + StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ); + + void initializeWorkParameters( ) + { + + aTerms_.resize( maximumStepIndex_ + 1 ); + aTerms_[ 0 ] = sequence_.at( 0 ) + 1; + + alphaTerms_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i <= maximumStepIndex_; i++ ) + { + alphaTerms_[ i ].resize( maximumStepIndex_ + 1 ); + + if( i > 0 ) + { + aTerms_[ i ] = sequence_.at( i ) + aTerms_[ i - 1 ]; + } + } + + for( unsigned int i = 0; i < maximumStepIndex_ ; i++ ) + { + for( unsigned int j = 0; j < maximumStepIndex_ ; j++ ) + { + alphaTerms_[ i ][ j ] = std::pow( + relativeErrorTolerance_.array( ).maxCoeff( ), static_cast< double >( aTerms_[ i + 1 ] - aTerms_[ j + 1 ] ) / + static_cast< double >( ( 2 * i - 1 ) * ( aTerms_[ j + 1 ] - aTerms_[ 0 ] + 1 ) ) ); + std::cout<<"Computing alpha: "< > integratedStates_; + + unsigned int maximumStepIndex_; + + unsigned int optimalOrder_; + + std::vector< double > subSteps_; + + bool stepSizeChanged_; + + std::vector< std::vector< double > > alphaTerms_; + + std::vector< int > aTerms_; + +private: +}; + +//! Perform a single integration step. +template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::performIntegrationStep( const IndependentVariableType stepSize ) +{ + //std::cout<<"Starting: "<currentIndependentVariable_<( + sequence_.at( p ) ); + } + + if( stepSizeChanged_ ) + { + optimalOrder_ = maximumStepIndex_; + } + + double stepSizeChangeFactor = TUDAT_NAN; + + for ( unsigned int i = 0; i <= maximumStepIndex_; i++ ) + { + // Compute Euler step and set as state at center point for use with mid-point method. + stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) + * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); + + // Apply modified mid-point rule. + stateAtFirstPoint_ = currentState_; + IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; + for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) + { + stateAtLastPoint_ = executeMidPointMethod( + stateAtFirstPoint_, stateAtCenterPoint_, + independentVariableAtFirstPoint_, subSteps_.at( i ) ); + + if ( j < sequence_.at( i ) - 2 ) + { + stateAtFirstPoint_ = stateAtCenterPoint_; + stateAtCenterPoint_ = stateAtLastPoint_; + independentVariableAtFirstPoint_ += subSteps_.at( i ); + } + } + + // Apply end-point correction. + integratedStates_[ i ][ 0 ] + = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( + currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); + + for ( unsigned int k = 1; k < i + 1; k++ ) + { + integratedStates_[ i ][ k ] = + integratedStates_[ i ][ k - 1 ] + 1.0 / + ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) + * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); + } + + if ( ( i > 1 ) )//&& ( ( i >= optimalOrder_ - 1 ) || stepSizeChanged_ ) ) + { + const StateType errorTolerance_ = + ( integratedStates_.at( i ).at( i ).array( ).abs( ) * relativeErrorTolerance_.array( ) ).matrix( ) + + absoluteErrorTolerance_; + + double maximumErrorValue = errorTolerance_.array( ).maxCoeff( ); + double errorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); + + double errorScaleTerm = std::pow( errorValue / maximumErrorValue, + static_cast< double >( 1 / ( 2 * i - 1 ) ) ); + + //std::cout<<"Scaling errors: "< +StateType +BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > +::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, + const IndependentVariableType independentVariableAtFirstPoint, + const IndependentVariableType subStepSize ) +{ + return stateAtFirstPoint + 2.0 * subStepSize + * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, + stateAtCenterPoint ); +} + +//! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, +//! independent variable = double). +/*! + * Typedef of a variable-step size Bulirsch-Stoer integrator with VectorXds as state and state + * derivative and double as independent variable. + */ +typedef BulirschStoerVariableStepSizeIntegrator< > BulirschStoerVariableStepSizeIntegratorXd; + +} // namespace numerical_integrators +} // namespace tudat + +#endif // TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 6d58040c75..3549c276eb 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -241,7 +241,7 @@ class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > const TimeType absoluteErrorTolerance = 1.0E-12, const int saveFrequency = 1, const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false, - const TimeType safetyFactorForNextStepSize = 0.8, + const TimeType safetyFactorForNextStepSize = 0.75, const TimeType maximumFactorIncreaseForNextStepSize = 4.0, const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ): IntegratorSettings< TimeType >( bulirsch_stoer, initialTime, initialTimeStep, saveFrequency, @@ -373,8 +373,12 @@ DependentVariableType, TimeStepType > > createIntegrator( bulirschStoerIntegratorSettings->maximumNumberOfSteps_ ), stateDerivativeFunction, integratorSettings->initialTime_, initialState, static_cast< TimeStepType >( bulirschStoerIntegratorSettings->minimumStepSize_ ), + static_cast< TimeStepType >( bulirschStoerIntegratorSettings->maximumStepSize_ ), bulirschStoerIntegratorSettings->relativeErrorTolerance_, - bulirschStoerIntegratorSettings->absoluteErrorTolerance_ ); + bulirschStoerIntegratorSettings->absoluteErrorTolerance_, + bulirschStoerIntegratorSettings->safetyFactorForNextStepSize_, + bulirschStoerIntegratorSettings->maximumFactorIncreaseForNextStepSize_, + bulirschStoerIntegratorSettings->minimumFactorDecreaseForNextStepSize_ ); } break; } From 35d76e9ce3a408e56aceeead95069389999d9c45 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Thu, 26 Oct 2017 14:23:22 +0200 Subject: [PATCH 05/10] Minor modifications to BS algorithm --- .../bulirschStoerVariableStepsizeIntegrator.h | 12 +++--------- .../NumericalIntegrators/createNumericalIntegrator.h | 4 ++-- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index d9b3688ff4..a0bddc4f9b 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -283,7 +283,7 @@ class BulirschStoerVariableStepSizeIntegrator : } if( !stepSuccessful ) { - if( errorScaleTerm < minimumFactorDecreaseForNextStepSize_ ) + if( safetyFactorForNextStepSize_ * errorScaleTerm < minimumFactorDecreaseForNextStepSize_ ) { this->stepSize_ = minimumFactorDecreaseForNextStepSize_ * errorScaleTerm; } @@ -304,15 +304,9 @@ class BulirschStoerVariableStepSizeIntegrator : if( errorScaleTerm > maximumFactorIncreaseForNextStepSize_ ) { this->stepSize_ = stepSize * maximumFactorIncreaseForNextStepSize_; - - if( stepSize < maximumStepSize_ && this->stepSize_ >= maximumStepSize_ ) - { - stepSize_ = maximumStepSize_; - performIntegrationStep( stepSize_ ); - } - else if( this->stepSize_ < maximumStepSize_ ) + if( this->stepSize_ >= maximumStepSize_ ) { - performIntegrationStep( stepSize_ ); + this->stepSize_ = maximumStepSize_ ; } } else diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 3549c276eb..f7ccff3bac 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -241,8 +241,8 @@ class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > const TimeType absoluteErrorTolerance = 1.0E-12, const int saveFrequency = 1, const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false, - const TimeType safetyFactorForNextStepSize = 0.75, - const TimeType maximumFactorIncreaseForNextStepSize = 4.0, + const TimeType safetyFactorForNextStepSize = 0.7, + const TimeType maximumFactorIncreaseForNextStepSize = 10.0, const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ): IntegratorSettings< TimeType >( bulirsch_stoer, initialTime, initialTimeStep, saveFrequency, assessPropagationTerminationConditionDuringIntegrationSubsteps ), From baf2c6e85a2bbebe68d62b341893057d4c79622b Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Tue, 16 Jan 2018 11:49:35 +0100 Subject: [PATCH 06/10] Removed unneed argument from ABAM settings --- Tudat/JsonInterface/Mathematics/integrator.h | 1 - Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp | 3 +-- .../NumericalIntegrators/createNumericalIntegrator.h | 3 +-- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/Tudat/JsonInterface/Mathematics/integrator.h b/Tudat/JsonInterface/Mathematics/integrator.h index f1b026f3c2..2aa62b27fc 100644 --- a/Tudat/JsonInterface/Mathematics/integrator.h +++ b/Tudat/JsonInterface/Mathematics/integrator.h @@ -195,7 +195,6 @@ void from_json( const nlohmann::json& jsonObject, boost::shared_ptr< IntegratorS integratorType, 0.0, 0.0, 0.0, 0.0 ); integratorSettings = boost::make_shared< AdamsBashforthMoultonSettings< TimeType > >( - integratorType, initialTime, getValue< TimeType >( jsonObject, K::initialStepSize ), getValue< TimeType >( jsonObject, K::minimumStepSize ), diff --git a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp index 86a20b909a..dff2ebdded 100644 --- a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp +++ b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp @@ -153,8 +153,7 @@ BOOST_AUTO_TEST_CASE( test_json_integrator_adamsBashforthMoulton ) const double absoluteErrorTolerance = 1.0E-2; const double bandwidth = 200; const boost::shared_ptr< IntegratorSettings< double > > manualSettings = - boost::make_shared< AdamsBashforthMoultonSettings< double > >( integratorType, - initialTime, + boost::make_shared< AdamsBashforthMoultonSettings< double > >( initialTime, initialStepSize, minimumStepSize, maximumStepSize, diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index bc1bb302e9..17d2040dd6 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -232,7 +232,6 @@ class AdamsBashforthMoultonSettings: public IntegratorSettings< TimeType > * \param bandwidth maximum error factor for doubling the stepsize (default: 200) */ AdamsBashforthMoultonSettings( - const AvailableIntegrators integratorType, const TimeType initialTime, const TimeType initialTimeStep, const TimeType minimumStepSize, const TimeType maximumStepSize, @@ -241,7 +240,7 @@ class AdamsBashforthMoultonSettings: public IntegratorSettings< TimeType > const int saveFrequency = 1, const bool assessPropagationTerminationConditionDuringIntegrationSubsteps = false, const TimeType bandwidth = 200. ): - IntegratorSettings< TimeType >( integratorType, initialTime, initialTimeStep, saveFrequency, + IntegratorSettings< TimeType >( adamsBashforthMoulton, initialTime, initialTimeStep, saveFrequency, assessPropagationTerminationConditionDuringIntegrationSubsteps ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), relativeErrorTolerance_( relativeErrorTolerance ), absoluteErrorTolerance_( absoluteErrorTolerance ), From 4fbbf06a7eae6a978b27a0fbc39468806ee653ba Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 24 Jan 2018 10:24:21 +0100 Subject: [PATCH 07/10] Removing old files, update to BS integrator test --- .../convertMeanToEccentricAnomalies.h | 1 - .../NumericalIntegrators/CMakeLists.txt | 2 +- ...ulirschStoerVariableStepSizeIntegrator.cpp | 364 +++++------- .../adamsBashforthMoultonIntegrator.h | 14 +- .../bulirschStoerVariableStepsizeIntegrator.h | 26 +- ...rVariableStepsizeVariableOrderIntegrator.h | 533 ------------------ ...iableStepsizeVariableOrderIntegrator_old.h | 526 ----------------- 7 files changed, 161 insertions(+), 1305 deletions(-) delete mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h delete mode 100644 Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h diff --git a/Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h b/Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h index 446abdc5ba..4c2357e004 100644 --- a/Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h +++ b/Tudat/Astrodynamics/BasicAstrodynamics/convertMeanToEccentricAnomalies.h @@ -271,7 +271,6 @@ ScalarType convertMeanAnomalyToEccentricAnomaly( eccentricAnomaly = bisectionRootfinder->execute( rootFunction, initialGuess ); } } - // Eccentricity is invalid: eccentricity < 0.0 or eccentricity >= 1.0. else { diff --git a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt index 4e530db537..0bb49d42aa 100755 --- a/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt +++ b/Tudat/Mathematics/NumericalIntegrators/CMakeLists.txt @@ -78,6 +78,6 @@ setup_custom_test_program(test_RungeKutta87DormandPrinceIntegrator "${SRCROOT}${ target_link_libraries(test_RungeKutta87DormandPrinceIntegrator tudat_numerical_integrators tudat_input_output ${Boost_LIBRARIES}) add_executable(test_BulirschStoerVariableStepSizeIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp") - setup_custom_test_program(test_BulirschStoerVariableStepSizeIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators") +setup_custom_test_program(test_BulirschStoerVariableStepSizeIntegrator "${SRCROOT}${MATHEMATICSDIR}/NumericalIntegrators") target_link_libraries(test_BulirschStoerVariableStepSizeIntegrator tudat_numerical_integrators tudat_input_output ${Boost_LIBRARIES}) diff --git a/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp b/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp index 0f2f81afff..79b6401304 100644 --- a/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp +++ b/Tudat/Mathematics/NumericalIntegrators/UnitTests/unitTestBulirschStoerVariableStepSizeIntegrator.cpp @@ -9,266 +9,162 @@ * * References * Burden, R.L., Faires, J.D. Numerical Analysis, 7th Edition, Books/Cole, 2001. - * The MathWorks, Inc. Symbolic Math Toolbox, 2012. + * Montenbruck, O., Gill, E. Satellite Orbits: Models, Methods, Applications, Springer, 2005. + * The MathWorks, Inc. RKF54b, Symbolic Math Toolbox, 2012. + * + * Notes + * For the tests using data from the Symbolic Math Toolbox (MathWorks, 2012), the single step + * and full integration error tolerances were picked to be as small as possible, without + * causing the tests to fail. These values are not deemed to indicate any bugs in the code; + * however, it is important to take these discrepancies into account when using this numerical + * integrator. * */ #define BOOST_TEST_MAIN -#include - #include +#include -#include +#include -#include -#include +#include #include "Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h" +#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaVariableStepSizeIntegrator.h" +#include "Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h" +#include "Tudat/Mathematics/NumericalIntegrators/UnitTests/numericalIntegratorTestFunctions.h" namespace tudat { namespace unit_tests { -const double TUDAT_MACHINE_PRECISION = std::numeric_limits< double >::epsilon( ); - -BOOST_AUTO_TEST_SUITE( test_bulirsch_stoer_variable_step_size_integrator ) - -using namespace tudat::numerical_integrators; - -//! Test the validity of the BurlischStoerVariableStepSize integrator. -/*! - * Tests the validity of the RungeKuttaVariableStepSize integrator. - * \param sequence The rational function sequence used for the Bulirsch-Stoer integrator being - * tested. - * \param stateDerivativeFunction Function pointer to the state derivative function. - * \param intervalStart The start of the integration interval. - * \param intervalEnd The end of the integration interval. - * \param stepSize The step size to take. - * \param initialState The initial state. - * \param expectedState Expected final state. - * \param tolerance Tolerance when comparing. - * \return True if actual final state equals the expected final state within the specified - * tolerance. - */ -bool testValidityOfBulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int > sequence, - const BulirschStoerVariableStepSizeIntegratorXd::StateDerivativeFunction& - stateDerivativeFunction, - const double intervalStart, const double intervalEnd, const double stepSize, - const Eigen::VectorXd& initialState, const Eigen::VectorXd expectedState, - const double tolerance ) -{ - // Create forward BulirschStoerVariableStepSize integrator. - { - std::cout<<"T1"< - 10.0 * TUDAT_MACHINE_PRECISION ) - { - return false; - } - - // Compute differences between computed and expected results and generate - // cerr statement if test fails. - if ( !expectedState.isApprox( finalState, tolerance ) ) - { - return false; - } - } - std::cout<<"T1"< TUDAT_MACHINE_PRECISION ) - { - return false; - } - - std::cout<<"T2"< - 10.0 * TUDAT_MACHINE_PRECISION ) - { - return false; - } - - // Compute differences between computed and expected results and generate - // cerr statement if test fails. - if ( !expectedState.isApprox( finalState, tolerance ) ) - { - return false; - } - - integrator.performIntegrationStep( stepSize ); - if ( !integrator.rollbackToPreviousState( ) ) - { - return false; - } - - // No need to check machine precision, because this interval is stored exact. - if ( std::fabs( integrator.getCurrentIndependentVariable( ) - intervalEnd ) / intervalEnd > - 10.0 * TUDAT_MACHINE_PRECISION ) - { - return false; - } - - // This result should be exactly the same. - if ( integrator.getCurrentState( ) != finalState ) - { - return false; - } - - if ( integrator.rollbackToPreviousState( ) ) - { - return false; - } - } - - return true; -} +BOOST_AUTO_TEST_SUITE( test_bulirsch_stoer_integrator ) -//! Test different types of states and state derivatives. -/*! - * Tests if different types of states and state derivatives work. If something - * is broken, then a compile time error will be generated. - * \return Unconditionally true - */ -bool testDifferentStateAndStateDerivativeTypes( ) +using numerical_integrator_test_functions::computeNonAutonomousModelStateDerivative; +using numerical_integrator_test_functions::computeVanDerPolStateDerivative; +using numerical_integrator_test_functions::computeFehlbergLogirithmicTestODEStateDerivative; +using numerical_integrator_test_functions::computeAnalyticalStateFehlbergODE; + +using namespace numerical_integrators; + +//! Test Compare with Runge Kutta 78 +BOOST_AUTO_TEST_CASE( test_BulirschStoer_Integrator_Compare78 ) { - using tudat::unit_tests::numerical_integrator_test_functions::computeZeroStateDerivative; - tudat::numerical_integrators::BulirschStoerVariableStepSizeIntegrator - < double, Eigen::Vector3d, Eigen::VectorXd > integrator( - getBulirschStoerStepSequence( ), &computeZeroStateDerivative, - 0.0, Eigen::Vector3d::Zero( ) ); + // Setup integrator + RungeKuttaCoefficients coeff_rk78 = + RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78); - integrator.integrateTo( 0.0, 0.1 ); + // Integrator settings + double minimumStepSize = std::numeric_limits< double >::epsilon( ); + double maximumStepSize = std::numeric_limits< double >::infinity( ); + double initialStepSize = 1E-4; // Don't make this too small + Eigen::VectorXd relativeTolerance(1); + relativeTolerance << 1E-12; + Eigen::VectorXd absoluteTolerance = relativeTolerance; - // No need to test anything, this is just to check compile time errors. - return true; + // Initial conditions + double initialTime = 0.5; + Eigen::VectorXd initialState( 1 ); + initialState << 0.5; // 1 large error + + + // Setup integrator + BulirschStoerVariableStepSizeIntegratorXd integrator_bs( + getBulirschStoerStepSequence( bulirsch_stoer_sequence, 4 ), + computeNonAutonomousModelStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + RungeKuttaVariableStepSizeIntegratorXd integrator_rk78( + coeff_rk78, computeNonAutonomousModelStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + double endTime = 1.5; + Eigen::VectorXd solution_bs = integrator_bs.integrateTo( endTime, initialStepSize ); + Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo( endTime, initialStepSize ); + + Eigen::VectorXd difference = solution_bs - solution_rk78; + + BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-11 ); } -//! Test the validity of the BulirschStoerVariableStepSize integrator. -/*! - * Tests the validity of the BulirschStoerVariableStepSize integrator. - * \param sequence Bulirsch-Stoer, variable-step size rational function sequence. - * \return True if actual final state equals the expected final state within the specified - * tolerance. - */ -bool testValidityOfBulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int >& sequence ) +//! Test Compare with Runge Kutta 78 +BOOST_AUTO_TEST_CASE( test_BulirschStoer_Integrator_Compare78_v2 ) { - // Test result initialised to false. - bool testBulirschStoerVariableStepSizeIsOk = true; - - using namespace tudat::unit_tests; -// std::map< BenchmarkFunctions, BenchmarkFunction >& benchmarkFunctions = -// getBenchmarkFunctions( ); - -// // Case 1: test with x_dot = 0, which results in x_f = x_0. -// { -// testBulirschStoerVariableStepSizeIsOk -// &= testValidityOfBulirschStoerVariableStepSizeIntegrator( -// sequence, -// &unit_tests::numerical_integrator_test_functions::computeZeroStateDerivative, -// 0.0, -// 100.0, 0.2, -// Eigen::Vector3d::Zero( ), -// Eigen::Vector3d::Zero( ), -// TUDAT_MACHINE_PRECISION ); -// } - -// // Case 2: test with x_dot = 1, which results in x_f = x_0 + t_f. -// { -// std::cout<<"test 2"<::epsilon( ); + double maximumStepSize = std::numeric_limits< double >::infinity( ); + double initialStepSize = 1.0; // Don't make this too small + double relativeTolerance = 1E-10; + double absoluteTolerance = 1E-10; + + // Initial conditions + double initialTime = 0.2; + Eigen::VectorXd initialState( 1 ); + initialState << -1.0; + + // Setup integrator + BulirschStoerVariableStepSizeIntegratorXd integrator_bs( + getBulirschStoerStepSequence( bulirsch_stoer_sequence, 4 ), + computeNonAutonomousModelStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + RungeKuttaVariableStepSizeIntegratorXd integrator_rk78( + coeff_rk78, computeNonAutonomousModelStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + double endTime = 2.0; + Eigen::VectorXd solution_bs = integrator_bs.integrateTo( endTime, initialStepSize ); + Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo( endTime, initialStepSize ); + + Eigen::VectorXd difference = solution_bs - solution_rk78; + + BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 1E-8 ); } -BOOST_AUTO_TEST_CASE( testBulirschStoerVariableStepSizeIntegrator ) +//! Test Compare with Runge Kutta 78 +BOOST_AUTO_TEST_CASE( test_BulirschStoer_Integrator_Compare78_VanDerPol ) { - // Case 1: test if difference in type between state and state derivative works. - //BOOST_CHECK( testDifferentStateAndStateDerivativeTypes( ) ); - - // Case 2: test if Bulirsch-Stoer sequence works. - BOOST_CHECK( testValidityOfBulirschStoerVariableStepSizeIntegrator( - getBulirschStoerStepSequence( ) ) ); - -// // Case 2: test if Deufelhard sequence works. -// BOOST_CHECK( testValidityOfBulirschStoerVariableStepSizeIntegrator( -// BulirschStoerRationalFunctionSequences::get( -// BulirschStoerRationalFunctionSequences::deufelhard ) ) ); + // Setup integrator + RungeKuttaCoefficients coeff_rk78 = + RungeKuttaCoefficients::get( RungeKuttaCoefficients::rungeKuttaFehlberg78 ); + + // Integrator settings + double minimumStepSize = std::numeric_limits< double >::epsilon( ); + double maximumStepSize = std::numeric_limits< double >::infinity( ); + double initialStepSize = 1; // Don't make this too small + double relativeTolerance = 1E-15; + double absoluteTolerance = 1E-15; + + // Initial conditions + double initialTime = 0.2; + Eigen::VectorXd initialState( 2 ); + initialState << -1.0, 1.0; + + // Setup integrator + BulirschStoerVariableStepSizeIntegratorXd integrator_bs( + getBulirschStoerStepSequence( bulirsch_stoer_sequence, 4 ), + computeVanDerPolStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + RungeKuttaVariableStepSizeIntegratorXd integrator_rk78( + coeff_rk78, computeVanDerPolStateDerivative, initialTime, initialState, + minimumStepSize, maximumStepSize, relativeTolerance, absoluteTolerance ); + + double endTime = 1.4; + Eigen::VectorXd solution_bs = integrator_bs.integrateTo( endTime,initialStepSize ); + Eigen::VectorXd solution_rk78 = integrator_rk78.integrateTo( endTime,initialStepSize ); + + Eigen::VectorXd difference = solution_rk78 - solution_bs; + + BOOST_CHECK_SMALL( std::fabs( difference( 0 ) ), 5E-12 ); + BOOST_CHECK_SMALL( std::fabs( difference( 1 ) ), 5E-12 ); } BOOST_AUTO_TEST_SUITE_END( ) diff --git a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h index 7aa9ec3a1a..93bbbaaf97 100644 --- a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h @@ -321,7 +321,7 @@ class AdamsBashforthMoultonIntegrator // isn't fixed and will not become too small, then halve the // stepsize. if ( errorTooLarge( predictorAbsoluteError, predictorRelativeError ) - && stepSize_ / 2.0 > minimumStepSize_ && !fixedStepSize_ ) { + && std::fabs( stepSize_ / 2.0 )> minimumStepSize_ && !fixedStepSize_ ) { // Set up new data for halving std::deque< StateType > tempStateHistory; @@ -385,7 +385,7 @@ class AdamsBashforthMoultonIntegrator // then double the stepsize. if ( errorTooSmall( predictorAbsoluteError, predictorRelativeError ) && sizeDerivativeHistory >= 2 * order_ - && stepSize_ * 2.0 <= maximumStepSize_ && !fixedStepSize_ ) { + && std::fabs( stepSize_ * 2.0 ) <= maximumStepSize_ && !fixedStepSize_ ) { // Predict error after doubling, to prevent error from becoming too big // This prevents fluttering and throwing away states from the history that will be @@ -576,6 +576,16 @@ class AdamsBashforthMoultonIntegrator void setMaximumOrder( unsigned int maximumOrder ){ maximumOrder_ = maximumOrder; } + IndependentVariableType getPreviousIndependentVariable( ) + { + return lastIndependentVariable_; + } + + StateType getPreviousState( ) + { + return lastState_; + } + protected: diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index a0bddc4f9b..3f2c2e853e 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -114,12 +114,12 @@ class BulirschStoerVariableStepSizeIntegrator : isMinimumStepSizeViolated_( false ) { maximumStepIndex_ = sequence_.size( ) - 1; - subSteps_.resize( maximumStepIndex_ ); + subSteps_.resize( maximumStepIndex_ + 1 ); - integratedStates_.resize( maximumStepIndex_ ); - for( unsigned int i = 0; i < maximumStepIndex_; i++ ) + integratedStates_.resize( maximumStepIndex_ + 1 ); + for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) { - integratedStates_[ i ].resize( maximumStepIndex_ ); + integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); } } @@ -285,14 +285,14 @@ class BulirschStoerVariableStepSizeIntegrator : { if( safetyFactorForNextStepSize_ * errorScaleTerm < minimumFactorDecreaseForNextStepSize_ ) { - this->stepSize_ = minimumFactorDecreaseForNextStepSize_ * errorScaleTerm; + this->stepSize_ = stepSize * minimumFactorDecreaseForNextStepSize_ * errorScaleTerm; } else { this->stepSize_ = stepSize * safetyFactorForNextStepSize_ * errorScaleTerm; } - if( stepSize_ < minimumStepSize_ ) + if( std::fabs( stepSize_ ) < std::fabs( minimumStepSize_ ) ) { isMinimumStepSizeViolated_ = true; throw std::runtime_error( "Error in BS integrator, minimum step size exceeded" ); @@ -304,9 +304,9 @@ class BulirschStoerVariableStepSizeIntegrator : if( errorScaleTerm > maximumFactorIncreaseForNextStepSize_ ) { this->stepSize_ = stepSize * maximumFactorIncreaseForNextStepSize_; - if( this->stepSize_ >= maximumStepSize_ ) + if( std::fabs( this->stepSize_ ) >= std::fabs( maximumStepSize_ ) ) { - this->stepSize_ = maximumStepSize_ ; + this->stepSize_ = stepSize / ( std::fabs( stepSize ) ) * maximumStepSize_ ; } } else @@ -347,6 +347,16 @@ class BulirschStoerVariableStepSizeIntegrator : */ bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } + IndependentVariableType getPreviousIndependentVariable( ) + { + return lastIndependentVariable_; + } + + StateType getPreviousState( ) + { + return lastState_; + } + private: //! Last used step size. diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h deleted file mode 100644 index 397445c34b..0000000000 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator.h +++ /dev/null @@ -1,533 +0,0 @@ -/* Copyright (c) 2010-2012 Delft University of Technology. - * - * This software is protected by national and international copyright. - * Any unauthorized use, reproduction or modification is unlawful and - * will be prosecuted. Commercial and non-private application of the - * software in any form is strictly prohibited unless otherwise granted - * by the authors. - * - * The code is provided without any warranty; without even the implied - * warranty of merchantibility or fitness for a particular purpose. - * - * Changelog - * YYMMDD Author Comment - * 120316 K. Kumar File created. - * - * References - * Press W.H., et al. Numerical Recipes in C++: The Art of - * Scientific Computing. Cambridge University Press, February 2002. - * - */ - -#ifndef TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H -#define TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H - -#include - -#include - -#include - -#include -#include - - -namespace tudat -{ -namespace numerical_integrators -{ - -enum ExtrapolationMethodStepSequences -{ - bulirsch_stoer_sequence, - deufelhard_sequence -}; - - -std::vector< unsigned int > getBulirschStoerStepSequence( - const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, - const unsigned int lengthOfSequence = 12 ); - -//! Class that implements the Bulirsch-Stoer variable stepsize integrator. -/*! - * Class that implements the Bulirsch-Stoer variable step size integrator. - * \tparam StateType The type of the state. This type should be an Eigen::Matrix derived type. - * \tparam StateDerivativeType The type of the state derivative. This type should be an - * Eigen::Matrix derived type. - * \tparam IndependentVariableType The type of the independent variable. This type should be - * either a float or double. - * \sa NumericalIntegrator. - */ -template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, - typename StateDerivativeType = Eigen::VectorXd > -class BulirschStoerVariableStepSizeIntegrator : - public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > -{ -public: - - //! Typedef of the base class. - /*! - * Typedef of the base class with all template parameters filled in. - */ - typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; - - //! Typedef to the state derivative function. - /*! - * Typedef to the state derivative function inherited from the base class. - * \sa NumericalIntegrator::StateDerivativeFunction. - */ - typedef typename Base::StateDerivativeFunction StateDerivativeFunction; - - //! Default constructor. - /*! - * Default constructor, taking sequence, a state derivative function, initial conditions, - * minimum step size and relative error tolerance per item in the state vector as argument. - * \param sequence Rational function sequence used by algorithm. - * \param stateDerivativeFunction State derivative function. - * \param intervalStart The start of the integration interval. - * \param initialState The initial state. - * \param minimumStepSize The minimum step size to take. If this constraint is violated, a - * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). - * \param relativeErrorTolerance The relative error tolerance, for each individual state - * vector element. - * \sa NumericalIntegrator::NumericalIntegrator. - */ - BulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int >& sequence, - const StateDerivativeFunction& stateDerivativeFunction, - const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize, - const StateType& relativeErrorTolerance ) : - Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), - currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), - relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ) - { - maximumStepIndex_ = sequence_.size( ) - 1; - subSteps_.resize( maximumStepIndex_ ); - - integratedStates_.resize( maximumStepIndex_ ); - for( unsigned int i = 0; i < maximumStepIndex_; i++ ) - { - integratedStates_[ i ].resize( maximumStepIndex_ ); - } - - initializeWorkParameters( ); - } - - //! Default constructor. - /*! - * Default constructor, taking coefficients, a state derivative function, initial conditions, - * minimum step size and relative error tolerance for all items in the state vector as argument. - * \param coefficients Coefficients to use with this integrator. - * \param stateDerivativeFunction State derivative function. - * \param intervalStart The start of the integration interval. - * \param initialState The initial state. - * \param minimumStepSize The minimum step size to take. If this constraint is violated, a - * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). - * \param relativeErrorTolerance The relative error tolerance, equal for all individual state - * vector elements. - * \sa NumericalIntegrator::NumericalIntegrator. - */ - BulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int >& sequence, - const StateDerivativeFunction& stateDerivativeFunction, - const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize = 1.0e-15, - const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, - const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): - Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), - currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), - relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), - relativeErrorTolerance ) ), - absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), - absoluteErrorTolerance ) ), - isMinimumStepSizeViolated_( false ) - { - maximumStepIndex_ = sequence_.size( ) - 1; - subSteps_.resize( maximumStepIndex_ + 1 ); - - integratedStates_.resize( maximumStepIndex_ + 1 ); - for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) - { - integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); - } - - initializeWorkParameters( ); - } - - //! Get step size of the next step. - /*! - * Returns the step size of the next step. - * \return Step size to be used for the next step. - */ - virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } - - //! Get current state. - /*! - * Returns the current state of the integrator. - * \return Current integrated state. - */ - virtual StateType getCurrentState( ) const { return currentState_; } - - //! Returns the current independent variable. - /*! - * Returns the current value of the independent variable of the integrator. - * \return Current independent variable. - */ - virtual IndependentVariableType getCurrentIndependentVariable( ) const - { - return currentIndependentVariable_; - } - - //! Perform a single integration step. - /*! - * Perform a single integration step and compute a new step size. - * \param stepSize The step size to take. If the time step is too large to satisfy the error - * constraints, the step is redone until the error constraint is satisfied. - * \return The state at the end of the interval. - */ - virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); - - //! Rollback internal state to the last state. - /*! - * Performs rollback of the internal state to the last state. This function can only be called - * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by - * implementations, and can not be called before any of these functions have been called. Will - * return true if the rollback was successful, and false otherwise. - * \return True if the rollback was successful. - */ - virtual bool rollbackToPreviousState( ) - { - if ( currentIndependentVariable_ == lastIndependentVariable_ ) - { - return false; - } - - currentIndependentVariable_ = lastIndependentVariable_; - currentState_ = lastState_; - return true; - } - - //! Check if minimum step size constraint was violated. - /*! - * Returns true if the minimum step size constraint has been violated since this integrator - * was constructed. - * \return True if the minimum step size constraint was violated. - */ - bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } - -protected: - - void initializeWorkParameters( ) - { - - aTerms_.resize( maximumStepIndex_ + 1 ); - aTerms_[ 0 ] = sequence_.at( 0 ) + 1; - - alphaTerms_.resize( maximumStepIndex_ + 1 ); - for( unsigned int i = 0; i <= maximumStepIndex_; i++ ) - { - alphaTerms_[ i ].resize( maximumStepIndex_ + 1 ); - - if( i > 0 ) - { - aTerms_[ i ] = sequence_.at( i ) + aTerms_[ i - 1 ]; - } - } - - for( unsigned int i = 0; i < maximumStepIndex_ ; i++ ) - { - for( unsigned int j = 0; j < maximumStepIndex_ ; j++ ) - { - alphaTerms_[ i ][ j ] = std::pow( - relativeErrorTolerance_.array( ).maxCoeff( ), static_cast< double >( aTerms_[ i + 1 ] - aTerms_[ j + 1 ] ) / - static_cast< double >( ( 2 * i - 1 ) * ( aTerms_[ j + 1 ] - aTerms_[ 0 ] + 1 ) ) ); - //std::cout<<"Computing alpha: "< sequence_; - - //! Minimum step size. - /*! - * Minimum step size. - */ - IndependentVariableType minimumStepSize_; - - //! Relative error tolerance. - /*! - * Relative error tolerance per element in the state. - */ - StateType relativeErrorTolerance_; - - StateType absoluteErrorTolerance_; - - //! Flag to indicate whether the minimum step size constraint has been violated. - /*! - * Flag to indicate whether the minimum step size constraint has been violated. - */ - bool isMinimumStepSizeViolated_; - - //! Execute mid-point method. - /*! - * Executes mid-point method, given a known state and state derivative. - * \param stateAtFirstPoint State at first point. - * \param stateAtCenterPoint State at center point. - * \param independentVariableAtFirstPoint Independent variable at first point. - * \param subStepSize Sub step size between successive states used by mid-point method. - * \param stateAtLastPoint State at last point. - */ - StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, - const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ); - - std::vector< std::vector< StateType > > integratedStates_; - - unsigned int maximumStepIndex_; - - std::vector< double > subSteps_; - - int optimalIndex_; - - std::vector< std::vector< double > > alphaTerms_; - - std::vector< int > aTerms_; - -private: -}; - -//! Perform a single integration step. -template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::performIntegrationStep( const IndependentVariableType stepSize ) -{ - //std::cout<<"Starting: "<currentIndependentVariable_<( - sequence_.at( p ) ); - } - - double errorScaleTerm = TUDAT_NAN; - int breakIndex = -1; - - for ( unsigned int i = 0; i <= maximumStepIndex_; i++ ) - { - breakIndex = i; - // Compute Euler step and set as state at center point for use with mid-point method. - stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) - * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); - - // Apply modified mid-point rule. - stateAtFirstPoint_ = currentState_; - IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; - for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) - { - stateAtLastPoint_ = executeMidPointMethod( - stateAtFirstPoint_, stateAtCenterPoint_, - independentVariableAtFirstPoint_, subSteps_.at( i ) ); - - if ( j < sequence_.at( i ) - 2 ) - { - stateAtFirstPoint_ = stateAtCenterPoint_; - stateAtCenterPoint_ = stateAtLastPoint_; - independentVariableAtFirstPoint_ += subSteps_.at( i ); - } - } - - // Apply end-point correction. - integratedStates_[ i ][ 0 ] - = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( - currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); - - for ( unsigned int k = 1; k < i + 1; k++ ) - { - integratedStates_[ i ][ k ] = - integratedStates_[ i ][ k - 1 ] + 1.0 / - ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) - * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); - } - - if( i != 1 ) - { - const StateType errorTolerance_ = - ( integratedStates_.at( i ).at( i ).array( ).abs( ) * relativeErrorTolerance_.array( ) ).matrix( ) - + absoluteErrorTolerance_; - - double maximumAllowableErrorValue = errorTolerance_.array( ).maxCoeff( ); - double maximumErrorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); - - errorScaleTerm = 0.8 * std::pow( maximumAllowableErrorValue / maximumErrorValue, - ( 1.0 / static_cast< double >( 2 * i - 1 ) ) ); - double inverseErrorScaleTerm = 1.0 / errorScaleTerm; - inverseErrorScaleTerms[ i - 1 ] = inverseErrorScaleTerm; - } - - if( i == maximumStepIndex_ && ( i >= optimalIndex_ - 1 ) ) - { - - if( maximumErrorValue < maximumAllowableErrorValue ) - { - // Accept the current step. - lastIndependentVariable_ = currentIndependentVariable_; - lastState_ = currentState_; - currentIndependentVariable_ += stepSize; - currentState_ = integratedStates_[ i ][ i ]; - stepSize_ = stepSize; - stepSuccessful = true; - break; - } - - if( ( i == maximumStepIndex_ ) || ( i == optimalIndex_ + 1 ) ) - { - std::cout<<"In A"< 0.7 ) - { - stepSizeChangeFactor = 0.7; - } - - this->stepSize_ = stepSizeChangeFactor * stepSize; - } - - firstStepDone = false; - - { - double scale = 1.0; - for( unsigned int i = 1; k < breakIndex; i++ ) - { - double factor = std::max( inverseErrorScaleTerms[ i ], 0.1 ); - double work = factor * aTerms_[ k + 1 ]; - if( work < minimumWork ) - { - scale = factor; - minimumWork = work; - optimalIndex_ = i + 1; - } - } - this->stepSize_ = this->stepSize_ / scale; - } - - if( !stepSuccessful ) - { - - performIntegrationStep( stepSize_ ); - } - else - { - if( errorScaleTerm > 4.0 ) - { - this->stepSize_ = stepSize * 4.0; - performIntegrationStep( stepSize_ ); - } - else - { - this->stepSize_ = stepSize * errorScaleTerm; - } - } - - - return currentState_; -} - -//! Execute mid-point method. -template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, - const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ) -{ - return stateAtFirstPoint + 2.0 * subStepSize - * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, - stateAtCenterPoint ); -} - -//! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, -//! independent variable = double). -/*! - * Typedef of a variable-step size Bulirsch-Stoer integrator with VectorXds as state and state - * derivative and double as independent variable. - */ -typedef BulirschStoerVariableStepSizeIntegrator< > BulirschStoerVariableStepSizeIntegratorXd; - -} // namespace numerical_integrators -} // namespace tudat - -#endif // TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h deleted file mode 100644 index 4d94560364..0000000000 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeVariableOrderIntegrator_old.h +++ /dev/null @@ -1,526 +0,0 @@ -/* Copyright (c) 2010-2012 Delft University of Technology. - * - * This software is protected by national and international copyright. - * Any unauthorized use, reproduction or modification is unlawful and - * will be prosecuted. Commercial and non-private application of the - * software in any form is strictly prohibited unless otherwise granted - * by the authors. - * - * The code is provided without any warranty; without even the implied - * warranty of merchantibility or fitness for a particular purpose. - * - * Changelog - * YYMMDD Author Comment - * 120316 K. Kumar File created. - * - * References - * Press W.H., et al. Numerical Recipes in C++: The Art of - * Scientific Computing. Cambridge University Press, February 2002. - * - */ - -#ifndef TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H -#define TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H - -#include - -#include - -#include - -#include -#include - - -namespace tudat -{ -namespace numerical_integrators -{ - -enum ExtrapolationMethodStepSequences -{ - bulirsch_stoer_sequence, - deufelhard_sequence -}; - - -std::vector< unsigned int > getBulirschStoerStepSequence( - const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, - const unsigned int lengthOfSequence = 12 ); - -//! Class that implements the Bulirsch-Stoer variable stepsize integrator. -/*! - * Class that implements the Bulirsch-Stoer variable step size integrator. - * \tparam StateType The type of the state. This type should be an Eigen::Matrix derived type. - * \tparam StateDerivativeType The type of the state derivative. This type should be an - * Eigen::Matrix derived type. - * \tparam IndependentVariableType The type of the independent variable. This type should be - * either a float or double. - * \sa NumericalIntegrator. - */ -template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, - typename StateDerivativeType = Eigen::VectorXd > -class BulirschStoerVariableStepSizeIntegrator : - public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > -{ -public: - - //! Typedef of the base class. - /*! - * Typedef of the base class with all template parameters filled in. - */ - typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; - - //! Typedef to the state derivative function. - /*! - * Typedef to the state derivative function inherited from the base class. - * \sa NumericalIntegrator::StateDerivativeFunction. - */ - typedef typename Base::StateDerivativeFunction StateDerivativeFunction; - - //! Default constructor. - /*! - * Default constructor, taking sequence, a state derivative function, initial conditions, - * minimum step size and relative error tolerance per item in the state vector as argument. - * \param sequence Rational function sequence used by algorithm. - * \param stateDerivativeFunction State derivative function. - * \param intervalStart The start of the integration interval. - * \param initialState The initial state. - * \param minimumStepSize The minimum step size to take. If this constraint is violated, a - * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). - * \param relativeErrorTolerance The relative error tolerance, for each individual state - * vector element. - * \sa NumericalIntegrator::NumericalIntegrator. - */ - BulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int >& sequence, - const StateDerivativeFunction& stateDerivativeFunction, - const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize, - const StateType& relativeErrorTolerance ) : - Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), - currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), - relativeErrorTolerance_( relativeErrorTolerance ), isMinimumStepSizeViolated_( false ), - stepSizeChanged_( true ) - { - maximumStepIndex_ = sequence_.size( ) - 1; - subSteps_.resize( maximumStepIndex_ ); - - integratedStates_.resize( maximumStepIndex_ ); - for( unsigned int i = 0; i < maximumStepIndex_; i++ ) - { - integratedStates_[ i ].resize( maximumStepIndex_ ); - } - - initializeWorkParameters( ); - } - - //! Default constructor. - /*! - * Default constructor, taking coefficients, a state derivative function, initial conditions, - * minimum step size and relative error tolerance for all items in the state vector as argument. - * \param coefficients Coefficients to use with this integrator. - * \param stateDerivativeFunction State derivative function. - * \param intervalStart The start of the integration interval. - * \param initialState The initial state. - * \param minimumStepSize The minimum step size to take. If this constraint is violated, a - * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). - * \param relativeErrorTolerance The relative error tolerance, equal for all individual state - * vector elements. - * \sa NumericalIntegrator::NumericalIntegrator. - */ - BulirschStoerVariableStepSizeIntegrator( - const std::vector< unsigned int >& sequence, - const StateDerivativeFunction& stateDerivativeFunction, - const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize = 1.0e-15, - const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, - const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12 ): - Base( stateDerivativeFunction ), currentIndependentVariable_( intervalStart ), - currentState_( initialState ), lastIndependentVariable_( intervalStart ), - sequence_( sequence ), minimumStepSize_( minimumStepSize ), - relativeErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), - relativeErrorTolerance ) ), - absoluteErrorTolerance_( StateType::Constant( initialState.rows( ), initialState.cols( ), - absoluteErrorTolerance ) ), - isMinimumStepSizeViolated_( false ), - stepSizeChanged_( true ) - { - maximumStepIndex_ = sequence_.size( ) - 1; - subSteps_.resize( maximumStepIndex_ + 1 ); - - integratedStates_.resize( maximumStepIndex_ + 1 ); - for( unsigned int i = 0; i < maximumStepIndex_ + 1 ; i++ ) - { - integratedStates_[ i ].resize( maximumStepIndex_ + 1 ); - } - - initializeWorkParameters( ); - } - - //! Get step size of the next step. - /*! - * Returns the step size of the next step. - * \return Step size to be used for the next step. - */ - virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } - - //! Get current state. - /*! - * Returns the current state of the integrator. - * \return Current integrated state. - */ - virtual StateType getCurrentState( ) const { return currentState_; } - - //! Returns the current independent variable. - /*! - * Returns the current value of the independent variable of the integrator. - * \return Current independent variable. - */ - virtual IndependentVariableType getCurrentIndependentVariable( ) const - { - return currentIndependentVariable_; - } - - //! Perform a single integration step. - /*! - * Perform a single integration step and compute a new step size. - * \param stepSize The step size to take. If the time step is too large to satisfy the error - * constraints, the step is redone until the error constraint is satisfied. - * \return The state at the end of the interval. - */ - virtual StateType performIntegrationStep( const IndependentVariableType stepSize ); - - //! Rollback internal state to the last state. - /*! - * Performs rollback of the internal state to the last state. This function can only be called - * once after calling integrateTo( ) or performIntegrationStep( ) unless specified otherwise by - * implementations, and can not be called before any of these functions have been called. Will - * return true if the rollback was successful, and false otherwise. - * \return True if the rollback was successful. - */ - virtual bool rollbackToPreviousState( ) - { - if ( currentIndependentVariable_ == lastIndependentVariable_ ) - { - return false; - } - - currentIndependentVariable_ = lastIndependentVariable_; - currentState_ = lastState_; - return true; - } - - //! Check if minimum step size constraint was violated. - /*! - * Returns true if the minimum step size constraint has been violated since this integrator - * was constructed. - * \return True if the minimum step size constraint was violated. - */ - bool isMinimumStepSizeViolated( ) const { return isMinimumStepSizeViolated_; } - -protected: - - //! Last used step size. - /*! - * Last used step size, passed to either integrateTo( ) or performIntegrationStep( ). - */ - IndependentVariableType stepSize_; - - //! Current independent variable. - /*! - * Current independent variable as computed by performIntegrationStep(). - */ - IndependentVariableType currentIndependentVariable_; - - //! Current state. - /*! - * Current state as computed by performIntegrationStep( ). - */ - StateType currentState_; - - //! Last independent variable. - /*! - * Last independent variable value as computed by performIntegrationStep(). - */ - IndependentVariableType lastIndependentVariable_; - - //! Last state. - /*! - * Last state as computed by performIntegrationStep( ). - */ - StateType lastState_; - - //! Sequence for the integrator. - /*! - * Rational function sequence for the integrator. - */ - std::vector< unsigned int > sequence_; - - //! Minimum step size. - /*! - * Minimum step size. - */ - IndependentVariableType minimumStepSize_; - - //! Relative error tolerance. - /*! - * Relative error tolerance per element in the state. - */ - StateType relativeErrorTolerance_; - - StateType absoluteErrorTolerance_; - - //! Flag to indicate whether the minimum step size constraint has been violated. - /*! - * Flag to indicate whether the minimum step size constraint has been violated. - */ - bool isMinimumStepSizeViolated_; - - //! Execute mid-point method. - /*! - * Executes mid-point method, given a known state and state derivative. - * \param stateAtFirstPoint State at first point. - * \param stateAtCenterPoint State at center point. - * \param independentVariableAtFirstPoint Independent variable at first point. - * \param subStepSize Sub step size between successive states used by mid-point method. - * \param stateAtLastPoint State at last point. - */ - StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, - const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ); - - void initializeWorkParameters( ) - { - - aTerms_.resize( maximumStepIndex_ + 1 ); - aTerms_[ 0 ] = sequence_.at( 0 ) + 1; - - alphaTerms_.resize( maximumStepIndex_ + 1 ); - for( unsigned int i = 0; i <= maximumStepIndex_; i++ ) - { - alphaTerms_[ i ].resize( maximumStepIndex_ + 1 ); - - if( i > 0 ) - { - aTerms_[ i ] = sequence_.at( i ) + aTerms_[ i - 1 ]; - } - } - - for( unsigned int i = 0; i < maximumStepIndex_ ; i++ ) - { - for( unsigned int j = 0; j < maximumStepIndex_ ; j++ ) - { - alphaTerms_[ i ][ j ] = std::pow( - relativeErrorTolerance_.array( ).maxCoeff( ), static_cast< double >( aTerms_[ i + 1 ] - aTerms_[ j + 1 ] ) / - static_cast< double >( ( 2 * i - 1 ) * ( aTerms_[ j + 1 ] - aTerms_[ 0 ] + 1 ) ) ); - std::cout<<"Computing alpha: "< > integratedStates_; - - unsigned int maximumStepIndex_; - - unsigned int optimalOrder_; - - std::vector< double > subSteps_; - - bool stepSizeChanged_; - - std::vector< std::vector< double > > alphaTerms_; - - std::vector< int > aTerms_; - -private: -}; - -//! Perform a single integration step. -template < typename IndependentVariableType, typename StateType, typename StateDerivativeType > -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::performIntegrationStep( const IndependentVariableType stepSize ) -{ - //std::cout<<"Starting: "<currentIndependentVariable_<( - sequence_.at( p ) ); - } - - if( stepSizeChanged_ ) - { - optimalOrder_ = maximumStepIndex_; - } - - double stepSizeChangeFactor = TUDAT_NAN; - - for ( unsigned int i = 0; i <= maximumStepIndex_; i++ ) - { - // Compute Euler step and set as state at center point for use with mid-point method. - stateAtCenterPoint_ = currentState_ + subSteps_.at( i ) - * this->stateDerivativeFunction_( currentIndependentVariable_, currentState_ ); - - // Apply modified mid-point rule. - stateAtFirstPoint_ = currentState_; - IndependentVariableType independentVariableAtFirstPoint_ = currentIndependentVariable_; - for ( unsigned int j = 0; j < sequence_.at( i ) - 1; j++ ) - { - stateAtLastPoint_ = executeMidPointMethod( - stateAtFirstPoint_, stateAtCenterPoint_, - independentVariableAtFirstPoint_, subSteps_.at( i ) ); - - if ( j < sequence_.at( i ) - 2 ) - { - stateAtFirstPoint_ = stateAtCenterPoint_; - stateAtCenterPoint_ = stateAtLastPoint_; - independentVariableAtFirstPoint_ += subSteps_.at( i ); - } - } - - // Apply end-point correction. - integratedStates_[ i ][ 0 ] - = 0.5 * ( stateAtLastPoint_ + stateAtCenterPoint_+ subSteps_.at( i ) * this->stateDerivativeFunction_( - currentIndependentVariable_ + stepSize, stateAtLastPoint_ ) ); - - for ( unsigned int k = 1; k < i + 1; k++ ) - { - integratedStates_[ i ][ k ] = - integratedStates_[ i ][ k - 1 ] + 1.0 / - ( pow( subSteps_.at( i - k ), 2.0 ) / std::pow( subSteps_.at( i ), 2.0 ) - 1.0 ) - * ( integratedStates_[ i ][ k - 1 ] - integratedStates_[ i - 1 ][ k - 1 ] ); - } - - if ( ( i > 1 ) )//&& ( ( i >= optimalOrder_ - 1 ) || stepSizeChanged_ ) ) - { - const StateType errorTolerance_ = - ( integratedStates_.at( i ).at( i ).array( ).abs( ) * relativeErrorTolerance_.array( ) ).matrix( ) - + absoluteErrorTolerance_; - - double maximumErrorValue = errorTolerance_.array( ).maxCoeff( ); - double errorValue = ( integratedStates_.at( i ).at( i ) - integratedStates_.at( i ).at( i - 1 ) ).array( ).abs( ).maxCoeff( ); - - double errorScaleTerm = std::pow( errorValue / maximumErrorValue, - static_cast< double >( 1 / ( 2 * i - 1 ) ) ); - - //std::cout<<"Scaling errors: "< -StateType -BulirschStoerVariableStepSizeIntegrator< IndependentVariableType, StateType, StateDerivativeType > -::executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, - const IndependentVariableType independentVariableAtFirstPoint, - const IndependentVariableType subStepSize ) -{ - return stateAtFirstPoint + 2.0 * subStepSize - * this->stateDerivativeFunction_( independentVariableAtFirstPoint + subStepSize, - stateAtCenterPoint ); -} - -//! Typedef of variable-step size Bulirsch-Stoer integrator (state/state derivative = VectorXd, -//! independent variable = double). -/*! - * Typedef of a variable-step size Bulirsch-Stoer integrator with VectorXds as state and state - * derivative and double as independent variable. - */ -typedef BulirschStoerVariableStepSizeIntegrator< > BulirschStoerVariableStepSizeIntegratorXd; - -} // namespace numerical_integrators -} // namespace tudat - -#endif // TUDAT_BULIRSCH_STOER_VARIABLE_STEP_SIZE_INTEGRATOR_H From bdcc371b17ae41b8617a44ffc2de749f37530150 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 24 Jan 2018 12:23:34 +0100 Subject: [PATCH 08/10] Updated JSON with BS integrator; update BS and ABAM integrators to be compatible with high precision time variables --- Tudat/JsonInterface/Mathematics/integrator.h | 46 ++++++++++++++++++- Tudat/JsonInterface/Support/keys.cpp | 2 + Tudat/JsonInterface/Support/keys.h | 2 + .../unitTestIntegrator/bulirschStoer.json | 10 ++++ .../inputs/unitTestIntegrator/types.json | 3 +- .../UnitTests/unitTestIntegrator.cpp | 36 +++++++++++++++ .../adamsBashforthMoultonIntegrator.h | 2 +- .../bulirschStoerVariableStepsizeIntegrator.h | 22 ++++----- .../createNumericalIntegrator.h | 8 ++-- .../variationalEquationsSolver.h | 4 +- 10 files changed, 114 insertions(+), 21 deletions(-) create mode 100644 Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/bulirschStoer.json diff --git a/Tudat/JsonInterface/Mathematics/integrator.h b/Tudat/JsonInterface/Mathematics/integrator.h index 2aa62b27fc..649637802d 100644 --- a/Tudat/JsonInterface/Mathematics/integrator.h +++ b/Tudat/JsonInterface/Mathematics/integrator.h @@ -27,7 +27,8 @@ static std::map< AvailableIntegrators, std::string > integratorTypes = { rungeKutta4, "rungeKutta4" }, { euler, "euler" }, { rungeKuttaVariableStepSize, "rungeKuttaVariableStepSize" }, - { adamsBashforthMoulton, "adamsBashforthMoulton" } + { adamsBashforthMoulton, "adamsBashforthMoulton" }, + { bulirschStoer, "bulirschStoer" }, }; //! `AvailableIntegrators` not supported by `json_interface`. @@ -130,6 +131,21 @@ void to_json( nlohmann::json& jsonObject, const boost::shared_ptr< IntegratorSet jsonObject[ K::bandwidth ] = adamsBashforthMoultonSettings->bandwidth_; return; } + case bulirschStoer: + { + boost::shared_ptr< BulirschStoerIntegratorSettings< TimeType > > bulirschStoerSettings = + boost::dynamic_pointer_cast< BulirschStoerIntegratorSettings< TimeType > >( integratorSettings ); + assertNonNullPointer( bulirschStoerSettings ); + jsonObject[ K::initialStepSize ] = bulirschStoerSettings->initialTimeStep_; + jsonObject[ K::minimumStepSize ] = bulirschStoerSettings->minimumStepSize_; + jsonObject[ K::maximumStepSize ] = bulirschStoerSettings->maximumStepSize_; + jsonObject[ K::relativeErrorTolerance ] = bulirschStoerSettings->relativeErrorTolerance_; + jsonObject[ K::absoluteErrorTolerance ] = bulirschStoerSettings->absoluteErrorTolerance_; + jsonObject[ K::extrapolationSequence ] = bulirschStoerSettings->extrapolationSequence_; + jsonObject[ K::maximumNumberOfSteps ] = bulirschStoerSettings->maximumNumberOfSteps_; + + return; + } default: handleUnimplementedEnumValue( integratorType, integratorTypes, unsupportedIntegratorTypes ); } @@ -192,7 +208,7 @@ void from_json( const nlohmann::json& jsonObject, boost::shared_ptr< IntegratorS case adamsBashforthMoulton: { AdamsBashforthMoultonSettings< TimeType > defaults( - integratorType, 0.0, 0.0, 0.0, 0.0 ); + 0.0, 0.0, 0.0, 0.0 ); integratorSettings = boost::make_shared< AdamsBashforthMoultonSettings< TimeType > >( initialTime, @@ -208,6 +224,32 @@ void from_json( const nlohmann::json& jsonObject, boost::shared_ptr< IntegratorS defaults.bandwidth_ ) ); return; } + case bulirschStoer: + { + BulirschStoerIntegratorSettings< TimeType > defaults( + 0.0, 0.0, bulirsch_stoer_sequence, 6, std::numeric_limits< limits >::epsilon( ), + std::numeric_limits< limits >::infinity( ) ); + + integratorSettings = boost::make_shared< BulirschStoerIntegratorSettings< TimeType > >( + initialTime, + getValue< TimeType >( jsonObject, K::initialStepSize ), + getValue( jsonObject, K::extrapolationSequence, defaults.extrapolationSequence_ ), + getValue( jsonObject, K::maximumNumberOfSteps, defaults.maximumNumberOfSteps_ ), + getValue< TimeType >( jsonObject, K::minimumStepSize ), + getValue< TimeType >( jsonObject, K::maximumStepSize ), + getValue( jsonObject, K::relativeErrorTolerance, defaults.relativeErrorTolerance_ ), + getValue( jsonObject, K::absoluteErrorTolerance, defaults.absoluteErrorTolerance_ ), + getValue( jsonObject, K::saveFrequency, defaults.saveFrequency_ ), + getValue( jsonObject, K::assessPropagationTerminationConditionDuringIntegrationSubsteps, + defaults.assessPropagationTerminationConditionDuringIntegrationSubsteps_ ), + getValue( jsonObject, K::safetyFactorForNextStepSize, + defaults.safetyFactorForNextStepSize_ ), + getValue( jsonObject, K::maximumFactorIncreaseForNextStepSize, + defaults.maximumFactorIncreaseForNextStepSize_ ), + getValue( jsonObject, K::minimumFactorDecreaseForNextStepSize, + defaults.minimumFactorDecreaseForNextStepSize_ ) ); + return; + } default: handleUnimplementedEnumValue( integratorType, integratorTypes, unsupportedIntegratorTypes ); } diff --git a/Tudat/JsonInterface/Support/keys.cpp b/Tudat/JsonInterface/Support/keys.cpp index db828acb63..eb9d43c72b 100644 --- a/Tudat/JsonInterface/Support/keys.cpp +++ b/Tudat/JsonInterface/Support/keys.cpp @@ -330,6 +330,8 @@ const std::string Keys::Integrator::safetyFactorForNextStepSize = "safetyFactorF const std::string Keys::Integrator::maximumFactorIncreaseForNextStepSize = "maximumFactorIncreaseForNextStepSize"; const std::string Keys::Integrator::minimumFactorDecreaseForNextStepSize = "minimumFactorDecreaseForNextStepSize"; const std::string Keys::Integrator::bandwidth = "bandwidth"; +const std::string Keys::Integrator::extrapolationSequence = "extrapolationSequence"; +const std::string Keys::Integrator::maximumNumberOfSteps = "maximumNumberOfSteps"; // Interpolation diff --git a/Tudat/JsonInterface/Support/keys.h b/Tudat/JsonInterface/Support/keys.h index 3c55fdcdbc..8241af8b5f 100644 --- a/Tudat/JsonInterface/Support/keys.h +++ b/Tudat/JsonInterface/Support/keys.h @@ -351,6 +351,8 @@ struct Keys static const std::string maximumFactorIncreaseForNextStepSize; static const std::string minimumFactorDecreaseForNextStepSize; static const std::string bandwidth; + static const std::string extrapolationSequence; + static const std::string maximumNumberOfSteps; }; struct Interpolation diff --git a/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/bulirschStoer.json b/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/bulirschStoer.json new file mode 100644 index 0000000000..b491fa369e --- /dev/null +++ b/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/bulirschStoer.json @@ -0,0 +1,10 @@ +{ + "initialStepSize": 1.4, + "minimumStepSize": 0.4, + "maximumStepSize": 2.4, + "maximumNumberOfSteps": 8, + "relativeErrorTolerance": 0.0001, + "absoluteErrorTolerance": 0.01, + "type": "bulirschStoer", + "initialTime": -0.3 +} diff --git a/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/types.json b/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/types.json index ce1582772e..44abfbf5d5 100644 --- a/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/types.json +++ b/Tudat/JsonInterface/UnitTests/inputs/unitTestIntegrator/types.json @@ -2,5 +2,6 @@ "euler", "rungeKutta4", "rungeKuttaVariableStepSize", - "adamsBashforthMoulton" + "adamsBashforthMoulton", + "bulirschStoer" ] diff --git a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp index e4f22dc1b6..f1b7c81b19 100644 --- a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp +++ b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp @@ -171,6 +171,42 @@ BOOST_AUTO_TEST_CASE( test_json_integrator_adamsBashforthMoulton ) BOOST_CHECK_EQUAL_JSON( fromFileSettings, manualSettings ); } +// Test 6: adamsBashforthMoulton +BOOST_AUTO_TEST_CASE( test_json_integrator_bulirschStoer ) +{ + using namespace tudat::numerical_integrators; + using namespace tudat::json_interface; + + // Create IntegratorSettings from JSON file + const boost::shared_ptr< IntegratorSettings< double > > fromFileSettings = + parseJSONFile< boost::shared_ptr< IntegratorSettings< double > > >( INPUT( "bulirschStoer" ) ); + + // Create IntegratorSettings manually + const double initialTime = -0.3; + const double initialStepSize = 1.4; + const double minimumStepSize = 0.4; + const double maximumStepSize = 2.4; + const double relativeErrorTolerance = 1.0E-4; + const double absoluteErrorTolerance = 1.0E-2; + const int maximumNumberOfSteps = 8; + + const boost::shared_ptr< IntegratorSettings< double > > manualSettings = + boost::make_shared< BulirschStoerIntegratorSettings( initialTime, + initialStepSize, + minimumStepSize, + maximumStepSize, + relativeErrorTolerance, + absoluteErrorTolerance, + minimumOrder, + maximumOrder, + 1, + false, + bandwidth ); + + // Compare + BOOST_CHECK_EQUAL_JSON( fromFileSettings, manualSettings ); +} + BOOST_AUTO_TEST_SUITE_END( ) } // namespace unit_tests diff --git a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h index 93bbbaaf97..c4ecc2ff5a 100644 --- a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h @@ -933,7 +933,7 @@ class AdamsBashforthMoultonIntegrator /*! * Last used step size, passed to either integrateTo( ) or performIntegrationStep( ). */ - IndependentVariableType stepSize_; + TimeStepType stepSize_; //! Predicted derivative state. /*! diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index 3f2c2e853e..883ea2ecd7 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -61,7 +61,7 @@ std::vector< unsigned int > getBulirschStoerStepSequence( template < typename IndependentVariableType = double, typename StateType = Eigen::VectorXd, typename StateDerivativeType = Eigen::VectorXd, typename TimeStepType = double > class BulirschStoerVariableStepSizeIntegrator : - public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > + public NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType, TimeStepType > { public: @@ -69,7 +69,7 @@ class BulirschStoerVariableStepSizeIntegrator : /*! * Typedef of the base class with all template parameters filled in. */ - typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType > Base; + typedef NumericalIntegrator< IndependentVariableType, StateType, StateDerivativeType, TimeStepType > Base; //! Typedef to the state derivative function. /*! @@ -96,8 +96,8 @@ class BulirschStoerVariableStepSizeIntegrator : const std::vector< unsigned int >& sequence, const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize, - const IndependentVariableType maximumStepSize, + const TimeStepType minimumStepSize, + const TimeStepType maximumStepSize, const StateType& relativeErrorTolerance, const StateType& absoluteErrorTolerance, const TimeStepType safetyFactorForNextStepSize = 0.6, @@ -141,8 +141,8 @@ class BulirschStoerVariableStepSizeIntegrator : const std::vector< unsigned int >& sequence, const StateDerivativeFunction& stateDerivativeFunction, const IndependentVariableType intervalStart, const StateType& initialState, - const IndependentVariableType minimumStepSize, - const IndependentVariableType maximumStepSize, + const TimeStepType minimumStepSize, + const TimeStepType maximumStepSize, const typename StateType::Scalar relativeErrorTolerance = 1.0e-12, const typename StateType::Scalar absoluteErrorTolerance = 1.0e-12, const TimeStepType safetyFactorForNextStepSize = 0.75, @@ -175,7 +175,7 @@ class BulirschStoerVariableStepSizeIntegrator : * Returns the step size of the next step. * \return Step size to be used for the next step. */ - virtual IndependentVariableType getNextStepSize( ) const { return stepSize_; } + virtual TimeStepType getNextStepSize( ) const { return stepSize_; } //! Get current state. /*! @@ -201,7 +201,7 @@ class BulirschStoerVariableStepSizeIntegrator : * constraints, the step is redone until the error constraint is satisfied. * \return The state at the end of the interval. */ - virtual StateType performIntegrationStep( const IndependentVariableType stepSize ) + virtual StateType performIntegrationStep( const TimeStepType stepSize ) { StateType stateAtFirstPoint_; StateType stateAtCenterPoint_; @@ -363,7 +363,7 @@ class BulirschStoerVariableStepSizeIntegrator : /*! * Last used step size, passed to either integrateTo( ) or performIntegrationStep( ). */ - IndependentVariableType stepSize_; + TimeStepType stepSize_; //! Current independent variable. /*! @@ -399,9 +399,9 @@ class BulirschStoerVariableStepSizeIntegrator : /*! * Minimum step size. */ - IndependentVariableType minimumStepSize_; + TimeStepType minimumStepSize_; - IndependentVariableType maximumStepSize_; + TimeStepType maximumStepSize_; //! Relative error tolerance. /*! diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 151e98b794..00b17cabfe 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -35,7 +35,7 @@ enum AvailableIntegrators rungeKutta4, euler, rungeKuttaVariableStepSize, - bulirsch_stoer, + bulirschStoer, adamsBashforthMoulton }; @@ -243,7 +243,7 @@ class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > const TimeType safetyFactorForNextStepSize = 0.7, const TimeType maximumFactorIncreaseForNextStepSize = 10.0, const TimeType minimumFactorDecreaseForNextStepSize = 0.1 ): - IntegratorSettings< TimeType >( bulirsch_stoer, initialTime, initialTimeStep, saveFrequency, + IntegratorSettings< TimeType >( bulirschStoer, initialTime, initialTimeStep, saveFrequency, assessPropagationTerminationConditionDuringIntegrationSubsteps ), extrapolationSequence_( extrapolationSequence ), maximumNumberOfSteps_( maximumNumberOfSteps ), minimumStepSize_( minimumStepSize ), maximumStepSize_( maximumStepSize ), @@ -433,7 +433,7 @@ DependentVariableType, TimeStepType > > createIntegrator( } break; } - case bulirsch_stoer: + case bulirschStoer: { // Check input consistency boost::shared_ptr< BulirschStoerIntegratorSettings< IndependentVariableType > > bulirschStoerIntegratorSettings = @@ -447,7 +447,7 @@ DependentVariableType, TimeStepType > > createIntegrator( { integrator = boost::make_shared< BulirschStoerVariableStepSizeIntegrator - < IndependentVariableType, DependentVariableType, DependentVariableType > > + < IndependentVariableType, DependentVariableType, DependentVariableType, TimeStepType > > ( getBulirschStoerStepSequence( bulirschStoerIntegratorSettings->extrapolationSequence_, bulirschStoerIntegratorSettings->maximumNumberOfSteps_ ), stateDerivativeFunction, integratorSettings->initialTime_, initialState, diff --git a/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h index 600ad681b9..ba3cba618c 100644 --- a/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h +++ b/Tudat/SimulationSetup/PropagationSetup/variationalEquationsSolver.h @@ -1155,7 +1155,7 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta // Integrate variational and state equations. - dynamicsSimulator_->getDynamicsStateDerivative( )->resetFunctionEvaluationCounter( ); + dynamicsSimulator_->getDynamicsStateDerivative( ).at( i )->resetFunctionEvaluationCounter( ); std::map< TimeType, MatrixType > rawNumericalSolution; EquationIntegrationInterface< MatrixType, TimeType >::integrateEquations( singleArcDynamicsSimulators.at( i )->getStateDerivativeFunction( ), @@ -1243,7 +1243,7 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta template cast< StateScalarType >( ); // Integrate variational equations for current arc - dynamicsSimulator_->getDynamicsStateDerivative( )->resetFunctionEvaluationCounter( ); + dynamicsSimulator_->getDynamicsStateDerivative( ).at( i )->resetFunctionEvaluationCounter( ); EquationIntegrationInterface< MatrixType, TimeType >::integrateEquations( singleArcDynamicsSimulators.at( i )->getStateDerivativeFunction( ), rawNumericalSolutions, initialVariationalState, From f47af429173306ce4125b4720e332dd029569c1e Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 24 Jan 2018 15:10:00 +0100 Subject: [PATCH 09/10] Fixes to ABAM JSON interface --- .../unitTestForwardsBackwardsIntegration.cpp | 2 +- Tudat/JsonInterface/Mathematics/integrator.h | 14 ++++++-- Tudat/JsonInterface/Support/keys.cpp | 3 +- Tudat/JsonInterface/Support/keys.h | 4 +++ .../UnitTests/unitTestIntegrator.cpp | 36 ++++++++----------- .../JsonInterface/UnitTests/unitTestSupport.h | 2 -- .../rungeKuttaCoefficients.h | 1 + 7 files changed, 34 insertions(+), 28 deletions(-) diff --git a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestForwardsBackwardsIntegration.cpp b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestForwardsBackwardsIntegration.cpp index d597382f24..da9a0d7c4c 100644 --- a/Tudat/Astrodynamics/Propagators/UnitTests/unitTestForwardsBackwardsIntegration.cpp +++ b/Tudat/Astrodynamics/Propagators/UnitTests/unitTestForwardsBackwardsIntegration.cpp @@ -69,7 +69,7 @@ boost::shared_ptr< IntegratorSettings< TimeType > > getIntegrationSettings( } else if( integratorCase < 5 ) { - RungeKuttaCoefficients::CoefficientSets coefficientSet; + RungeKuttaCoefficients::CoefficientSets coefficientSet = RungeKuttaCoefficients::undefinedCoefficientSet; if( integratorCase == 1 ) { coefficientSet = RungeKuttaCoefficients::rungeKuttaFehlberg45; diff --git a/Tudat/JsonInterface/Mathematics/integrator.h b/Tudat/JsonInterface/Mathematics/integrator.h index 649637802d..c5b65f881b 100644 --- a/Tudat/JsonInterface/Mathematics/integrator.h +++ b/Tudat/JsonInterface/Mathematics/integrator.h @@ -128,6 +128,8 @@ void to_json( nlohmann::json& jsonObject, const boost::shared_ptr< IntegratorSet jsonObject[ K::maximumStepSize ] = adamsBashforthMoultonSettings->maximumStepSize_; jsonObject[ K::relativeErrorTolerance ] = adamsBashforthMoultonSettings->relativeErrorTolerance_; jsonObject[ K::absoluteErrorTolerance ] = adamsBashforthMoultonSettings->absoluteErrorTolerance_; + jsonObject[ K::minimumOrder ] = adamsBashforthMoultonSettings->maximumOrder_; + jsonObject[ K::maximumOrder ] = adamsBashforthMoultonSettings->maximumOrder_; jsonObject[ K::bandwidth ] = adamsBashforthMoultonSettings->bandwidth_; return; } @@ -143,6 +145,12 @@ void to_json( nlohmann::json& jsonObject, const boost::shared_ptr< IntegratorSet jsonObject[ K::absoluteErrorTolerance ] = bulirschStoerSettings->absoluteErrorTolerance_; jsonObject[ K::extrapolationSequence ] = bulirschStoerSettings->extrapolationSequence_; jsonObject[ K::maximumNumberOfSteps ] = bulirschStoerSettings->maximumNumberOfSteps_; + jsonObject[ K::safetyFactorForNextStepSize ] = + bulirschStoerSettings->safetyFactorForNextStepSize_; + jsonObject[ K::maximumFactorIncreaseForNextStepSize ] = + bulirschStoerSettings->maximumFactorIncreaseForNextStepSize_; + jsonObject[ K::minimumFactorDecreaseForNextStepSize ] = + bulirschStoerSettings->minimumFactorDecreaseForNextStepSize_; return; } @@ -217,6 +225,8 @@ void from_json( const nlohmann::json& jsonObject, boost::shared_ptr< IntegratorS getValue< TimeType >( jsonObject, K::maximumStepSize ), getValue( jsonObject, K::relativeErrorTolerance, defaults.relativeErrorTolerance_ ), getValue( jsonObject, K::absoluteErrorTolerance, defaults.absoluteErrorTolerance_ ), + getValue( jsonObject, K::minimumOrder, defaults.minimumOrder_ ), + getValue( jsonObject, K::maximumOrder, defaults.maximumOrder_ ), getValue( jsonObject, K::saveFrequency, defaults.saveFrequency_ ), getValue( jsonObject, K::assessPropagationTerminationConditionDuringIntegrationSubsteps, defaults.assessPropagationTerminationConditionDuringIntegrationSubsteps_ ), @@ -227,8 +237,8 @@ void from_json( const nlohmann::json& jsonObject, boost::shared_ptr< IntegratorS case bulirschStoer: { BulirschStoerIntegratorSettings< TimeType > defaults( - 0.0, 0.0, bulirsch_stoer_sequence, 6, std::numeric_limits< limits >::epsilon( ), - std::numeric_limits< limits >::infinity( ) ); + 0.0, 0.0, bulirsch_stoer_sequence, 6, std::numeric_limits< double >::epsilon( ), + std::numeric_limits< double >::infinity( ) ); integratorSettings = boost::make_shared< BulirschStoerIntegratorSettings< TimeType > >( initialTime, diff --git a/Tudat/JsonInterface/Support/keys.cpp b/Tudat/JsonInterface/Support/keys.cpp index eb9d43c72b..d3cd3c35ba 100644 --- a/Tudat/JsonInterface/Support/keys.cpp +++ b/Tudat/JsonInterface/Support/keys.cpp @@ -332,7 +332,8 @@ const std::string Keys::Integrator::minimumFactorDecreaseForNextStepSize = "mini const std::string Keys::Integrator::bandwidth = "bandwidth"; const std::string Keys::Integrator::extrapolationSequence = "extrapolationSequence"; const std::string Keys::Integrator::maximumNumberOfSteps = "maximumNumberOfSteps"; - +const std::string Keys::Integrator::maximumOrder = "maximumOrder"; +const std::string Keys::Integrator::minimumOrder = "minimumOrder"; // Interpolation diff --git a/Tudat/JsonInterface/Support/keys.h b/Tudat/JsonInterface/Support/keys.h index 8241af8b5f..c0a15acd35 100644 --- a/Tudat/JsonInterface/Support/keys.h +++ b/Tudat/JsonInterface/Support/keys.h @@ -353,6 +353,10 @@ struct Keys static const std::string bandwidth; static const std::string extrapolationSequence; static const std::string maximumNumberOfSteps; + static const std::string minimumOrder; + static const std::string maximumOrder; + + }; struct Interpolation diff --git a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp index f1b7c81b19..d89d043540 100644 --- a/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp +++ b/Tudat/JsonInterface/UnitTests/unitTestIntegrator.cpp @@ -156,22 +156,22 @@ BOOST_AUTO_TEST_CASE( test_json_integrator_adamsBashforthMoulton ) const int maximumOrder = 11; const boost::shared_ptr< IntegratorSettings< double > > manualSettings = boost::make_shared< AdamsBashforthMoultonSettings< double > >( initialTime, - initialStepSize, - minimumStepSize, - maximumStepSize, - relativeErrorTolerance, - absoluteErrorTolerance, - minimumOrder, - maximumOrder, - 1, - false, - bandwidth ); + initialStepSize, + minimumStepSize, + maximumStepSize, + relativeErrorTolerance, + absoluteErrorTolerance, + minimumOrder, + maximumOrder, + 1, + false, + bandwidth ); // Compare BOOST_CHECK_EQUAL_JSON( fromFileSettings, manualSettings ); } -// Test 6: adamsBashforthMoulton +// Test 6: bulirschStoer BOOST_AUTO_TEST_CASE( test_json_integrator_bulirschStoer ) { using namespace tudat::numerical_integrators; @@ -191,17 +191,9 @@ BOOST_AUTO_TEST_CASE( test_json_integrator_bulirschStoer ) const int maximumNumberOfSteps = 8; const boost::shared_ptr< IntegratorSettings< double > > manualSettings = - boost::make_shared< BulirschStoerIntegratorSettings( initialTime, - initialStepSize, - minimumStepSize, - maximumStepSize, - relativeErrorTolerance, - absoluteErrorTolerance, - minimumOrder, - maximumOrder, - 1, - false, - bandwidth ); + boost::make_shared< BulirschStoerIntegratorSettings< double > >( + initialTime, initialStepSize, bulirsch_stoer_sequence, maximumNumberOfSteps, + minimumStepSize, maximumStepSize, relativeErrorTolerance, absoluteErrorTolerance ); // Compare BOOST_CHECK_EQUAL_JSON( fromFileSettings, manualSettings ); diff --git a/Tudat/JsonInterface/UnitTests/unitTestSupport.h b/Tudat/JsonInterface/UnitTests/unitTestSupport.h index ee02e463f6..771802246e 100644 --- a/Tudat/JsonInterface/UnitTests/unitTestSupport.h +++ b/Tudat/JsonInterface/UnitTests/unitTestSupport.h @@ -82,8 +82,6 @@ void checkConsistentEnum( const std::string& filename, } } - std::cout << "JSON file: " << filename << std::endl; - // Check that values and supportedValues are equivalent const std::vector< Enum > values = parseJSONFile< std::vector< Enum > >( filename ); BOOST_CHECK( containsAllOf( values, supportedValues ) && containsAllOf( supportedValues, values ) ); diff --git a/Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h b/Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h index 04585f85d0..fe74ca5360 100644 --- a/Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h +++ b/Tudat/Mathematics/NumericalIntegrators/rungeKuttaCoefficients.h @@ -92,6 +92,7 @@ struct RungeKuttaCoefficients //! Enum of predefined coefficient sets. enum CoefficientSets { + undefinedCoefficientSet = -1, rungeKuttaFehlberg45, rungeKuttaFehlberg56, rungeKuttaFehlberg78, From e2e933653f275a8720cfd44c635e5be92d6a5363 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Wed, 24 Jan 2018 15:58:24 +0100 Subject: [PATCH 10/10] Added minor missing comments to BS/ABAM integrators --- .../dynamicsStateDerivativeModel.h | 20 +++++--- .../adamsBashforthMoultonIntegrator.h | 26 +++++++--- ...ulirschStoerVariableStepsizeIntegrator.cpp | 2 +- .../bulirschStoerVariableStepsizeIntegrator.h | 49 ++++++++++++++++--- .../createNumericalIntegrator.h | 13 +++-- 5 files changed, 84 insertions(+), 26 deletions(-) diff --git a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h index b2db0e27d3..14d7443a04 100644 --- a/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h +++ b/Tudat/Astrodynamics/Propagators/dynamicsStateDerivativeModel.h @@ -449,12 +449,23 @@ class DynamicsStateDerivativeModel return stateTypeStartIndex_; } - + //! Function to retrieve number of calls to the computeStateDerivative function + /*! + * Function to retrieve number of calls to the computeStateDerivative function since object creation/last call to + * resetFunctionEvaluationCounter function + * \return Number of calls to the computeStateDerivative function since object creation/last call to + * resetFunctionEvaluationCounter function + */ int getNumberOfFunctionEvaluations( ) { return functionEvaluationCounter_; } + //! Function to resetr the number of calls to the computeStateDerivative function to zero. + /*! + * Function to resetr the number of calls to the computeStateDerivative function to zero. Typically called before any + * start of numerical integration of dynamics (automatically by DynamicsSimulator) + */ void resetFunctionEvaluationCounter( ) { functionEvaluationCounter_ = 0; @@ -504,8 +515,6 @@ class DynamicsStateDerivativeModel // Get state block indices of current state derivative model currentIndices = stateIndices_.at( stateDerivativeModelsIterator_->first ).at( i ); - // std::cout << "Pre-converted state: " << state.block( currentIndices.first, startColumn, currentIndices.second, 1 ).transpose( ) << std::endl; - // Set current block in split state (in global form) stateDerivativeModelsIterator_->second.at( i )->convertCurrentStateToGlobalRepresentation( state.block( currentIndices.first, startColumn, currentIndices.second, 1 ), time, @@ -513,10 +522,6 @@ class DynamicsStateDerivativeModel stateDerivativeModelsIterator_->first ).block( currentStateTypeSize, 0, currentIndices.second, 1 ) ); - // std::cout << "Converted state: " << currentStatesPerTypeInConventionalRepresentation_.at( - // stateDerivativeModelsIterator_->first ).block( - // currentStateTypeSize, 0, currentIndices.second, 1 ).transpose( ) << std::endl; - currentStateTypeSize += currentIndices.second; } } @@ -573,6 +578,7 @@ class DynamicsStateDerivativeModel std::unordered_map< IntegratedStateType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > > currentStatesPerTypeInConventionalRepresentation_; + //! Variable to keep track of the number of calls to the computeStateDerivative function int functionEvaluationCounter_ = 0; }; diff --git a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h index c4ecc2ff5a..77ea11f330 100644 --- a/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/adamsBashforthMoultonIntegrator.h @@ -546,8 +546,8 @@ class AdamsBashforthMoultonIntegrator //! Set the minimum and maximum step size. /*! * Change the bounds of the stepsize. - * \param minimum stepsize - * \param maximum stepsize + * \param minimumStepSize minimum stepsize + * \param maximumStepSize maximum stepsize */ void setStepSizeBounds( IndependentVariableType minimumStepSize, IndependentVariableType maximumStepSize ) { minimumStepSize_ = minimumStepSize; @@ -572,8 +572,18 @@ class AdamsBashforthMoultonIntegrator */ void setStepSize( IndependentVariableType stepSize ) { stepSize_ = stepSize; } + //! Function to reset the minimum order of the integrator + /*! + * Function to reset the minimum order of the integrator + * \param minimumOrder New minimum order of the integrator + */ void setMinimumOrder( unsigned int minimumOrder ){ minimumOrder_ = minimumOrder; } + //! Function to reset the maximum order of the integrator + /*! + * Function to reset the maximum order of the integrator + * \param maximumOrder New maximum order of the integrator + */ void setMaximumOrder( unsigned int maximumOrder ){ maximumOrder_ = maximumOrder; } IndependentVariableType getPreviousIndependentVariable( ) @@ -793,8 +803,8 @@ class AdamsBashforthMoultonIntegrator //! Perform predictor step. /*! * Using the order find predicted estimate using the Adams-Bashforth predictor - * \param order of the integration. - * \param boolean if stepsize should be considered double, true for estimating doubling error. + * \param order Order of the integration. + * \param doubleStep Boolean if stepsize should be considered double, true for estimating doubling error. * \return state after predictor step */ StateType performPredictorStep( unsigned int order, bool doubleStep ) @@ -865,8 +875,8 @@ class AdamsBashforthMoultonIntegrator * true if first one is better than second (smaller is better). * \param absoluteError1 absolute error one. * \param relativeError1 relative error one. - * \param absoluteError1 absolute error two. - * \param relativeError1 relative error two. + * \param absoluteError2 absolute error two. + * \param relativeError2 relative error two. * \return true if one is better than two, false otherwise. */ bool errorCompare( StateType absoluteError1, StateType relativeError1, @@ -912,8 +922,8 @@ class AdamsBashforthMoultonIntegrator /*! * Checks error (absolute and relative form) and return true if * the exceed tolerance limits. - * \param a absolute error. - * \param r relative error. + * \param absoluteError absolute error. + * \param relativeError relative error. * \return true if one error is too small, false if within limits */ bool errorTooSmall( StateType absoluteError, StateType relativeError ) diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp index 56e76a7c85..53d8a32b3f 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.cpp @@ -26,7 +26,7 @@ namespace tudat namespace numerical_integrators { - +//! Function to retrieve the sequence of number of steps to used for Bulirsch-Stoer integration std::vector< unsigned int > getBulirschStoerStepSequence( const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType, const unsigned int lengthOfSequence ) diff --git a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h index 883ea2ecd7..d0a7c42249 100644 --- a/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/bulirschStoerVariableStepsizeIntegrator.h @@ -37,13 +37,20 @@ namespace tudat namespace numerical_integrators { +//! Types of sequences available for extrapolation integration methods enum ExtrapolationMethodStepSequences { bulirsch_stoer_sequence, deufelhard_sequence }; - +//! Function to retrieve the sequence of number of steps to used for Bulirsch-Stoer integration +/*! + * Function to retrieve the sequence of number of steps to used for Bulirsch-Stoer integration + * \param extrapolationMethodStepSequenceType Type of sequence that is to be retrieved + * \param lengthOfSequence Length of the sequence that is to be retrieved (default 12) + * \return Function to retrieve the sequence of number of steps to used for Bulirsch-Stoer integration + */ std::vector< unsigned int > getBulirschStoerStepSequence( const ExtrapolationMethodStepSequences& extrapolationMethodStepSequenceType = bulirsch_stoer_sequence, const unsigned int lengthOfSequence = 12 ); @@ -88,8 +95,14 @@ class BulirschStoerVariableStepSizeIntegrator : * \param initialState The initial state. * \param minimumStepSize The minimum step size to take. If this constraint is violated, a * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param maximumStepSize The maximum step size to take. * \param relativeErrorTolerance The relative error tolerance, for each individual state * vector element. + * \param absoluteErrorTolerance The absolute error tolerance, for each individual state + * vector element. + * \param safetyFactorForNextStepSize Safety factor used to scale prediction of next step size. + * \param maximumFactorIncreaseForNextStepSize Maximum factor increase for next step size. + * \param minimumFactorDecreaseForNextStepSize Maximum factor decrease for next step size. * \sa NumericalIntegrator::NumericalIntegrator. */ BulirschStoerVariableStepSizeIntegrator( @@ -127,14 +140,20 @@ class BulirschStoerVariableStepSizeIntegrator : /*! * Default constructor, taking coefficients, a state derivative function, initial conditions, * minimum step size and relative error tolerance for all items in the state vector as argument. - * \param coefficients Coefficients to use with this integrator. + * \param sequence Rational function sequence used by algorithm. * \param stateDerivativeFunction State derivative function. * \param intervalStart The start of the integration interval. * \param initialState The initial state. * \param minimumStepSize The minimum step size to take. If this constraint is violated, a * flag will be set that can be retrieved with isMinimumStepSizeViolated( ). + * \param maximumStepSize The maximum step size to take. * \param relativeErrorTolerance The relative error tolerance, equal for all individual state * vector elements. + * \param absoluteErrorTolerance The absolute error tolerance, for each individual state + * vector element. + * \param safetyFactorForNextStepSize Safety factor used to scale prediction of next step size. + * \param maximumFactorIncreaseForNextStepSize Maximum factor increase for next step size. + * \param minimumFactorDecreaseForNextStepSize Maximum factor decrease for next step size. * \sa NumericalIntegrator::NumericalIntegrator. */ BulirschStoerVariableStepSizeIntegrator( @@ -401,6 +420,10 @@ class BulirschStoerVariableStepSizeIntegrator : */ TimeStepType minimumStepSize_; + //! Minimum step size. + /*! + * Maximum step size. + */ TimeStepType maximumStepSize_; //! Relative error tolerance. @@ -415,13 +438,27 @@ class BulirschStoerVariableStepSizeIntegrator : */ StateType absoluteErrorTolerance_; - + //! Safety factor for next step size. + /*! + * Safety factor used to scale prediction of next step size. This is usually picked between + * 0.8 and 0.9 (Burden and Faires, 2001). + */ TimeStepType safetyFactorForNextStepSize_; - + //! Maximum factor increase for next step size. + /*! + * The maximum factor by which the next step size can increase compared to the current value. + * The need for this maximum stems from a need to ensure that the step size changes do not + * alias with the dynamics of the model being integrated. + */ TimeStepType maximumFactorIncreaseForNextStepSize_; - + //! Minimum factor decrease for next step size. + /*! + * The minimum factor by which the next step size can decrease compared to the current value. + * The need for this minimum stems from a need to ensure that the step size changes do not + * alias with the dynamics of the model being integrated. + */ TimeStepType minimumFactorDecreaseForNextStepSize_; //! Flag to indicate whether the minimum step size constraint has been violated. @@ -437,7 +474,7 @@ class BulirschStoerVariableStepSizeIntegrator : * \param stateAtCenterPoint State at center point. * \param independentVariableAtFirstPoint Independent variable at first point. * \param subStepSize Sub step size between successive states used by mid-point method. - * \param stateAtLastPoint State at last point. + * \return Result of midpoint method */ StateType executeMidPointMethod( StateType stateAtFirstPoint, StateType stateAtCenterPoint, const IndependentVariableType independentVariableAtFirstPoint, diff --git a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h index 00b17cabfe..fa99f07e6c 100644 --- a/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h +++ b/Tudat/Mathematics/NumericalIntegrators/createNumericalIntegrator.h @@ -211,11 +211,12 @@ class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > //! Constructor /*! * Constructor for variable step RK integrator settings. - * \param integratorType Type of numerical integrator (must be an RK variable step type) * \param initialTime Start time (independent variable) of numerical integration. * \param initialTimeStep Initial time (independent variable) step used in numerical integration. * Adapted during integration - * \param coefficientSet Coefficient set (butcher tableau) to use in integration. + * \param extrapolationSequence Type of sequence that is to be used for Bulirsch-Stoer integrator + * \param maximumNumberOfSteps Number of entries in teh sequence, e.g. number of integrations used for a single + * extrapolation. * \param minimumStepSize Minimum step size for integration. Integration stops (exception thrown) if time step * comes below this value. * \param maximumStepSize Maximum step size for integration. @@ -258,8 +259,10 @@ class BulirschStoerIntegratorSettings: public IntegratorSettings< TimeType > */ ~BulirschStoerIntegratorSettings( ){ } + //! Type of sequence that is to be used for Bulirsch-Stoer integrator ExtrapolationMethodStepSequences extrapolationSequence_; + //! Number of entries in teh sequence, e.g. number of integrations used for a single extrapolation. unsigned int maximumNumberOfSteps_; //! Minimum step size for integration. @@ -302,16 +305,16 @@ class AdamsBashforthMoultonSettings: public IntegratorSettings< TimeType > //! Constructor /*! * Constructor for variable step RK integrator settings. - * \param integratorType Type of numerical integrator (must be an RK variable step type) * \param initialTime Start time (independent variable) of numerical integration. * \param initialTimeStep Initial time (independent variable) step used in numerical integration. * Adapted during integration - * \param coefficientSet Coefficient set (butcher tableau) to use in integration. * \param minimumStepSize Minimum step size for integration. Integration stops (exception thrown) if time step * comes below this value. * \param maximumStepSize Maximum step size for integration. * \param relativeErrorTolerance Relative error tolerance for step size control * \param absoluteErrorTolerance Absolute error tolerance for step size control + * \param minimumOrder Minimum order of integrator (default 6) + * \param maximumOrder Maximum order of integrator (default 11) * \param saveFrequency Frequency at which to save the numerical integrated states (in units of i.e. per n integration * time steps, with n = saveFrequency). * \param assessPropagationTerminationConditionDuringIntegrationSubsteps Whether the propagation termination @@ -358,8 +361,10 @@ class AdamsBashforthMoultonSettings: public IntegratorSettings< TimeType > //! Absolute error tolerance for step size control TimeType absoluteErrorTolerance_; + //! Minimum order of integrator const int minimumOrder_; + //! Maximum order of integrator const int maximumOrder_; //! Safety factor for step size control