diff --git a/.bumpversion.cfg b/.bumpversion.cfg index 120ac7ac24..6774c43588 100644 --- a/.bumpversion.cfg +++ b/.bumpversion.cfg @@ -1,5 +1,5 @@ [bumpversion] -current_version = 2.12.1.dev24 +current_version = 2.12.1.dev30 commit = True tag = True parse = (?P\d+)\.(?P\d+)\.(?P\d+)(\.(?P[a-z]+)(?P\d+))? diff --git a/include/tudat/astro/observation_models/observationManager.h b/include/tudat/astro/observation_models/observationManager.h index 877c539322..ab5600069a 100644 --- a/include/tudat/astro/observation_models/observationManager.h +++ b/include/tudat/astro/observation_models/observationManager.h @@ -450,9 +450,6 @@ class ObservationManager: public ObservationManagerBase< ObservationScalarType, // Partial w.r.t. observation time property if ( isParameterObservationLinkTimeProperty( partialIterator->second->getParameterIdentifier( ).first ) ) { -// partialMatrix.block( 0, currentIndexInfo.first, observationSize, currentIndexInfo.second ) += -// observationPartialWrtTimeProperty * singlePartialSet[ i ].first; - // Iterate (again) over all observation partials to retrieve those associated with given link ends states. for( auto itr : currentLinkEndPartials ) { @@ -489,7 +486,9 @@ class ObservationManager: public ObservationManagerBase< ObservationScalarType, { throw std::runtime_error( "Error, required dependent variable interfaces, but none found when computing partials." ); } - Eigen::VectorXd acceleration = dependentVariablesInterface_->getSingleDependentVariable( totalAccelerationVariable, times.at( indexLinkEndType ) ); + + Eigen::VectorXd acceleration = dependentVariablesInterface_->getSingleDependentVariable( + totalAccelerationVariable, times.at( indexLinkEndType ) ); Eigen::Vector6d stateDerivativeVector = Eigen::Vector6d::Zero( ); stateDerivativeVector.segment( 0, 3 ) = states.at( indexLinkEndType ).segment( 3, 3 ); stateDerivativeVector.segment( 3, 3 ) = acceleration; diff --git a/include/tudat/astro/orbit_determination/podInputOutputTypes.h b/include/tudat/astro/orbit_determination/podInputOutputTypes.h index d67985a139..ab0b225e92 100644 --- a/include/tudat/astro/orbit_determination/podInputOutputTypes.h +++ b/include/tudat/astro/orbit_determination/podInputOutputTypes.h @@ -70,6 +70,167 @@ class CovarianceAnalysisInput observationCollection_->getTotalObservableSize( ), constantWeight ); } + //! Set constant scalar weight for all observables of given type + void setConstantSingleObservableWeights( + const observation_models::ObservableType currentObservable, + const double weight ) + { + std::map< observation_models::ObservableType, std::pair< int, int > > observationTypeStartAndSize = + observationCollection_->getObservationTypeStartAndSize( ); + + if( observationTypeStartAndSize.count( currentObservable) == 0 ) + { + std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << + " No data of given type found." < indicesToUse = observationTypeStartAndSize.at( currentObservable ); + + weightsMatrixDiagonals_.segment( indicesToUse.first, indicesToUse.second ) = + Eigen::VectorXd::Constant( indicesToUse.second, weight ); + } + } + + //! Set constant vector weight for all observables of given type + void setConstantSingleObservableVectorWeights( + const observation_models::ObservableType currentObservable, + const Eigen::VectorXd weight ) + { + std::map< observation_models::ObservableType, std::pair< int, int > > observationTypeStartAndSize = + observationCollection_->getObservationTypeStartAndSize( ); + + if( observationTypeStartAndSize.count( currentObservable) == 0 ) + { + std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << + " No data of given type found." < indicesToUse = observationTypeStartAndSize.at( currentObservable ); + + int observableSize = observation_models::getObservableSize( currentObservable ); + if( observableSize != weight.rows( ) ) + { + throw std::runtime_error( "Error when setting weight vector for observable, size should be " + + std::to_string( observableSize ) + " but is " + std::to_string( weight.rows( ) ) ); + } + int numberOfObservations = indicesToUse.second / observableSize; + + weightsMatrixDiagonals_.segment( indicesToUse.first, indicesToUse.second ) = + utilities::getSuccesivelyConcatenatedVector( weight, numberOfObservations ); + } + } + + //! Set constant scalar weight for all observables of given type and link ends + void setConstantSingleObservableAndLinkEndsWeights( + const observation_models::ObservableType currentObservable, + const observation_models::LinkEnds currentLinkEnds, + const double weight ) + { + std::map< observation_models::ObservableType, + std::map< observation_models::LinkEnds, std::pair< int, int > > > observationLinkEndStartAndSize = + observationCollection_->getObservationTypeAndLinkEndStartAndSize( ); + + if( observationLinkEndStartAndSize.count( currentObservable) == 0 ) + { + std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << + " No data of given type found." < indicesToUse = observationLinkEndStartAndSize.at( currentObservable ).at( currentLinkEnds ); + weightsMatrixDiagonals_.segment( indicesToUse.first, + indicesToUse.second ) = + Eigen::VectorXd::Constant( indicesToUse.second, weight ); + } + } + + //! Set constant vector weight for all observables of given type and link ends + void setConstantSingleObservableAndLinkEndsVectorWeights( + const observation_models::ObservableType currentObservable, + const observation_models::LinkEnds currentLinkEnds, + const Eigen::VectorXd weight ) + { + std::map< observation_models::ObservableType, + std::map< observation_models::LinkEnds, std::pair< int, int > > > observationLinkEndStartAndSize = + observationCollection_->getObservationTypeAndLinkEndStartAndSize( ); + + if( observationLinkEndStartAndSize.count( currentObservable) == 0 ) + { + std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << + " No data of given type found." < indicesToUse = + observationLinkEndStartAndSize.at( currentObservable ).at( currentLinkEnds ); + + int observableSize = observation_models::getObservableSize( currentObservable ); + if( observableSize != weight.rows( ) ) + { + throw std::runtime_error( "Error when setting weight vector for observable, size should be " + + std::to_string( observableSize ) + " but is " + std::to_string( weight.rows( ) ) ); + } + int numberOfObservations = indicesToUse.second / observableSize; + + + weightsMatrixDiagonals_.segment( indicesToUse.first, + indicesToUse.second ) = + utilities::getSuccesivelyConcatenatedVector( weight, numberOfObservations ); + } + } + + //! Set constant vector weight for all observables of given type and link ends + void setTabulatedSingleObservableAndLinkEndsWeights( + const observation_models::ObservableType currentObservable, + const observation_models::LinkEnds currentLinkEnds, + const Eigen::VectorXd weight ) + { + std::map< observation_models::ObservableType, + std::map< observation_models::LinkEnds, std::pair< int, int > > > observationLinkEndStartAndSize = + observationCollection_->getObservationTypeAndLinkEndStartAndSize( ); + + if( observationLinkEndStartAndSize.count( currentObservable) == 0 ) + { + std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << + " No data of given type found." < indicesToUse = + observationLinkEndStartAndSize.at( currentObservable ).at( currentLinkEnds ); + + if( indicesToUse.second != weight.rows( ) ) + { + throw std::runtime_error( "Error when setting total weight vector for observable and link ends, size should be " + + std::to_string( indicesToUse.second ) + " but is " + std::to_string( weight.rows( ) ) ); + } + + weightsMatrixDiagonals_.segment( indicesToUse.first, + indicesToUse.second ) = weight; + } + } + + //! Function to set a values for observation weights, constant per observable type /*! * Function to set a values for observation weights, constant per observable type @@ -78,24 +239,20 @@ class CovarianceAnalysisInput void setConstantPerObservableWeightsMatrix( const std::map< observation_models::ObservableType, double > weightPerObservable ) { - std::map< observation_models::ObservableType, std::pair< int, int > > observationTypeStartAndSize = - observationCollection_->getObservationTypeStartAndSize( ); - for( auto observableIterator : weightPerObservable ) { - observation_models::ObservableType currentObservable = observableIterator.first; - if( observationTypeStartAndSize.count( observableIterator.first ) == 0 ) - { - std::cerr<<"Warning when setting weights for data type " < weightPerObservable ) + { + for( auto observableIterator : weightPerObservable ) + { + setConstantSingleObservableVectorWeights( + observableIterator.first, observableIterator.second ); } } @@ -108,46 +265,32 @@ class CovarianceAnalysisInput const std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, double > > weightPerObservableAndLinkEnds ) { - std::map< observation_models::ObservableType, - std::map< observation_models::LinkEnds, std::vector< std::pair< int, int > > > > observationSetStartAndSize = - observationCollection_->getObservationSetStartAndSize( ); - for( auto observableIterator : weightPerObservableAndLinkEnds ) { observation_models::ObservableType currentObservable = observableIterator.first; - if( observationSetStartAndSize.count( currentObservable) == 0 ) + for( auto linkEndIterator : observableIterator.second ) { - std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << - " No data of given type found." <( currentLinkEnds ) + - ". No data of given type and link ends found." < > indicesToUse = - observationSetStartAndSize.at( currentObservable ).at( currentLinkEnds ); - for( unsigned int i = 0; i < indicesToUse.size( ); i++ ) - { - weightsMatrixDiagonals_.segment( indicesToUse.at( i ).first, - indicesToUse.at( i ).second ) = - Eigen::VectorXd::Constant( indicesToUse.at( i ).second, linkEndIterator.second ); - } - } - } + void setConstantPerObservableAndLinkEndsVectorWeights( + const std::map< observation_models::ObservableType, + std::map< observation_models::LinkEnds, Eigen::VectorXd > > weightPerObservableAndLinkEnds ) + { + for( auto observableIterator : weightPerObservableAndLinkEnds ) + { + observation_models::ObservableType currentObservable = observableIterator.first; + for( auto linkEndIterator : observableIterator.second ) + { + observation_models::LinkEnds currentLinkEnds = linkEndIterator.first; + setConstantSingleObservableAndLinkEndsVectorWeights( currentObservable, currentLinkEnds, linkEndIterator.second ); } - } } + void setConstantPerObservableAndLinkEndsWeights( const observation_models::ObservableType observableType, const std::vector< observation_models::LinkEnds >& linkEnds, @@ -161,29 +304,30 @@ class CovarianceAnalysisInput setConstantPerObservableAndLinkEndsWeights( weightPerObservableAndLinkEnds ); } - - void setConstantPerObservableAndLinkEndsWeights( - const observation_models::ObservableType observableType, - const observation_models::LinkEnds& linkEnds, - const double weight ) + void setConstantPerObservableAndLinkEndsVectorWeights( + const observation_models::ObservableType observableType, + const std::vector< observation_models::LinkEnds >& linkEnds, + const Eigen::VectorXd weight ) { - std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, double > > weightPerObservableAndLinkEnds; - weightPerObservableAndLinkEnds[ observableType ][ linkEnds ] = weight; - setConstantPerObservableAndLinkEndsWeights( weightPerObservableAndLinkEnds ); + std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, Eigen::VectorXd > > weightPerObservableAndLinkEnds; + for( unsigned int i = 0; i < linkEnds.size( ); i++ ) + { + weightPerObservableAndLinkEnds[ observableType ][ linkEnds.at( i ) ] = weight; + } + setConstantPerObservableAndLinkEndsVectorWeights( weightPerObservableAndLinkEnds ); } - void setTabulatedPerObservableAndLinkEndsWeights( const std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, Eigen::VectorXd > > weightsPerObservableAndLinkEnds ) { - std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, std::vector< std::pair< int, int > > > > observationSetStartAndSize = - observationCollection_->getObservationSetStartAndSize( ); + std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, std::pair< int, int > > > observationLinkEndStartAndSize = + observationCollection_->observationTypeAndLinkEndStartAndSize_( ); for( auto observableIterator : weightsPerObservableAndLinkEnds ) { observation_models::ObservableType currentObservable = observableIterator.first; - if( observationSetStartAndSize.count( currentObservable) == 0 ) + if( observationLinkEndStartAndSize.count( currentObservable) == 0 ) { std::cerr<< "Warning when setting weights for data type "<< std::to_string( currentObservable) << ". " << " No data of given type found." < > indicesToUse = observationSetStartAndSize.at( currentObservable ).at( currentLinkEnds ); - for( unsigned int i = 0; i < indicesToUse.size( ); i++ ) - { - if ( indicesToUse.at( i ).second != linkEndIterator.second.size( ) ) - { - throw std::runtime_error( "Error when setting tabulated weights for data type " + std::to_string( currentObservable) + - ", weights vector is inconsistent with the number of observations." ); - } - weightsMatrixDiagonals_.segment( indicesToUse.at( i ).first, indicesToUse.at( i ).second ) = linkEndIterator.second; - } + setTabulatedSingleObservableAndLinkEndsWeights( currentObservable, currentLinkEnds, linkEndIterator.second ); } } } @@ -219,6 +354,19 @@ class CovarianceAnalysisInput } } + void setTabulatedPerObservableAndLinkEndsWeights( + const observation_models::ObservableType observableType, + const std::vector< observation_models::LinkEnds >& linkEnds, + const Eigen::VectorXd weights ) + { + std::map< observation_models::ObservableType, std::map< observation_models::LinkEnds, Eigen::VectorXd > > weightsPerObservableAndLinkEnds; + for( unsigned int i = 0; i < linkEnds.size( ); i++ ) + { + weightsPerObservableAndLinkEnds[ observableType ][ linkEnds.at( i ) ] = weights; + } + setTabulatedPerObservableAndLinkEndsWeights( weightsPerObservableAndLinkEnds ); + } + //! Function to return the total data structure of observations and associated times/link ends/type (by reference) /*! * Function to return the total data structure of observations and associated times/link ends/type (by reference) diff --git a/include/tudat/astro/propagators/stateTransitionMatrixInterface.h b/include/tudat/astro/propagators/stateTransitionMatrixInterface.h index 7b26385afd..9c575b36d7 100644 --- a/include/tudat/astro/propagators/stateTransitionMatrixInterface.h +++ b/include/tudat/astro/propagators/stateTransitionMatrixInterface.h @@ -1009,7 +1009,6 @@ class HybridArcCombinedStateTransitionAndSensitivityMatrixInterface: public Comb currentArc.second, false, arcDefiningBodies ); // Set coupled block - std::cout << multiArcStateTransition.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ ) << "\n\n"; combinedStateTransitionMatrix.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ ) = multiArcStateTransition.block( singleArcStateSize_, 0, originalMultiArcStateSize, singleArcStateSize_ ) * singleArcStateTransitionAtArcStart.block( 0, 0, singleArcStateSize_, singleArcStateSize_ ); diff --git a/include/tudat/basics/utilities.h b/include/tudat/basics/utilities.h index b69e7c1434..929374d2e9 100755 --- a/include/tudat/basics/utilities.h +++ b/include/tudat/basics/utilities.h @@ -11,6 +11,7 @@ #ifndef TUDAT_UTILITIES_H #define TUDAT_UTILITIES_H +#include #include #include #include @@ -92,6 +93,25 @@ std::vector< VectorArgument > createVectorFromMapValues( const std::map< KeyType return outputVector; } +template< typename VectorArgument, typename KeyType > +std::vector< KeyType > createVectorFromUnorderedMapKeys( const std::unordered_map< KeyType, VectorArgument >& inputMap ) +{ + // Create and size return vector. + std::vector< KeyType > outputVector; + outputVector.resize( inputMap.size( ) ); + + // Iterate over all map entries and create vector + int currentIndex = 0; + for( typename std::unordered_map< KeyType, VectorArgument >::const_iterator mapIterator = inputMap.begin( ); + mapIterator != inputMap.end( ); mapIterator++, currentIndex++ ) + { + outputVector[ currentIndex ] = mapIterator->first; + } + + return outputVector; +} + + //! Function to create a vector from the keys of a map /*! * Function to create a vector from the keys of a map. The output vector is in the order of the map entries, i.e. as provided by a forward iterator. @@ -845,6 +865,21 @@ std::string to_string_with_precision(const T a_value, const int n = 6) return std::move(out).str(); } +template +Eigen::Matrix< T, Eigen::Dynamic, 1 > getSuccesivelyConcatenatedVector( + const Eigen::Matrix< T, Eigen::Dynamic, 1 > baseVector, const unsigned int numberOfConcatenations ) +{ + int singleSize = baseVector.rows( ); + Eigen::Matrix< T, Eigen::Dynamic, 1 > concatenatedVector = Eigen::Matrix< T, Eigen::Dynamic, 1 >::Zero( + numberOfConcatenations * singleSize ); + for( unsigned int i = 0; i < numberOfConcatenations; i++ ) + { + concatenatedVector.segment( i * singleSize, singleSize ) = baseVector; + } + return concatenatedVector; +} + + } // namespace utilities } // namespace tudat diff --git a/include/tudat/math/interpolators/oneDimensionalInterpolator.h b/include/tudat/math/interpolators/oneDimensionalInterpolator.h index ca5b6a352c..d3d6a767e6 100644 --- a/include/tudat/math/interpolators/oneDimensionalInterpolator.h +++ b/include/tudat/math/interpolators/oneDimensionalInterpolator.h @@ -75,7 +75,7 @@ class OneDimensionalInterpolator : public Interpolator< IndependentVariableType, { } //! Constructor. - /*! + /*!throw_exception_at_boundary * Constructor. * \param boundaryHandling Boundary handling method in case independent variable is outside the * specified range. @@ -185,6 +185,11 @@ class OneDimensionalInterpolator : public Interpolator< IndependentVariableType, return boundaryHandling_; } + void resetBoundaryHandling( const BoundaryInterpolationType boundaryHandling ) + { + boundaryHandling_ = boundaryHandling; + } + std::pair< DependentVariableType, DependentVariableType > getDefaultExtrapolationValue( ) { return defaultExtrapolationValue_; diff --git a/include/tudat/simulation/environment_setup/body.h b/include/tudat/simulation/environment_setup/body.h index 5abdd0c901..ece27a8661 100644 --- a/include/tudat/simulation/environment_setup/body.h +++ b/include/tudat/simulation/environment_setup/body.h @@ -2105,6 +2105,16 @@ class SystemOfBodies return bodyMap_.count( bodyName ); } + bool doesBodyExist( const std::string& bodyName ) const + { + return ( this->count( bodyName ) != 0 ); + } + + std::vector< std::string > getListOfBodies( ) const + { + return utilities::createVectorFromUnorderedMapKeys( bodyMap_ ); + } + int getNumberOfBodies( ) const { return bodyMap_.size( ); diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h index 29d8c4489e..3664f4a594 100644 --- a/include/tudat/simulation/estimation_setup/observations.h +++ b/include/tudat/simulation/estimation_setup/observations.h @@ -248,6 +248,11 @@ class ObservationCollection return observationSetStartAndSize_; } + std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > getObservationTypeAndLinkEndStartAndSize( ) + { + return observationTypeAndLinkEndStartAndSize_; + } + std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > getObservationSetStartAndSizePerLinkEndIndex( ) { return observationSetStartAndSizePerLinkEndIndex_; @@ -399,6 +404,8 @@ class ObservationCollection for( auto linkEndIterator : observationIterator.second ) { LinkEnds currentLinkEnds = linkEndIterator.first; + int currentLinkEndStartIndex = currentStartIndex; + int currentLinkEndSize = 0; for( unsigned int i = 0; i < linkEndIterator.second.size( ); i++ ) { int currentNumberOfObservables = linkEndIterator.second.at( i )->getNumberOfObservables( ); @@ -408,10 +415,13 @@ class ObservationCollection std::make_pair( currentStartIndex, currentObservableVectorSize ) ); currentStartIndex += currentObservableVectorSize; currentObservableTypeSize += currentObservableVectorSize; + currentLinkEndSize += currentObservableVectorSize; totalObservableSize_ += currentObservableVectorSize; totalNumberOfObservables_ += currentNumberOfObservables; } + observationTypeAndLinkEndStartAndSize_[ currentObservableType ][ currentLinkEnds ] = std::make_pair( + currentLinkEndStartIndex, currentLinkEndSize ); } observationTypeStartAndSize_[ currentObservableType ] = std::make_pair( currentTypeStartIndex, currentObservableTypeSize ); @@ -504,6 +514,8 @@ class ObservationCollection std::map< ObservableType, std::map< int, std::vector< std::pair< int, int > > > > observationSetStartAndSizePerLinkEndIndex_; + std::map< ObservableType, std::map< LinkEnds, std::pair< int, int > > > observationTypeAndLinkEndStartAndSize_; + std::map< ObservableType, std::pair< int, int > > observationTypeStartAndSize_; int totalObservableSize_; diff --git a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h index 8aa894d7d0..13cc03c38b 100644 --- a/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h +++ b/include/tudat/simulation/estimation_setup/orbitDeterminationManager.h @@ -1101,7 +1101,7 @@ class OrbitDeterminationManager integrateAndEstimateOrbit_ = false; } - propagatorSettings->getOutputSettingsBase( )->setCreateDependentVariablesInterface( true ); + propagatorSettings->getOutputSettingsBase( )->setUpdateDependentVariableInterpolator( true ); if( integrateAndEstimateOrbit_ ) { variationalEquationsSolver_ = simulation_setup::createVariationalEquationsSolver< ObservationScalarType, TimeType >( diff --git a/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h b/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h index e8da5ea0b4..2e2a864947 100644 --- a/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h +++ b/include/tudat/simulation/estimation_setup/variationalEquationsSolver.h @@ -795,7 +795,8 @@ class SingleArcVariationalEquationsSolver: public VariationalEquationsSolver< St // Create object that will contain and process the propagation results variationalPropagationResults_ = std::make_shared< SingleArcVariationalSimulationResults< StateScalarType, TimeType>>( - dynamicsSimulator_->getSingleArcPropagationResults( ), this->stateTransitionMatrixSize_, this->parameterVectorSize_ - this->stateTransitionMatrixSize_ ); + dynamicsSimulator_->getSingleArcPropagationResults( ), + this->stateTransitionMatrixSize_, this->parameterVectorSize_ - this->stateTransitionMatrixSize_ ); // Integrate variational equations from initial state estimate. if( integrateEquationsOnCreation ) @@ -1219,7 +1220,8 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta arcWiseStateTransitionMatrixSize_.at( i ), arcWiseParameterVectorSize_.at( i ) - arcWiseStateTransitionMatrixSize_.at( i ) ) ); } variationalPropagationResults_ = std::make_shared< MultiArcSimulationResults< SingleArcVariationalSimulationResults, StateScalarType, TimeType > >( - singleArcVariationalResults ); + singleArcVariationalResults, + dynamicsSimulator_->getMultiArcPropagationResults( )->getMultiArcDependentVariablesInterface( ) ); for( unsigned int i = 0; i < singleArcDynamicsSimulators.size( ); i++ ) @@ -1356,7 +1358,8 @@ class MultiArcVariationalEquationsSolver: public VariationalEquationsSolver< Sta { // Propagate variational equations and equations of motion concurrently - if( integrateEquationsConcurrently ) { + if( integrateEquationsConcurrently ) + { // Propagate dynamics and variational equations and store results in variationalPropagationResults_ object dynamicsSimulator_->template integrateEquationsOfMotion( variationalPropagationResults_, getInitialStateProvider( initialStateEstimate )); @@ -1816,6 +1819,9 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St void integrateVariationalAndDynamicalEquations( const VectorType& initialStateEstimate, const bool integrateEquationsConcurrently ) { + // TODO: do process depdendent variables in original multi-arc solver, do not process dependent variables in + // extended solver. Also add dependent variables to original multi-arc solver. + // Reset initial time and propagate multi-arc equations singleArcSolver_->integrateVariationalAndDynamicalEquations( initialStateEstimate.block( 0, 0, singleArcDynamicsSize_, 1 ), @@ -1841,8 +1847,15 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St removeSingleArcBodiesFromMultiArcSolultion( numericalMultiArcSolution ); originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->restartPropagation(); + // Reset original multi-arc bodies' dynamics - originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults( numericalMultiArcSolution ); + originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults( + numericalMultiArcSolution ); + originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetSecondaryData( + multiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( ) ); + + + originalMultiArcSolver_->getDynamicsSimulator( )->processNumericalEquationsOfMotionSolution( ); // Create state transition matrix if not yet created. @@ -1903,6 +1916,8 @@ class HybridArcVariationalEquationsSolver: public VariationalEquationsSolver< St // Reset original multi-arc bodies' dynamics originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetPropagationResults( numericalMultiArcSolution ); + originalMultiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( )->manuallySetSecondaryData( + multiArcSolver_->getDynamicsSimulator( )->getMultiArcPropagationResults( ) ); originalMultiArcSolver_->getDynamicsSimulator( )->processNumericalEquationsOfMotionSolution( ); } diff --git a/include/tudat/simulation/propagation_setup/dependentVariablesInterface.h b/include/tudat/simulation/propagation_setup/dependentVariablesInterface.h index 74e4c8530f..ee06952df4 100644 --- a/include/tudat/simulation/propagation_setup/dependentVariablesInterface.h +++ b/include/tudat/simulation/propagation_setup/dependentVariablesInterface.h @@ -42,22 +42,9 @@ class DependentVariablesInterface * Constructor * \param dependentVariablesSettings Vector of single dependent variable settings */ - DependentVariablesInterface( - const std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesSettings ): - dependentVariablesSettings_( dependentVariablesSettings ) + DependentVariablesInterface( ) { - dependentVariablesSize_ = 0; - - dependentVariablesIdsAndIndices_.clear( ); - - for ( unsigned int i= 0 ; i < dependentVariablesSettings_.size( ) ; i++ ) - { - dependentVariablesTypes_.push_back( dependentVariablesSettings_[ i ]->dependentVariableType_ ); - dependentVariablesIdsAndIndices_[ getDependentVariableId( dependentVariablesSettings_[ i ] ) ] = dependentVariablesSize_; - - dependentVariablesSize_ += getDependentVariableSaveSize( dependentVariablesSettings_[ i ] ); - } } //! Destructor. @@ -71,89 +58,18 @@ class DependentVariablesInterface */ virtual Eigen::VectorXd getDependentVariables( const TimeType evaluationTime ) = 0; - //! Function to get the value of a single dependent variable at a given time. - Eigen::VectorXd getSingleDependentVariable( - const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, - const TimeType evaluationTime ) - { - Eigen::VectorXd dependentVariable = Eigen::VectorXd( getDependentVariableSaveSize( dependentVariableSettings ) ); - - // Retrieve ID and index of dependent variable of interest. - std::string dependentVariableId = getDependentVariableId( dependentVariableSettings ); - int dependentVariableIndex = dependentVariablesIdsAndIndices_.find( dependentVariableId )->second; - - // Retrieve full vector of dependent variables at a given time. - Eigen::VectorXd fullDependentVariablesVector = getDependentVariables( evaluationTime ); - - dependentVariable = - fullDependentVariablesVector.segment( dependentVariableIndex, getDependentVariableSaveSize( dependentVariableSettings ) ); - - return dependentVariable; - } - - //! Function to get the value of a single dependent variable at a given time, from the dependent variable ID. - Eigen::VectorXd getSingleDependentVariable( - const std::string dependentVariableId, - const int dependentVariableSize, - const TimeType evaluationTime ) - { - Eigen::VectorXd dependentVariable = Eigen::VectorXd( dependentVariableSize ); - - // Retrieve index of dependent variable of interest. - int dependentVariableIndex = dependentVariablesIdsAndIndices_.find( dependentVariableId )->second; - - // Retrieve full vector of dependent variables at a given time. - Eigen::VectorXd fullDependentVariablesVector = getDependentVariables( evaluationTime ); - - dependentVariable = - fullDependentVariablesVector.segment( dependentVariableIndex, dependentVariableSize ); - - return dependentVariable; - } - - - - //! Function to get the size of the dependent variables - /*! - * Function to get the size of the dependent variables - * \return Size of dependent variables - */ - int getDependentVariablesize( ) - { - return dependentVariablesSize_; - } - - //! Function to retrieve vector of single dependent variables settings. - /*! - * Function to retrieve the dependent variables settings object. - * \return dependent variables settings - */ - std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > getDependentVariablesSettings( ) - { - return dependentVariablesSettings_; - } - - - //! Function to retrieve the map containing the dependent variables Ids and indices. - std::map< std::string, int > getDependentVariablesIdsAndIndices( ) - { - return dependentVariablesIdsAndIndices_; - } - + virtual Eigen::VectorXd getSingleDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const TimeType evaluationTime ) = 0; +// +// //! Function to get the value of a single dependent variable at a given time, from the dependent variable ID. +// virtual Eigen::VectorXd getSingleDependentVariable( +// const std::string dependentVariableId, +// const int dependentVariableSize, +// const TimeType evaluationTime ) = 0; protected: - //! Vector of single dependent variable settings objects - std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesSettings_; - - //! Type of the dependent variables of interest - std::vector< PropagationDependentVariables > dependentVariablesTypes_; - - //! Size of dependent variable vector - int dependentVariablesSize_; - - //! Map containing the dependent variables Ids and indices - std::map< std::string, int > dependentVariablesIdsAndIndices_; }; @@ -163,8 +79,6 @@ class SingleArcDependentVariablesInterface : public DependentVariablesInterface< { public: - using DependentVariablesInterface< TimeType >::dependentVariablesSize_; - //! Constructor /*! * Constructor @@ -173,11 +87,29 @@ class SingleArcDependentVariablesInterface : public DependentVariablesInterface< */ SingleArcDependentVariablesInterface( const std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > dependentVariablesInterpolator, - const std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesSettings ): - DependentVariablesInterface< TimeType >( dependentVariablesSettings ), - dependentVariablesInterpolator_( dependentVariablesInterpolator ) + const std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesSettings, + const std::map< std::pair< int, int >, std::string > dependentVariableIds, + const std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > orderedDependentVariableSettings ): + dependentVariablesSettings_( dependentVariablesSettings ), + dependentVariablesInterpolator_( dependentVariablesInterpolator ), + dependentVariableIds_( dependentVariableIds ), + orderedDependentVariableSettings_( orderedDependentVariableSettings ) { + dependentVariablesSize_ = 0; + dependentVariablesIdsAndIndices_.clear( ); + + for ( unsigned int i= 0 ; i < dependentVariablesSettings_.size( ) ; i++ ) + { + dependentVariablesTypes_.push_back( dependentVariablesSettings_[ i ]->dependentVariableType_ ); + dependentVariablesIdsAndIndices_[ getDependentVariableId( dependentVariablesSettings_[ i ] ) ] = dependentVariablesSize_; + dependentVariablesSize_ += getDependentVariableSaveSize( dependentVariablesSettings_[ i ] ); + } dependentVariables_ = Eigen::VectorXd::Zero( dependentVariablesSize_ ); + + if( dependentVariablesInterpolator_ != nullptr ) + { + dependentVariablesInterpolator_->resetBoundaryHandling( interpolators::throw_exception_at_boundary ); + } } //! Destructor. @@ -192,6 +124,10 @@ class SingleArcDependentVariablesInterface : public DependentVariablesInterface< const std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > dependentVariablesInterpolator ) { dependentVariablesInterpolator_ = dependentVariablesInterpolator; + if( dependentVariablesInterpolator_ != nullptr ) + { + dependentVariablesInterpolator_->resetBoundaryHandling( interpolators::throw_exception_at_boundary ); + } } //! Function to get the interpolator returning the dependent variables as a function of time. @@ -204,6 +140,16 @@ class SingleArcDependentVariablesInterface : public DependentVariablesInterface< return dependentVariablesInterpolator_; } + std::map< std::pair< int, int >, std::string > getDependentVariableIds( ) const + { + return dependentVariableIds_; + } + + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > getOrderedDependentVariableSettings( ) const + { + return orderedDependentVariableSettings_; + } + //! Function to get the concatenated dependent variables values at a given time. /*! * Function to get the concatenated dependent variables values at a given time. @@ -215,19 +161,107 @@ class SingleArcDependentVariablesInterface : public DependentVariablesInterface< dependentVariables_.setZero( ); // Set dependent variable. - dependentVariables_ = dependentVariablesInterpolator_->interpolate( evaluationTime ); + if( dependentVariablesInterpolator_ != nullptr ) + { + dependentVariables_ = dependentVariablesInterpolator_->interpolate( evaluationTime ); + } return dependentVariables_; } + //! Function to get the value of a single dependent variable at a given time. + Eigen::VectorXd getSingleDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const TimeType evaluationTime ) + { + Eigen::VectorXd dependentVariable = Eigen::VectorXd( 0 ); + + // Retrieve ID and index of dependent variable of interest. + std::string dependentVariableId = getDependentVariableId( dependentVariableSettings ); + if( dependentVariablesIdsAndIndices_.count( dependentVariableId ) > 0 ) + { + int dependentVariableIndex = dependentVariablesIdsAndIndices_.find( dependentVariableId )->second; + + // Retrieve full vector of dependent variables at a given time. + Eigen::VectorXd fullDependentVariablesVector = getDependentVariables( evaluationTime ); + + dependentVariable = fullDependentVariablesVector.segment( dependentVariableIndex, getDependentVariableSaveSize( dependentVariableSettings ) ); + } + return dependentVariable; + } + +// //! Function to get the value of a single dependent variable at a given time, from the dependent variable ID. +// Eigen::VectorXd getSingleDependentVariable( +// const std::string dependentVariableId, +// const int dependentVariableSize, +// const TimeType evaluationTime ) +// { +// Eigen::VectorXd dependentVariable = Eigen::VectorXd( dependentVariableSize ); +// +// // Retrieve index of dependent variable of interest. +// int dependentVariableIndex = dependentVariablesIdsAndIndices_.find( dependentVariableId )->second; +// +// // Retrieve full vector of dependent variables at a given time. +// Eigen::VectorXd fullDependentVariablesVector = getDependentVariables( evaluationTime ); +// +// dependentVariable = +// fullDependentVariablesVector.segment( dependentVariableIndex, dependentVariableSize ); +// +// return dependentVariable; +// } + + + + //! Function to get the size of the dependent variables + /*! + * Function to get the size of the dependent variables + * \return Size of dependent variables + */ + int getDependentVariablesize( ) + { + return dependentVariablesSize_; + } + + //! Function to retrieve vector of single dependent variables settings. + /*! + * Function to retrieve the dependent variables settings object. + * \return dependent variables settings + */ + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > getDependentVariablesSettings( ) const + { + return dependentVariablesSettings_; + } + + + //! Function to retrieve the map containing the dependent variables Ids and indices. + std::map< std::string, int > getDependentVariablesIdsAndIndices( ) + { + return dependentVariablesIdsAndIndices_; + } private: - //! Predefined vector to use as return value when calling getDependentVariables. - Eigen::VectorXd dependentVariables_; + + //! Vector of single dependent variable settings objects + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesSettings_; //! Interpolator returning the dependent variables as a function of time. std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > dependentVariablesInterpolator_; + std::map< std::pair< int, int >, std::string > dependentVariableIds_; + + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > orderedDependentVariableSettings_; + + //! Predefined vector to use as return value when calling getDependentVariables. + Eigen::VectorXd dependentVariables_; + + //! Type of the dependent variables of interest + std::vector< PropagationDependentVariables > dependentVariablesTypes_; + + //! Size of dependent variable vector + int dependentVariablesSize_; + + //! Map containing the dependent variables Ids and indices + std::map< std::string, int > dependentVariablesIdsAndIndices_; }; @@ -238,7 +272,6 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T { public: - using DependentVariablesInterface< TimeType >::dependentVariablesSize_; //! Constructor /*! @@ -249,13 +282,15 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T * \param arcEndTimes Times at which the multiple arcs end */ MultiArcDependentVariablesInterface( - const std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > > dependentVariablesInterpolators, - const std::vector< std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > > dependentVariablesSettings, + const std::vector< std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > > singleArcInterfaces, const std::vector< double >& arcStartTimes, const std::vector< double >& arcEndTimes ): - DependentVariablesInterface< TimeType >( dependentVariablesSettings.at( 0 ) ) + DependentVariablesInterface< TimeType >( ), + singleArcInterfaces_( singleArcInterfaces ), + arcStartTimes_( arcStartTimes ), + arcEndTimes_( arcEndTimes ) { - updateDependentVariablesInterpolators( dependentVariablesInterpolators, arcStartTimes, arcEndTimes ); + } //! Destructor @@ -273,7 +308,6 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T const std::vector< double >& arcStartTimes, const std::vector< double >& arcEndTimes ) { - if( arcStartTimes.size( ) != arcEndTimes.size( ) ) { throw std::runtime_error( "Error when updating MultiArcDependentVariablesInterface, incompatible time lists" ); @@ -284,12 +318,19 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T throw std::runtime_error( "Error when updating MultiArcDependentVariablesInterface, incompatible interpolator lists" ); } - dependentVariablesInterpolators_ = dependentVariablesInterpolators; + if( dependentVariablesInterpolators.size( ) != singleArcInterfaces_.size( ) ) + { + throw std::runtime_error( "Error when updating MultiArcDependentVariablesInterface, size of interpolator lists is incompatible with number of arcs" ); + + } + arcStartTimes_ = arcStartTimes; arcEndTimes_ = arcEndTimes; - numberArcs_ = arcStartTimes_.size( ); - + for( unsigned int i = 0; i < dependentVariablesInterpolators.size( ); i++ ) + { + singleArcInterfaces_.at( i )->updateDependentVariablesInterpolator( dependentVariablesInterpolators.at( i ) ); + } std::vector< double > arcSplitTimes = arcStartTimes_; arcSplitTimes.push_back( std::numeric_limits< double >::max( ) ); @@ -301,9 +342,9 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T * Function to get the vector of interpolators returning the dependent variables as a function of time. * \return Vector of interpolators returning the dependent variables as a function of time. */ - std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > > getDependentVariablesInterpolators( ) + std::vector< std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > > getSingleArcInterfaces( ) { - return dependentVariablesInterpolators_; + return singleArcInterfaces_; } //! Function to get the concatenated dependent variables values at a given time. @@ -314,19 +355,34 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T */ Eigen::VectorXd getDependentVariables( const TimeType evaluationTime ) { - Eigen::VectorXd dependentVariables = Eigen::VectorXd::Zero( dependentVariablesSize_ ); - + Eigen::VectorXd dependentVariables = Eigen::VectorXd::Zero( 0 ); int currentArc = getCurrentArc( evaluationTime ).first; // Set dependent variables vector. if( currentArc >= 0 ) { - dependentVariables.segment( 0, dependentVariablesSize_) = dependentVariablesInterpolators_.at( currentArc )->interpolate( evaluationTime ); + dependentVariables = singleArcInterfaces_.at( currentArc )->getDependentVariables( evaluationTime ); } return dependentVariables; } + //! Function to get the value of a single dependent variable at a given time. + Eigen::VectorXd getSingleDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const TimeType evaluationTime ) + { + Eigen::VectorXd dependentVariables = Eigen::VectorXd::Zero( 0 ); + int currentArc = getCurrentArc( evaluationTime ).first; + + // Set dependent variables vector. + if( currentArc >= 0 ) + { + dependentVariables = singleArcInterfaces_.at( currentArc )->getSingleDependentVariable( dependentVariableSettings, evaluationTime ); + } + + return dependentVariables; + } //! Function to retrieve the current arc for a given time /*! @@ -336,6 +392,11 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T */ std::pair< int, double > getCurrentArc( const TimeType evaluationTime ) { + if( lookUpscheme_ == nullptr ) + { + throw std::runtime_error( "Error when accessing multi-arc dependent variable interface; interface not yet set" ); + } + int currentArc = lookUpscheme_->findNearestLowerNeighbour( evaluationTime ); if( evaluationTime <= arcEndTimes_.at( currentArc ) && evaluationTime >= arcStartTimes_.at( currentArc ) ) { @@ -360,8 +421,7 @@ class MultiArcDependentVariablesInterface: public DependentVariablesInterface< T private: - //! List of interpolators returning the dependent variables as a function of time. - std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > > dependentVariablesInterpolators_; + std::vector< std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > > singleArcInterfaces_; //! Times at which the multiple arcs start std::vector< double > arcStartTimes_; @@ -398,73 +458,9 @@ class HybridArcDependentVariablesInterface: public DependentVariablesInterface< HybridArcDependentVariablesInterface( const std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > singleArcInterface, const std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > multiArcInterface ): - DependentVariablesInterface< TimeType >( multiArcInterface->getDependentVariablesSettings( ) ), + DependentVariablesInterface< TimeType >( ), singleArcInterface_( singleArcInterface ), multiArcInterface_( multiArcInterface ) { - singleArcDependentVariablesSize_ = 0; - multiArcDependentVariablesSize_ = 0; - numberArcs_ = 0; - - if ( singleArcInterface_ != nullptr ) - { - singleArcDependentVariablesIdsAndIndices_ = singleArcInterface_->getDependentVariablesIdsAndIndices( ); - singleArcDependentVariablesSize_ = singleArcInterface_->getDependentVariablesize( ); - } - std::map< std::string, int > multiArcDependentVariablesIdsAndIndices; - if ( multiArcInterface_ != nullptr ) - { - multiArcDependentVariablesIdsAndIndices = multiArcInterface_->getDependentVariablesIdsAndIndices( ); - multiArcDependentVariablesSize_ = multiArcInterface_->getDependentVariablesize( ) /*- singleArcDependentVariablesSize_*/; - numberArcs_ = multiArcInterface->getNumberOfArcs( ); - } - - // Check input consistency: verify that the same dependent variable is not saved twice (in both the single and multi-arc - // dependent variables interface) - if ( ( singleArcInterface != nullptr ) && ( multiArcInterface != nullptr ) ) - { - for ( std::map< std::string, int >::iterator itr = multiArcDependentVariablesIdsAndIndices.begin( ) ; - itr != multiArcDependentVariablesIdsAndIndices.end( ) ; itr++ ) - { - if ( singleArcDependentVariablesIdsAndIndices_.count( itr->first ) != 0 ) - { - // Remove dependent variable from multi-arc dependent variables interface. - idsAndIndicesMultiArcDependentVariablesToBeRemoved_[ itr->first ] = itr->second; - - std::cerr << "Warning when making hybrid dependent variables interface, dependent variable " - << itr->first << " required by the multi-arc interface is already included in the single arc interface." << std::endl; - } - } - } - - int sizeRemovedDependentVariables = 0; - int counterDependentVariable = 0; - for ( std::map< std::string, int >::iterator itr = multiArcDependentVariablesIdsAndIndices.begin( ) ; - itr != multiArcDependentVariablesIdsAndIndices.end( ) ; itr++ ) - { - if ( idsAndIndicesMultiArcDependentVariablesToBeRemoved_.count( itr->first ) == 0 ) - { - multiArcDependentVariablesIdsAndIndices_[ itr->first ] = itr->second - sizeRemovedDependentVariables; - } - else - { - sizeRemovedDependentVariables += getDependentVariableSaveSize( - multiArcInterface_->getDependentVariablesSettings( )[ counterDependentVariable ] ); - multiArcDependentVariablesSize_ -= getDependentVariableSaveSize( - multiArcInterface_->getDependentVariablesSettings( )[ counterDependentVariable ] ); - } - counterDependentVariable += 1; - } - - hybridArcDependentVariablesSize_ = singleArcDependentVariablesSize_ + multiArcDependentVariablesSize_; - -// // Reset dependent variables IDs and indices map. -// dependentVariablesIdsAndIndices_.clear( ); -// dependentVariablesIdsAndIndices_ = singleArcDependentVariablesIdsAndIndices_; -// for ( std::map< std::string, int >::iterator itr = multiArcDependentVariablesIdsAndIndices_.begin( ) ; -// itr != multiArcDependentVariablesIdsAndIndices_.end( ) ; itr++ ) -// { -// dependentVariablesIdsAndIndices_[ itr->first ] = itr->second + singleArcDependentVariablesSize_; -// } } //! Destructor @@ -478,61 +474,34 @@ class HybridArcDependentVariablesInterface: public DependentVariablesInterface< */ Eigen::VectorXd getDependentVariables( const TimeType evaluationTime ) { - Eigen::VectorXd dependentVariables = Eigen::VectorXd::Zero( hybridArcDependentVariablesSize_ ); + throw std::runtime_error( "Error when retrieving interpolated dependent variables from hybrid-arc interface. This functionality is not supported as the definition is ambiguous. Retreieve the single- or multi-arc interface, and retrieve the dependent variable from there." ); + return Eigen::VectorXd::Zero( 0 ); + } + + Eigen::VectorXd getSingleDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings, + const TimeType evaluationTime ) + { + Eigen::VectorXd dependentVariables = Eigen::VectorXd::Zero( 0 ); // Get single-arc dependent variables. - Eigen::VectorXd singleArcDependentVariables; - if ( singleArcInterface_ != nullptr ) - { - singleArcDependentVariables = singleArcInterface_->getDependentVariables( evaluationTime ); - } - else - { - singleArcDependentVariables = Eigen::VectorXd::Zero( 0 ); - } + Eigen::VectorXd singleArcDependentVariables = singleArcInterface_->getSingleDependentVariable( dependentVariableSettings, evaluationTime ); // Get multi-arc dependent variables. - Eigen::VectorXd multiArcDependentVariables = multiArcInterface_->getDependentVariables( evaluationTime ); - std::pair< int, double > currentArc = multiArcInterface_->getCurrentArc( evaluationTime ); - + Eigen::VectorXd multiArcDependentVariables = multiArcInterface_->getSingleDependentVariable( dependentVariableSettings, evaluationTime ); -// if ( ( multiArcDependentVariables.size( ) ) != hybridArcDependentVariablesSize_ ) -// { -// throw std::runtime_error( "Error when getting dependent variables from hybrid arc interfaces, size inconsistent " -// "with size of single- and multi-arc dependent variables." ); -// } - - dependentVariables.segment( 0, singleArcDependentVariablesSize_ ) = singleArcDependentVariables; - - if( !( currentArc.first < 0 ) ) + // Single-arc result takes preference over multi-arc result + if( singleArcDependentVariables.rows( ) > 0 ) { - dependentVariables.segment( singleArcDependentVariablesSize_, multiArcDependentVariablesSize_ ) = multiArcDependentVariables; - -// int currentMultiArcDependentVariableIndex = singleArcDependentVariablesSize_; -// -// for ( std::map< std::string, int >::iterator itr = multiArcDependentVariablesIdsAndIndices_.begin( ) ; -// itr != multiArcDependentVariablesIdsAndIndices_.end( ) ; itr++ ) -// { -// int sizeCurrentDependentVariable = 0; -// for ( unsigned int i = 0 ; -// i < multiArcInterface_->getDependentVariablesSettings( ).size( ) ; i++ ) -// { -// if ( itr->first == getDependentVariableId( multiArcInterface_->getDependentVariablesSettings( )[ i ] ) ) -// { -// sizeCurrentDependentVariable = getDependentVariableSaveSize( multiArcInterface_->getDependentVariablesSettings( )[ i ] ); -// } -// } -// -// dependentVariables.segment( currentMultiArcDependentVariableIndex, sizeCurrentDependentVariable ) = -// multiArcDependentVariables.segment( itr->second, sizeCurrentDependentVariable ); -// -// currentMultiArcDependentVariableIndex += sizeCurrentDependentVariable; -// } + dependentVariables = singleArcDependentVariables; + } + else if( multiArcDependentVariables.rows( ) > 0 ) + { + dependentVariables = multiArcDependentVariables; } return dependentVariables; } - private: //! Object to retrieve dependent variable for single arc component @@ -540,28 +509,6 @@ class HybridArcDependentVariablesInterface: public DependentVariablesInterface< //! Object to retrieve dependent variable for multi arc component std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > multiArcInterface_; - - //! Dependent variables IDs and indices for single-arc interface. - std::map< std::string, int > singleArcDependentVariablesIdsAndIndices_; - - //! Dependent variables IDs and indices for multi-arc interface. - std::map< std::string, int > multiArcDependentVariablesIdsAndIndices_; - - //! IDs and indices of dependent variables to be removed from multi-arc interface (in case already included in single arc - //! interface) - std::map< std::string, int > idsAndIndicesMultiArcDependentVariablesToBeRemoved_; - - //! Size of single-arc dependent variables - int singleArcDependentVariablesSize_; - - //! Size of multi-arc dependent variables. - int multiArcDependentVariablesSize_; - - //! Size of hybrid arc dependent variables. - int hybridArcDependentVariablesSize_; - - //! Number of arcs in multi-arc model. - int numberArcs_; }; diff --git a/include/tudat/simulation/propagation_setup/dynamicsSimulator.h b/include/tudat/simulation/propagation_setup/dynamicsSimulator.h index 2d3913d70a..d4a70160f2 100644 --- a/include/tudat/simulation/propagation_setup/dynamicsSimulator.h +++ b/include/tudat/simulation/propagation_setup/dynamicsSimulator.h @@ -566,7 +566,7 @@ std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > vali const bool printNumberOfFunctionEvaluations = false, const bool printDependentVariableData = false, const bool printStateData = false, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > singleArcPropagatorSettings = std::dynamic_pointer_cast< SingleArcPropagatorSettings< StateScalarType, TimeType > >( propagatorSettings ); @@ -592,7 +592,7 @@ std::shared_ptr< SingleArcPropagatorSettings< StateScalarType, TimeType > > vali singleArcPropagatorSettings->getOutputSettings( )->setClearNumericalSolutions( clearNumericalSolutions ); singleArcPropagatorSettings->getOutputSettings( )->setIntegratedResult( setIntegratedResult ); - singleArcPropagatorSettings->getOutputSettings( )->setCreateDependentVariablesInterface( setDependentVariablesInterface ); + singleArcPropagatorSettings->getOutputSettings( )->setUpdateDependentVariableInterpolator( updateDependentVariableInterpolator ); singleArcPropagatorSettings->getOutputSettings( )->getPrintSettings( )->reset( printNumberOfFunctionEvaluations, printDependentVariableData, singleArcPropagatorSettings->getOutputSettings( )->getPrintSettings( )->getResultsPrintFrequencyInSeconds( ), 0, @@ -759,12 +759,15 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim getIntegratedTypeAndBodyList( propagatorSettings_ ); std::map< std::pair< int, int >, std::string > dependentVariableIds_; + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > orderedDependentVariableSettings_; + // Create functions that compute the dependent variables if( propagatorSettings_->getDependentVariablesToSave( ).size( ) > 0 ) { std::pair< std::function< Eigen::VectorXd( ) >, std::map< std::pair< int, int >, std::string > > dependentVariableData = createDependentVariableListFunction< TimeType, StateScalarType >( propagatorSettings_->getDependentVariablesToSave( ), bodies_, + orderedDependentVariableSettings_, dynamicsStateDerivative_->getStateDerivativeModels( ), predefinedStateDerivativeModels.stateDerivativePartials_ ); dependentVariablesFunctions_ = dependentVariableData.first; @@ -772,15 +775,15 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim } // Create object that will contain and process the propagation results - std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > dependentVariableInterface = nullptr; - if ( propagatorSettings_->getOutputSettingsWithCheck( )->getCreateDependentVariablesInterface( ) ) - { - dependentVariableInterface = std::make_shared< SingleArcDependentVariablesInterface< TimeType > >( - std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > >( ), - propagatorSettings_->getDependentVariablesToSave( ) ); - } + std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > dependentVariableInterface = + std::make_shared< SingleArcDependentVariablesInterface< TimeType > >( + std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > >( ), + propagatorSettings_->getDependentVariablesToSave( ), + dependentVariableIds_, + orderedDependentVariableSettings_ ); + propagationResults_= std::make_shared< SingleArcSimulationResults< StateScalarType, TimeType > >( - dependentVariableIds_, integratedStateAndBodyList, propagatorSettings_->getOutputSettingsWithCheck( ), + integratedStateAndBodyList, propagatorSettings_->getOutputSettingsWithCheck( ), std::bind( &DynamicsStateDerivativeModel< TimeType, StateScalarType >::convertNumericalStateSolutionsToOutputSolutions, dynamicsStateDerivative_, std::placeholders::_1, std::placeholders::_2 ), dependentVariableInterface, sequentialPropagation_ ) ; @@ -824,11 +827,11 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim const std::chrono::steady_clock::time_point initialClockTime = std::chrono::steady_clock::now( ), const bool printDependentVariableData = false, const bool printStateData = false, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): SingleArcDynamicsSimulator( bodies, validateDeprecatedSingleArcSettings( integratorSettings, propagatorSettings, clearNumericalSolutions, setIntegratedResult, printNumberOfFunctionEvaluations, - printDependentVariableData, printStateData, setDependentVariablesInterface ), + printDependentVariableData, printStateData, updateDependentVariableInterpolator ), areEquationsOfMotionToBeIntegrated, predefinedStateDerivativeModels ){ } @@ -842,7 +845,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim const bool printNumberOfFunctionEvaluations = false, const bool printDependentVariableData = false, const bool printStateData = false, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): SingleArcDynamicsSimulator( bodies, integratorSettings, propagatorSettings, PredefinedSingleArcStateDerivativeModels< StateScalarType, TimeType >( ), areEquationsOfMotionToBeIntegrated, @@ -852,7 +855,7 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim std::chrono::steady_clock::now( ), printDependentVariableData, printStateData, - setDependentVariablesInterface ){ } + updateDependentVariableInterpolator ){ } //! Destructor ~SingleArcDynamicsSimulator( ) { } @@ -1081,7 +1084,11 @@ class SingleArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim { propagationResults_->clearSolutionMaps( ); } - propagationResults_->updateDependentVariableInterface( ); + + if( propagatorSettings_->getOutputSettings( )->getUpdateDependentVariableInterpolator( ) ) + { + propagationResults_->updateDependentVariableInterface( ); + } } void suppressDependentVariableDataPrinting( ) @@ -1473,7 +1480,7 @@ std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > valid const std::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, const bool clearNumericalSolutions = false, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > multiArcPropagatorSettings = std::dynamic_pointer_cast< MultiArcPropagatorSettings< StateScalarType, TimeType > >( propagatorSettings ); @@ -1521,7 +1528,7 @@ std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > valid } multiArcPropagatorSettings->getOutputSettings( )->setClearNumericalSolutions( clearNumericalSolutions ); multiArcPropagatorSettings->getOutputSettings( )->setIntegratedResult( setIntegratedResult ); - multiArcPropagatorSettings->getOutputSettings( )->setCreateDependentVariablesInterface( setDependentVariablesInterface ); + multiArcPropagatorSettings->getOutputSettings( )->setUpdateDependentVariableInterpolator( updateDependentVariableInterpolator ); return multiArcPropagatorSettings; } @@ -1533,7 +1540,7 @@ std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > valid const std::vector< double > propagationStartTimes, const bool clearNumericalSolutions = false, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > multiArcPropagatorSettings = std::dynamic_pointer_cast< MultiArcPropagatorSettings< StateScalarType, TimeType > >( propagatorSettings ); @@ -1555,7 +1562,7 @@ std::shared_ptr< MultiArcPropagatorSettings< StateScalarType, TimeType > > valid } return validateDeprecatedMultiArcSettings( - independentIntegratorSettingsList, propagatorSettings, clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ); + independentIntegratorSettingsList, propagatorSettings, clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ); } template< typename StateScalarType = double > @@ -1715,19 +1722,16 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time singleArcDynamicsSimulators_.at( i )->createAndSetIntegratedStateProcessors( ); } - std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > dependentVariableInterface = nullptr; - if ( multiArcPropagatorSettings_->getOutputSettings( )->getCreateDependentVariablesInterface( ) ) + std::vector< std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > > singleArcInterfaces; + for ( unsigned int i = 0 ; i < singleArcSettings.size( ) ; i++ ) { - std::vector< std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > > dependentVariablesSettings; - for ( unsigned int i = 0 ; i < singleArcSettings.size( ) ; i++ ) - { - dependentVariablesSettings.push_back( multiArcPropagatorSettings_->getSingleArcSettings( ).at( i )->getDependentVariablesToSave( ) ); - } - - dependentVariableInterface = std::make_shared< MultiArcDependentVariablesInterface< TimeType > >( - std::vector< std::shared_ptr< interpolators::OneDimensionalInterpolator< TimeType, Eigen::VectorXd > > >( ), - dependentVariablesSettings, std::vector< double >( ), std::vector< double >( ) ); + singleArcInterfaces.push_back( singleArcDynamicsSimulators_.at( i )->getSingleArcPropagationResults( )->getSingleArcDependentVariablesInterface( ) ); } + + std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > dependentVariableInterface = + std::make_shared< MultiArcDependentVariablesInterface< TimeType > >( + singleArcInterfaces, std::vector< double >( ), std::vector< double >( ) ); + propagationResults_ = std::make_shared( singleArcResults, dependentVariableInterface ); // Integrate equations of motion if required. @@ -1761,10 +1765,10 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time const bool areEquationsOfMotionToBeIntegrated = true, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): MultiArcDynamicsSimulator( bodies, validateDeprecatedMultiArcSettings( integratorSettings, propagatorSettings, arcStartTimes, - clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ), + clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), areEquationsOfMotionToBeIntegrated ){ } @@ -1788,10 +1792,10 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time const bool areEquationsOfMotionToBeIntegrated = true, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): MultiArcDynamicsSimulator( bodies, validateDeprecatedMultiArcSettings( integratorSettings, propagatorSettings, - clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ), + clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), areEquationsOfMotionToBeIntegrated ){ } //! Destructor @@ -1964,7 +1968,8 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time std::shared_ptr > > multiArcStateProcessors = createMultiArcIntegratedStateProcessors( bodies_, propagationResults_->getArcStartTimes( ), singleArcIntegratedStatesProcessors ); - for ( auto itr: multiArcStateProcessors ) { + for ( auto itr: multiArcStateProcessors ) + { itr.second->processIntegratedMultiArcStates( propagationResults_->getConcatenatedEquationsOfMotionResults( multiArcPropagatorSettings_->getOutputSettings( )->getClearNumericalSolutions( )), @@ -1990,7 +1995,10 @@ class MultiArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Time } // Reset dependent variables interface - propagationResults_->updateDependentVariableInterface( ); + if( multiArcPropagatorSettings_->getOutputSettings( )->getUpdateDependentVariableInterpolator( ) ) + { + propagationResults_->updateDependentVariableInterface( ); + } } @@ -2141,7 +2149,7 @@ std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > vali const std::shared_ptr< PropagatorSettings< StateScalarType > > propagatorSettings, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > hybridArcPropagatorSettings = std::dynamic_pointer_cast< HybridArcPropagatorSettings< StateScalarType, TimeType > >( propagatorSettings ); @@ -2154,7 +2162,7 @@ std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > vali singleArcIntegratorSettings, hybridArcPropagatorSettings->getSingleArcPropagatorSettings( ) ); validateDeprecatedMultiArcSettings< StateScalarType, TimeType >( multiArcIntegratorSettings, hybridArcPropagatorSettings->getMultiArcPropagatorSettings( ), - clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ); + clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ); hybridArcPropagatorSettings->getOutputSettingsWithCheck( )->setClearNumericalSolutions( clearNumericalSolutions ); hybridArcPropagatorSettings->getOutputSettingsWithCheck( )->setIntegratedResult( setIntegratedResult ); @@ -2170,7 +2178,7 @@ std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > vali const std::vector< double > arcStartTimes, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > hybridArcPropagatorSettings = std::dynamic_pointer_cast< HybridArcPropagatorSettings< StateScalarType, TimeType > >( propagatorSettings ); @@ -2190,7 +2198,7 @@ std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > vali } hybridArcPropagatorSettings->getSingleArcPropagatorSettings( )->resetInitialTime( singleArcIntegratorSettings->initialTimeDeprecated_ ); return validateDeprecatedHybridArcSettings( singleArcIntegratorSettings, independentIntegratorSettingsList, - propagatorSettings, clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ); + propagatorSettings, clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ); } template< typename StateScalarType = double, typename TimeType = double > @@ -2200,11 +2208,11 @@ std::shared_ptr< HybridArcPropagatorSettings< StateScalarType, TimeType > > vali const std::vector< double > arcStartTimes, const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, - const bool setDependentVariablesInterface = false ) + const bool updateDependentVariableInterpolator = false ) { return validateDeprecatedHybridArcSettings( integratorSettings, utilities::deepcopyPointer( integratorSettings ), propagatorSettings, - arcStartTimes, clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ); + arcStartTimes, clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ); } @@ -2253,8 +2261,7 @@ class HybridArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim false ); propagationResults_ = std::make_shared< HybridArcResults >( singleArcDynamicsSimulator_->getSingleArcPropagationResults( ), - multiArcDynamicsSimulator_->getMultiArcPropagationResults( ), - hybridPropagatorSettings_->getOutputSettings( )->getCreateDependentVariablesInterface( ) ); + multiArcDynamicsSimulator_->getMultiArcPropagationResults( ) ); } else { @@ -2292,10 +2299,10 @@ class HybridArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, const bool addSingleArcBodiesToMultiArcDynamics = false, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): HybridArcDynamicsSimulator( bodies, validateDeprecatedHybridArcSettings( integratorSettings, propagatorSettings, arcStartTimes, - clearNumericalSolutions, setIntegratedResult, setDependentVariablesInterface ), + clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), areEquationsOfMotionToBeIntegrated, addSingleArcBodiesToMultiArcDynamics ){ } @@ -2309,11 +2316,11 @@ class HybridArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim const bool clearNumericalSolutions = true, const bool setIntegratedResult = true, const bool addSingleArcBodiesToMultiArcDynamics = false, - const bool setDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): HybridArcDynamicsSimulator( bodies, validateDeprecatedHybridArcSettings( singleArcIntegratorSettings, multiArcIntegratorSettings, propagatorSettings, arcStartTimes, clearNumericalSolutions, setIntegratedResult ), - areEquationsOfMotionToBeIntegrated, addSingleArcBodiesToMultiArcDynamics, setDependentVariablesInterface ){ } + areEquationsOfMotionToBeIntegrated, addSingleArcBodiesToMultiArcDynamics, updateDependentVariableInterpolator ){ } //! Destructor ~HybridArcDynamicsSimulator( ){ } @@ -2347,7 +2354,10 @@ class HybridArcDynamicsSimulator: public DynamicsSimulator< StateScalarType, Tim { singleArcDynamicsSimulator_->processNumericalEquationsOfMotionSolution( ); multiArcDynamicsSimulator_->processNumericalEquationsOfMotionSolution( ); - propagationResults_->updateDependentVariableInterface( ); + if( hybridPropagatorSettings_->getOutputSettings( )->getUpdateDependentVariableInterpolator( ) ) + { + propagationResults_->updateDependentVariableInterface( ); + } } //! Function to retrieve the single-arc dynamics simulator diff --git a/include/tudat/simulation/propagation_setup/propagationOutput.h b/include/tudat/simulation/propagation_setup/propagationOutput.h index f4057ceca1..8ab72c24c4 100644 --- a/include/tudat/simulation/propagation_setup/propagationOutput.h +++ b/include/tudat/simulation/propagation_setup/propagationOutput.h @@ -122,9 +122,15 @@ int getDependentVariableSaveSize( int getDependentVariableSize( const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ); +std::pair< int, int > getDependentVariableShape( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ); + bool isScalarDependentVariable( const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ); +bool isMatrixDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ); + //! Get the vector representation of a rotation matrix. /*! * Get the vector representation of a rotation matrix. @@ -2549,10 +2555,9 @@ template< typename TimeType = double, typename StateScalarType = double > std::pair< std::function< Eigen::VectorXd( ) >, std::map< std::pair< int, int >, std::string > > createDependentVariableListFunction( const std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables, const simulation_setup::SystemOfBodies& bodies, + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > >& orderedDependentVariables, const std::unordered_map< IntegratedStateType, - std::vector< std::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >& stateDerivativeModels = - std::unordered_map< IntegratedStateType, - std::vector< std::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >( ), + std::vector< std::shared_ptr< SingleStateTypeDerivative< StateScalarType, TimeType > > > >& stateDerivativeModels, const std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap >& stateDerivativePartials = std::map< propagators::IntegratedStateType, orbit_determination::StateDerivativePartialsMap >( ) ) { @@ -2593,9 +2598,13 @@ std::pair< std::function< Eigen::VectorXd( ) >, std::map< std::pair< int, int >, // Set list of variable ids/indices in correc otder. int totalVariableSize = 0; std::map< std::pair< int, int >, std::string > dependentVariableIds; + + int variableCounter = 0; for( std::pair< std::string, int > vectorVariable: vectorVariableList ) { dependentVariableIds[ { totalVariableSize, vectorVariable.second } ] = vectorVariable.first; + orderedDependentVariables[ { totalVariableSize, vectorVariable.second } ] = dependentVariables.at( variableCounter ); + variableCounter++; totalVariableSize += vectorVariable.second; } diff --git a/include/tudat/simulation/propagation_setup/propagationProcessingSettings.h b/include/tudat/simulation/propagation_setup/propagationProcessingSettings.h index 7b5ab6b0fc..895918be78 100644 --- a/include/tudat/simulation/propagation_setup/propagationProcessingSettings.h +++ b/include/tudat/simulation/propagation_setup/propagationProcessingSettings.h @@ -38,10 +38,10 @@ class PropagatorProcessingSettings PropagatorProcessingSettings( const bool clearNumericalSolutions = false, const bool setIntegratedResult = false, - const bool createDependentVariablesInterface = false ): + const bool updateDependentVariableInterpolator = false ): clearNumericalSolutions_( clearNumericalSolutions ), setIntegratedResult_( setIntegratedResult ), - createDependentVariablesInterface_( createDependentVariablesInterface ) + updateDependentVariableInterpolator_( updateDependentVariableInterpolator ) { } virtual ~PropagatorProcessingSettings( ){ } @@ -56,9 +56,9 @@ class PropagatorProcessingSettings return setIntegratedResult_; } - bool getCreateDependentVariablesInterface( ) + bool getUpdateDependentVariableInterpolator( ) { - return createDependentVariablesInterface_; + return updateDependentVariableInterpolator_; } virtual void setClearNumericalSolutions( const bool clearNumericalSolutions ) @@ -71,9 +71,9 @@ class PropagatorProcessingSettings setIntegratedResult_ = setIntegratedResult; } - virtual void setCreateDependentVariablesInterface( const bool createDependentVariablesInterface ) + virtual void setUpdateDependentVariableInterpolator( const bool updateDependentVariableInterpolator ) { - createDependentVariablesInterface_ = createDependentVariablesInterface; + updateDependentVariableInterpolator_ = updateDependentVariableInterpolator; } virtual bool printAnyOutput( ) = 0; @@ -86,7 +86,7 @@ class PropagatorProcessingSettings bool clearNumericalSolutions_; bool setIntegratedResult_; - bool createDependentVariablesInterface_; + bool updateDependentVariableInterpolator_; }; //! Base class for defining output and processing settings for single-arc propagation. @@ -103,8 +103,8 @@ class SingleArcPropagatorProcessingSettings: public PropagatorProcessingSettings const double resultsSaveFrequencyInSeconds = TUDAT_NAN, const std::shared_ptr< PropagationPrintSettings > printSettings = std::make_shared< PropagationPrintSettings >( ), - const bool createDependentVariablesInterface = false ): - PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, createDependentVariablesInterface ), + const bool updateDependentVariableInterpolator = false ): + PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), resultsSaveFrequencyInSteps_( resultsSaveFrequencyInSteps ), resultsSaveFrequencyInSeconds_( resultsSaveFrequencyInSeconds ), printSettings_( printSettings ), @@ -217,8 +217,8 @@ class MultiArcPropagatorProcessingSettings: public PropagatorProcessingSettings const bool setIntegratedResult = false, const bool printFirstArcOnly = false, const bool printCurrentArcIndex = false, - const bool createDependentVariablesInterface = false ): - PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, createDependentVariablesInterface ), + const bool updateDependentVariableInterpolator = false ): + PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), consistentSingleArcPrintSettings_( consistentSingleArcPrintSettings ), useIdenticalSettings_( true ), printFirstArcOnly_( printFirstArcOnly ), @@ -233,8 +233,8 @@ class MultiArcPropagatorProcessingSettings: public PropagatorProcessingSettings const bool setIntegratedResult = false, const bool printFirstArcOnly = false, const bool printCurrentArcIndex = false, - const bool createDependentVariablesInterface = false ): - PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, createDependentVariablesInterface ), + const bool updateDependentVariableInterpolator = false ): + PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), consistentSingleArcPrintSettings_( nullptr ), useIdenticalSettings_( false ), printFirstArcOnly_( printFirstArcOnly ), @@ -427,8 +427,8 @@ class HybridArcPropagatorProcessingSettings: public PropagatorProcessingSettings const bool clearNumericalSolutions = false, const bool setIntegratedResult = false, const bool printStateTypeStart = false, - const bool createDependentVariablesInterface = false ): - PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, createDependentVariablesInterface ), + const bool updateDependentVariableInterpolator = false ): + PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), consistentArcPrintSettings_( consistentArcPrintSettings ), useIdenticalSettings_( true ), printStateTypeStart_( printStateTypeStart ){ } @@ -437,8 +437,8 @@ class HybridArcPropagatorProcessingSettings: public PropagatorProcessingSettings const bool clearNumericalSolutions = false, const bool setIntegratedResult = false, const bool printStateTypeStart = false, - const bool createDependentVariablesInterface = false ): - PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, createDependentVariablesInterface ), + const bool updateDependentVariableInterpolator = false ): + PropagatorProcessingSettings( clearNumericalSolutions, setIntegratedResult, updateDependentVariableInterpolator ), useIdenticalSettings_( false ), printStateTypeStart_( printStateTypeStart ){ } @@ -458,6 +458,13 @@ class HybridArcPropagatorProcessingSettings: public PropagatorProcessingSettings multiArcSettings_->setIntegratedResult( setIntegratedResult ); } + virtual void setUpdateDependentVariableInterpolator( const bool updateDependentVariableInterpolator ) + { + this->updateDependentVariableInterpolator_ = updateDependentVariableInterpolator; + singleArcSettings_->setUpdateDependentVariableInterpolator( updateDependentVariableInterpolator ); + multiArcSettings_->setUpdateDependentVariableInterpolator( updateDependentVariableInterpolator ); + } + void resetArcSettings( const bool printWarning = false ) { if( !areArcSettingsSet_ ) diff --git a/include/tudat/simulation/propagation_setup/propagationResults.h b/include/tudat/simulation/propagation_setup/propagationResults.h index 51715b618f..0fdb9ef50a 100644 --- a/include/tudat/simulation/propagation_setup/propagationResults.h +++ b/include/tudat/simulation/propagation_setup/propagationResults.h @@ -71,15 +71,13 @@ namespace tudat static const bool is_variational = false; static const int number_of_columns = 1; - SingleArcSimulationResults(const std::map , std::string> &dependentVariableIds, - const std::map< IntegratedStateType, std::vector< std::tuple< std::string, std::string, PropagatorType > > > integratedStateAndBodyList, + SingleArcSimulationResults(const std::map< IntegratedStateType, std::vector< std::tuple< std::string, std::string, PropagatorType > > > integratedStateAndBodyList, const std::shared_ptr &outputSettings, const std::function< void ( std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >&, const std::map< TimeType, Eigen::Matrix< StateScalarType, Eigen::Dynamic, 1 > >& ) > rawSolutionConversionFunction, - const std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > dependentVariableInterface = nullptr, + const std::shared_ptr< SingleArcDependentVariablesInterface< TimeType > > dependentVariableInterface, const bool sequentialPropagation = true ) : SimulationResults(), - dependentVariableIds_(dependentVariableIds), processedStateIds_(getProcessedStateStrings( integratedStateAndBodyList ) ), propagatedStateIds_( getPropagatedStateStrings( integratedStateAndBodyList ) ), integratedStateAndBodyList_( integratedStateAndBodyList ), @@ -93,14 +91,6 @@ namespace tudat propagationTerminationReason_( std::make_shared(propagation_never_run)) { - if( outputSettings->getCreateDependentVariablesInterface( ) && ( dependentVariableInterface == nullptr ) ) - { - throw std::runtime_error( "Error, inconsistent dependent variable settings interface input (pointer is null) for single-arc results" ); - } - else if( !outputSettings->getCreateDependentVariablesInterface( ) && ( dependentVariableInterface != nullptr ) ) - { - throw std::runtime_error( "Error, inconsistent dependent variable settings interface input (pointer is not null) for single-arc results" ); - } } //! Function that resets the state of this object, typically to signal that a new propagation is to be performed. @@ -112,6 +102,15 @@ namespace tudat onlyProcessedSolutionSet_ = false; propagationTerminationReason_ = std::make_shared(propagation_never_run); } + + void manuallySetSecondaryData( const std::shared_ptr< SingleArcSimulationResults< StateScalarType, TimeType > > resultsToCopy ) + { + dependentVariableHistory_ = resultsToCopy->getDependentVariableHistory( ); + cumulativeComputationTimeHistory_ = resultsToCopy->getCumulativeComputationTimeHistory( ); + cumulativeNumberOfFunctionEvaluations_ = resultsToCopy->getCumulativeNumberOfFunctionEvaluations( ); + propagationTerminationReason_ = resultsToCopy->getPropagationTerminationReason( ); + propagationIsPerformed_ = true; + } //! Function that sets new numerical results of a propagation, after the propagation of the dynamics void reset( @@ -220,39 +219,39 @@ namespace tudat return equationsOfMotionNumericalSolutionRaw_; } - std::map &getDependentVariableHistory( ) + std::map &getDependentVariableHistory( ) { - checkAvailabilityOfSolution( "dependent variable history" ); + checkAvailabilityOfSolution( "dependent variable history", false ); return dependentVariableHistory_; } - std::map &getCumulativeComputationTimeHistory( ) + std::map &getCumulativeComputationTimeHistory( ) { - checkAvailabilityOfSolution( "cumulative computation time history" ); + checkAvailabilityOfSolution( "cumulative computation time history", false ); return cumulativeComputationTimeHistory_; } double getTotalComputationRuntime( ) { - checkAvailabilityOfSolution( "cumulative computation time history" ); + checkAvailabilityOfSolution( "cumulative computation time history", false ); return std::max( cumulativeComputationTimeHistory_.begin( )->second, cumulativeComputationTimeHistory_.rbegin( )->second ); } - std::map &getCumulativeNumberOfFunctionEvaluations( ) + std::map &getCumulativeNumberOfFunctionEvaluations( ) { - checkAvailabilityOfSolution( "cumulative number of function evaluations" ); + checkAvailabilityOfSolution( "cumulative number of function evaluations", false ); return cumulativeNumberOfFunctionEvaluations_; } double getTotalNumberOfFunctionEvaluations( ) { - checkAvailabilityOfSolution( "cumulative number of function evaluations" ); + checkAvailabilityOfSolution( "cumulative number of function evaluations", false ); return std::max( cumulativeNumberOfFunctionEvaluations_.begin( )->second, cumulativeNumberOfFunctionEvaluations_.rbegin( )->second ); } - std::shared_ptr getPropagationTerminationReason( ) + std::shared_ptr getPropagationTerminationReason( ) { return propagationTerminationReason_; } @@ -264,9 +263,40 @@ namespace tudat } - std::map , std::string> getDependentVariableId( ) + std::map , std::string> getDependentVariableId( ) const { - return dependentVariableIds_; + if( dependentVariableInterface_ != nullptr ) + { + return dependentVariableInterface_->getDependentVariableIds( ); + } + else + { + return std::map , std::string>( ); + } + } + + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > getOrderedDependentVariableSettings( ) const + { + if( dependentVariableInterface_ != nullptr ) + { + return dependentVariableInterface_->getOrderedDependentVariableSettings( ); + } + else + { + return std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > >( ); + } + } + + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > getOriginalDependentVariableSettings( ) const + { + if( dependentVariableInterface_ != nullptr ) + { + return dependentVariableInterface_->getDependentVariablesSettings( ); + } + else + { + return std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > >( ); + } } std::map , std::string> getProcessedStateIds( ) @@ -371,9 +401,6 @@ namespace tudat //! Map of cumulative number of function evaluations that was saved during numerical propagation. std::map cumulativeNumberOfFunctionEvaluations_; - //! Map listing starting entry of dependent variables in output vector, along with associated ID. - std::map , std::string> dependentVariableIds_; - std::map , std::string> processedStateIds_; std::map , std::string> propagatedStateIds_; @@ -396,7 +423,7 @@ namespace tudat bool propagationIsPerformed_; bool solutionIsCleared_; - + bool onlyProcessedSolutionSet_; //! Event that triggered the termination of the propagation @@ -423,11 +450,14 @@ namespace tudat SingleArcVariationalSimulationResults( const std::shared_ptr > singleArcDynamicsResults, const int stateTransitionMatrixSize, const int sensitivityMatrixSize ): - singleArcDynamicsResults_( singleArcDynamicsResults ), - stateTransitionMatrixSize_( stateTransitionMatrixSize ), - sensitivityMatrixSize_( sensitivityMatrixSize ) { } + singleArcDynamicsResults_( singleArcDynamicsResults ), + stateTransitionMatrixSize_( stateTransitionMatrixSize ), + sensitivityMatrixSize_( sensitivityMatrixSize ) + { - void reset( ) + } + + void reset( ) { clearSolutionMaps( ); singleArcDynamicsResults_->reset( ); @@ -450,6 +480,11 @@ namespace tudat propagationTerminationReason ); } + void manuallySetSecondaryData( const std::shared_ptr< SingleArcVariationalSimulationResults< StateScalarType, TimeType > > resultsToCopy ) + { + singleArcDynamicsResults_->manuallySetSecondaryData( resultsToCopy->getDynamicsResults( ) ); + } + //! Function to split the full numerical solution into the solution for state transition matrix, sensitivity matrix, and unprocessed dynamics solution void splitSolution( const std::map >& fullSolution, @@ -579,11 +614,17 @@ namespace tudat MultiArcSimulationResults( const std::vector< std::shared_ptr< SingleArcResults< StateScalarType, TimeType > > > singleArcResults, - const std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > dependentVariableInterface = nullptr ): + const std::shared_ptr< MultiArcDependentVariablesInterface< TimeType > > dependentVariableInterface ): singleArcResults_( singleArcResults ), propagationIsPerformed_( false ), solutionIsCleared_( false ), - dependentVariableInterface_( dependentVariableInterface ){ } + dependentVariableInterface_( dependentVariableInterface ) + { + if( dependentVariableInterface_ == nullptr ) + { + throw std::runtime_error( "Error when creating MultiArcSimulationResults, dependentVariableInterface_ is NULL " ); + } + } - ~MultiArcSimulationResults( ) + ~MultiArcSimulationResults( ) {} bool getPropagationIsPerformed( ) @@ -641,6 +682,19 @@ namespace tudat } } + void manuallySetSecondaryData( const std::shared_ptr< MultiArcSimulationResults< SingleArcResults, StateScalarType, TimeType > > resultsToCopy ) + { + if( resultsToCopy->getSingleArcResults( ).size( ) != singleArcResults_.size( ) ) + { + throw std::runtime_error( "Error when manually resetting multi-arc secondary data; arc sizes are incompatible" ); + } + + for( unsigned int i = 0; i < resultsToCopy->getSingleArcResults( ).size( ); i++ ) + { + singleArcResults_.at( i )->manuallySetSecondaryData( resultsToCopy->getSingleArcResults( ).at( i ) ); + } + } + std::vector< std::shared_ptr< SingleArcResults< StateScalarType, TimeType > > > getSingleArcResults( ) { return singleArcResults_; @@ -769,32 +823,30 @@ namespace tudat void updateDependentVariableInterface( ) { - if( dependentVariableInterface_ != nullptr ) + std::vector > > dependentVariablesInterpolators; + for ( unsigned int i = 0; i < arcStartTimes_.size( ); i++ ) { - std::vector > > dependentVariablesInterpolators; - for ( unsigned int i = 0; i < arcStartTimes_.size( ); i++ ) + if( singleArcResults_.at( i )->getDependentVariableHistory( ).size( ) > 0 ) { - if ( dependentVariableInterface_->getDependentVariablesSettings( ).size( ) > 0 ) - { - std::shared_ptr > dependentVariablesInterpolator = - std::make_shared >( - utilities::createVectorFromMapKeys( - singleArcResults_.at( i )->getDependentVariableHistory( )), - utilities::createVectorFromMapValues( - singleArcResults_.at( i )->getDependentVariableHistory( )), 8 ); - dependentVariablesInterpolators.push_back( dependentVariablesInterpolator ); - } - else - { - dependentVariablesInterpolators.push_back( - std::shared_ptr >( )); - } + std::shared_ptr > + dependentVariablesInterpolator = + std::make_shared >( + utilities::createVectorFromMapKeys( + singleArcResults_.at( i )->getDependentVariableHistory( )), + utilities::createVectorFromMapValues( + singleArcResults_.at( i )->getDependentVariableHistory( )), 8 ); + dependentVariablesInterpolators.push_back( dependentVariablesInterpolator ); + } + else + { + dependentVariablesInterpolators.push_back( nullptr ); } - // Update arc end times - dependentVariableInterface_->updateDependentVariablesInterpolators( - dependentVariablesInterpolators, arcStartTimes_, arcEndTimes_ ); } + + // Update arc end times + dependentVariableInterface_->updateDependentVariablesInterpolators( + dependentVariablesInterpolators, arcStartTimes_, arcEndTimes_ ); } private: @@ -821,16 +873,12 @@ namespace tudat public: HybridArcSimulationResults( const std::shared_ptr< SingleArcResults< StateScalarType, TimeType > > singleArcResults, - const std::shared_ptr< MultiArcSimulationResults< SingleArcResults, StateScalarType, TimeType > > multiArcResults, - const bool createDependentVariableInterface = false ): + const std::shared_ptr< MultiArcSimulationResults< SingleArcResults, StateScalarType, TimeType > > multiArcResults ): singleArcResults_( singleArcResults ), multiArcResults_( multiArcResults ) { - if( createDependentVariableInterface ) - { - dependentVariableInterface_ = std::make_shared >( - singleArcResults_->getSingleArcDependentVariablesInterface( ), - multiArcResults_->getMultiArcDependentVariablesInterface( )); - } + dependentVariableInterface_ = std::make_shared >( + singleArcResults_->getSingleArcDependentVariablesInterface( ), + multiArcResults_->getMultiArcDependentVariablesInterface( ) ); } ~HybridArcSimulationResults( ) diff --git a/src/interface/spice/spiceInterface.cpp b/src/interface/spice/spiceInterface.cpp index e3f28bc645..6709f74c78 100644 --- a/src/interface/spice/spiceInterface.cpp +++ b/src/interface/spice/spiceInterface.cpp @@ -416,6 +416,7 @@ void loadStandardSpiceKernels(const std::vector alternativeEphemeri { loadSpiceKernelInTudat(kernelPath + "/codes_300ast_20100725.bsp"); + loadSpiceKernelInTudat(kernelPath + "/codes_300ast_20100725.tf"); loadSpiceKernelInTudat(kernelPath + "/inpop19a_TDB_m100_p100_spice.bsp"); loadSpiceKernelInTudat(kernelPath + "/NOE-4-2020.bsp"); loadSpiceKernelInTudat(kernelPath + "/NOE-5-2021.bsp"); diff --git a/src/simulation/propagation_setup/propagationOutput.cpp b/src/simulation/propagation_setup/propagationOutput.cpp index 0eff7a8436..d0a9851ef3 100644 --- a/src/simulation/propagation_setup/propagationOutput.cpp +++ b/src/simulation/propagation_setup/propagationOutput.cpp @@ -494,6 +494,54 @@ int getDependentVariableSize( return variableSize; } +std::pair< int, int > getDependentVariableShape( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ) +{ + int dependentVariableSize = getDependentVariableSaveSize( dependentVariableSettings ); + std::pair< int, int > dependentVariableShape; + switch ( dependentVariableSettings->dependentVariableType_ ) + { + case inertial_to_body_fixed_rotation_matrix_variable: + dependentVariableShape = { 3, 3 }; + break; + case intermediate_aerodynamic_rotation_matrix_variable: + dependentVariableShape = { 3, 3 }; + break; + case tnw_to_inertial_frame_rotation_dependent_variable: + dependentVariableShape = { 3, 3 }; + break; + case rsw_to_inertial_frame_rotation_dependent_variable: + dependentVariableShape = { 3, 3 }; + break; + case acceleration_partial_wrt_body_translational_state: + dependentVariableShape = { 3, 3 }; + break; + case body_inertia_tensor: + dependentVariableShape = { 3, 3 }; + break; + default: + dependentVariableShape = { dependentVariableSize, 1 }; + break; + } + + if( isMatrixDependentVariable( dependentVariableSettings ) && dependentVariableShape.second == 1 ) + { + throw std::runtime_error( "Error when finding shape of dependent variable: (" + getDependentVariableName( dependentVariableSettings ) + + "), dependent variable should be a matrix, but number of columns is equal to 1 " ); + } + + if( dependentVariableShape.first * dependentVariableShape.second != dependentVariableSize ) + { + throw std::runtime_error( "Error when finding shape of dependent variable: (" + getDependentVariableName( dependentVariableSettings ) + + "), vector size (" + std::to_string( dependentVariableSize ) + ") and matrix size (" + + std::to_string( dependentVariableShape.first ) + ", " + std::to_string( dependentVariableShape.second ) + ") are incompatible" ); + } + return dependentVariableShape; + + + return dependentVariableShape; +} + bool isScalarDependentVariable( const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ) { @@ -516,6 +564,37 @@ bool isScalarDependentVariable( } + +bool isMatrixDependentVariable( + const std::shared_ptr< SingleDependentVariableSaveSettings > dependentVariableSettings ) +{ + bool isMatrixVariable = false; + switch ( dependentVariableSettings->dependentVariableType_ ) + { + case inertial_to_body_fixed_rotation_matrix_variable: + isMatrixVariable = true; + break; + case intermediate_aerodynamic_rotation_matrix_variable: + isMatrixVariable = true; + break; + case tnw_to_inertial_frame_rotation_dependent_variable: + isMatrixVariable = true; + break; + case rsw_to_inertial_frame_rotation_dependent_variable: + isMatrixVariable = true; + break; + case acceleration_partial_wrt_body_translational_state: + isMatrixVariable = true; + break; + case body_inertia_tensor: + isMatrixVariable = true; + break; + default: + break; + } + return isMatrixVariable; +} + Eigen::VectorXd getConstellationMinimumDistance( const std::function< Eigen::Vector3d( ) >& mainBodyPositionFunction, const std::vector< std::function< Eigen::Vector3d( ) > >& bodiesToCheckPositionFunctions ) diff --git a/tests/src/astro/orbit_determination/unitTestConsiderParameters.cpp b/tests/src/astro/orbit_determination/unitTestConsiderParameters.cpp index ec6a1d9628..e2591c74d3 100644 --- a/tests/src/astro/orbit_determination/unitTestConsiderParameters.cpp +++ b/tests/src/astro/orbit_determination/unitTestConsiderParameters.cpp @@ -333,7 +333,7 @@ BOOST_AUTO_TEST_CASE( testConsiderParameters ) } // Compare with manually computed covariance - TUDAT_CHECK_MATRIX_CLOSE_FRACTION( covarianceOutput->unnormalizedCovarianceWithConsiderParameters_, computedCovarianceConsiderParameters, 1.0e-12 ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( covarianceOutput->unnormalizedCovarianceWithConsiderParameters_, computedCovarianceConsiderParameters, 1.0e-11 ); // Manually perform full estimation step Eigen::VectorXd rightHandSide = normalisedEstimatedPartials.transpose( ) * estimationInput->getWeightsMatrixDiagonals( ).cwiseProduct( normalisedConsiderPartials * diff --git a/tests/src/astro/orbit_determination/unitTestEstimationInput.cpp b/tests/src/astro/orbit_determination/unitTestEstimationInput.cpp index fc3d3ab4d5..f647fe4900 100644 --- a/tests/src/astro/orbit_determination/unitTestEstimationInput.cpp +++ b/tests/src/astro/orbit_determination/unitTestEstimationInput.cpp @@ -170,6 +170,257 @@ BOOST_AUTO_TEST_CASE( test_CovarianceAsFunctionOfTime ) } } +BOOST_AUTO_TEST_CASE( test_WeightDefinitions ) + +{ + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + const double startTime = double( 1.0E7 ); + const int numberOfDaysOfData = 3; + + // Define bodies in simulation + std::vector< std::string > bodyNames; + bodyNames.push_back( "Earth" ); + bodyNames.push_back( "Sun" ); + bodyNames.push_back( "Moon" ); + bodyNames.push_back( "Mars" ); + + // Specify initial time + double initialEphemerisTime = startTime; + double finalEphemerisTime = initialEphemerisTime + numberOfDaysOfData * 86400.0; + + // Create bodies needed in simulation + BodyListSettings bodySettings = + getDefaultBodySettings( bodyNames, "Earth", "ECLIPJ2000" ); + bodySettings.at( "Earth" )->rotationModelSettings = std::make_shared< SimpleRotationModelSettings >( + "ECLIPJ2000", "IAU_Earth", + spice_interface::computeRotationQuaternionBetweenFrames( + "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ), + initialEphemerisTime, 2.0 * mathematical_constants::PI / + ( physical_constants::JULIAN_DAY ) ); + + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + bodies.createEmptyBody( "Vehicle" ); + bodies.at( "Vehicle" )->setConstantBodyMass( 400.0 ); + + + bodies.at( "Vehicle" )->setEphemeris( std::make_shared< TabulatedCartesianEphemeris< > >( + std::shared_ptr< interpolators::OneDimensionalInterpolator + < double, Eigen::Vector6d > >( ), "Earth", "ECLIPJ2000" ) ); + + + // Creatre ground stations: same position, but different representation + std::vector< std::string > groundStationNames; + groundStationNames.push_back( "Station1" ); + groundStationNames.push_back( "Station2" ); + groundStationNames.push_back( "Station3" ); + + createGroundStation( bodies.at( "Earth" ), "Station1", ( Eigen::Vector3d( ) << 0.0, 0.35, 0.0 ).finished( ), geodetic_position ); + createGroundStation( bodies.at( "Earth" ), "Station2", ( Eigen::Vector3d( ) << 0.0, -0.55, 2.0 ).finished( ), geodetic_position ); + createGroundStation( bodies.at( "Earth" ), "Station3", ( Eigen::Vector3d( ) << 0.0, 0.05, 4.0 ).finished( ), geodetic_position ); + + // Set accelerations on Vehicle that are to be taken into account. + SelectedAccelerationMap accelerationMap; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfVehicle; + accelerationsOfVehicle[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 8, 8 ) ); + accelerationMap[ "Vehicle" ] = accelerationsOfVehicle; + + // Set bodies for which initial state is to be estimated and integrated. + std::vector< std::string > bodiesToIntegrate; + std::vector< std::string > centralBodies; + bodiesToIntegrate.push_back( "Vehicle" ); + centralBodies.push_back( "Earth" ); + + // Create acceleration models + AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToIntegrate, centralBodies ); + + // Set Keplerian elements for Asterix. + Eigen::Vector6d asterixInitialStateInKeplerianElements; + asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7200.0E3; + asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.05; + asterixInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + + // Set (perturbed) initial state. + Eigen::Matrix< double, 6, 1 > systemInitialState = convertKeplerianToCartesianElements( + asterixInitialStateInKeplerianElements, earthGravitationalParameter ); + + // Create propagator settings + std::shared_ptr< TranslationalStatePropagatorSettings< double, double > > propagatorSettings = + std::make_shared< TranslationalStatePropagatorSettings< double, double > > + ( centralBodies, accelerationModelMap, bodiesToIntegrate, systemInitialState, + double( finalEphemerisTime ), cowell ); + + // Create integrator settings + std::shared_ptr< IntegratorSettings< double > > integratorSettings = + std::make_shared< RungeKuttaVariableStepSizeSettings< double > > + ( double( initialEphemerisTime ), 40.0, + CoefficientSets::rungeKuttaFehlberg78, + 40.0, 40.0, 1.0, 1.0 ); + + // Define parameters. + std::vector< LinkEnds > stationReceiverLinkEnds; + std::vector< LinkEnds > stationTransmitterLinkEnds; + + for( unsigned int i = 0; i < groundStationNames.size( ); i++ ) + { + LinkEnds linkEnds; + linkEnds[ transmitter ] = LinkEndId( "Earth", groundStationNames.at( i ) ); + linkEnds[ receiver ] = LinkEndId( "Vehicle", "" ); + stationTransmitterLinkEnds.push_back( linkEnds ); + + linkEnds.clear( ); + linkEnds[ receiver ] = LinkEndId( "Earth", groundStationNames.at( i ) ); + linkEnds[ transmitter ] = LinkEndId( "Vehicle", "" ); + stationReceiverLinkEnds.push_back( linkEnds ); + } + + std::map< ObservableType, std::vector< LinkEnds > > linkEndsPerObservable; + linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 0 ] ); + linkEndsPerObservable[ one_way_range ].push_back( stationTransmitterLinkEnds[ 0 ] ); + linkEndsPerObservable[ one_way_range ].push_back( stationReceiverLinkEnds[ 1 ] ); + + linkEndsPerObservable[ one_way_doppler ].push_back( stationReceiverLinkEnds[ 1 ] ); + linkEndsPerObservable[ one_way_doppler ].push_back( stationTransmitterLinkEnds[ 2 ] ); + + linkEndsPerObservable[ angular_position ].push_back( stationReceiverLinkEnds[ 2 ] ); + linkEndsPerObservable[ angular_position ].push_back( stationTransmitterLinkEnds[ 1 ] ); + + std::vector< std::shared_ptr< EstimatableParameterSettings > > parameterNames; + parameterNames.push_back( + std::make_shared< InitialTranslationalStateEstimatableParameterSettings< double > >( + "Vehicle", systemInitialState, "Earth" ) ); + + // Create parameters + std::shared_ptr< estimatable_parameters::EstimatableParameterSet< double > > parametersToEstimate = + createParametersToEstimate< double, double >( parameterNames, bodies ); + + std::vector< std::shared_ptr< ObservationModelSettings > > observationSettingsList; + + for( std::map< ObservableType, std::vector< LinkEnds > >::iterator linkEndIterator = linkEndsPerObservable.begin( ); + linkEndIterator != linkEndsPerObservable.end( ); linkEndIterator++ ) + { + ObservableType currentObservable = linkEndIterator->first; + + + std::vector< LinkEnds > currentLinkEndsList = linkEndIterator->second; + for( unsigned int i = 0; i < currentLinkEndsList.size( ); i++ ) + { + observationSettingsList.push_back( + std::make_shared< ObservationModelSettings >( + currentObservable, currentLinkEndsList.at( i ) ) ); + } + } + + // Create orbit determination object. + OrbitDeterminationManager< double, double > orbitDeterminationManager = + OrbitDeterminationManager< double, double >( + bodies, parametersToEstimate, observationSettingsList, + integratorSettings, propagatorSettings ); + + std::vector< double > baseTimeList; + double observationTimeStart = initialEphemerisTime + 1000.0; + double observationInterval = 20.0; + for( int i = 0; i < numberOfDaysOfData; i++ ) + { + for( unsigned int j = 0; j < 500; j++ ) + { + baseTimeList.push_back( observationTimeStart + static_cast< double >( i ) * 86400.0 + + static_cast< double >( j ) * observationInterval ); + } + } + + std::vector< std::shared_ptr< ObservationSimulationSettings< double > > > measurementSimulationInput = + getObservationSimulationSettings< double >( + linkEndsPerObservable, baseTimeList, receiver ); + + // Simulate observations + std::shared_ptr< ObservationCollection< double, double > > simulatedObservations = + simulateObservations< double, double >( + measurementSimulationInput, orbitDeterminationManager.getObservationSimulators( ), bodies ); + + + // Define estimation input + std::shared_ptr< EstimationInput< double, double > > estimationInput = + std::make_shared< EstimationInput< double, double > >( + simulatedObservations ); + + std::map< ObservableType, std::pair< int, int > > observationTypeStartAndSize = + simulatedObservations->getObservationTypeStartAndSize( ); + + { + estimationInput->setConstantWeightsMatrix( 0.1 ); + Eigen::VectorXd totalWeights = estimationInput->getWeightsMatrixDiagonals( ); + + for( unsigned int i = 0; i < totalWeights.rows( ); i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( i ), 0.1, std::numeric_limits< double >::epsilon( ) ); + } + } + + { + std::map weightPerObservable; + weightPerObservable[ one_way_range ] = 1.0 / ( 3.0 * 3.0 ); + weightPerObservable[ angular_position ] = 1.0 / ( 1.0E-5 * 1.0E-5 ); + weightPerObservable[ one_way_doppler ] = 1.0 / ( 1.0E-11 * 1.0E-11 * SPEED_OF_LIGHT * SPEED_OF_LIGHT ); + + estimationInput->setConstantPerObservableWeightsMatrix( weightPerObservable ); + Eigen::VectorXd totalWeights = estimationInput->getWeightsMatrixDiagonals( ); + + for( auto it : weightPerObservable ) + { + for( int i = 0; i < observationTypeStartAndSize.at( it.first ).second; i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( observationTypeStartAndSize.at( it.first ).first + i ), it.second, std::numeric_limits< double >::epsilon( ) ); + } + } + } + + { + Eigen::Vector2d angularPositionWeight; + angularPositionWeight << 0.1, 0.2; + + estimationInput->setConstantWeightsMatrix( 2.0 ); + estimationInput->setConstantSingleObservableVectorWeights( + angular_position, angularPositionWeight ); + Eigen::VectorXd totalWeights = estimationInput->getWeightsMatrixDiagonals( ); + + std::pair< int, int > startEndIndex = observationTypeStartAndSize.at( angular_position ); + + for( int i = 0; i < startEndIndex.first; i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( i ), 2.0, std::numeric_limits< double >::epsilon( ) ); + } + + for( int i = 0; i < startEndIndex.second; i++ ) + { + if( i % 2 == 0 ) + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( startEndIndex.first + i ), angularPositionWeight( 0 ), std::numeric_limits< double >::epsilon( ) ); + } + else + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( startEndIndex.first + i ), angularPositionWeight( 1 ), std::numeric_limits< double >::epsilon( ) ); + } + } + + for( unsigned int i = startEndIndex.first + startEndIndex.second; i < totalWeights.rows( ); i++ ) + { + BOOST_CHECK_CLOSE_FRACTION( totalWeights( i ), 2.0, std::numeric_limits< double >::epsilon( ) ); + } + } + +} + BOOST_AUTO_TEST_SUITE_END( ) } diff --git a/tests/src/astro/propagators/unitTestDependentVariableOutput.cpp b/tests/src/astro/propagators/unitTestDependentVariableOutput.cpp index cd5e46c9b8..8e0f671542 100644 --- a/tests/src/astro/propagators/unitTestDependentVariableOutput.cpp +++ b/tests/src/astro/propagators/unitTestDependentVariableOutput.cpp @@ -71,564 +71,582 @@ Eigen::VectorXd customDependentVariable2( const simulation_setup::SystemOfBodies return ( Eigen::VectorXd( 1 ) << bodies.at( "Apollo" )->getPosition( ).norm( ) / 2.0 ).finished( ); } -////! Propagate entry of Apollo capsule, and save a list of dependent variables during entry. The saved dependent variables -////! are compared against theoretical/manual values in this test. -//BOOST_AUTO_TEST_CASE( testDependentVariableOutput ) -//{ -// // Load Spice kernels. -// spice_interface::loadStandardSpiceKernels( ); - -// // Set simulation start epoch. -// const double simulationStartEpoch = 0.0; - -// // Set simulation end epoch. -// const double simulationEndEpoch = 3300.0; - -// // Set numerical integration fixed step size. -// const double fixedStepSize = 1.0; - - -// // Set Keplerian elements for Capsule. -// Eigen::Vector6d apolloInitialStateInKeplerianElements; -// apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3; -// apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005; -// apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); -// apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) -// = unit_conversions::convertDegreesToRadians( 235.7 ); -// apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) -// = unit_conversions::convertDegreesToRadians( 23.4 ); -// apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); - -// // Convert apollo state from Keplerian elements to Cartesian elements. -// const double earthGravitationalParameter = getBodyGravitationalParameter( "Earth" ); -// const Eigen::Vector6d apolloInitialState = convertKeplerianToCartesianElements( -// apolloInitialStateInKeplerianElements, -// getBodyGravitationalParameter( "Earth" ) ); - -//#if TUDAT_BUILD_WITH_NRLMSISE -// unsigned int maximumTestCase = 3; -//#else -// unsigned int maximumTestCase = 2; -//#endif -// for( unsigned int testCase = 0; testCase < maximumTestCase; testCase++ ) -// { -// std::map< double, Eigen::Vector3d > cowellAcceleration; -// for( unsigned int propagatorType = 0; propagatorType < 2; propagatorType++ ) -// { -// // Define simulation body settings. -// BodyListSettings bodySettings = -// getDefaultBodySettings( { "Earth", "Moon" }, simulationStartEpoch - 10.0 * fixedStepSize, -// simulationEndEpoch + 10.0 * fixedStepSize, "Earth", "ECLIPJ2000" ); -// bodySettings.at( "Earth" )->gravityFieldSettings = -// std::make_shared< simulation_setup::GravityFieldSettings >( central_spice ); - -// if( testCase >= 2 ) -// { -// bodySettings.at( "Earth" )->atmosphereSettings = -// std::make_shared< simulation_setup::AtmosphereSettings >( nrlmsise00 ); -// } - -// bool isOblateSpheroidUsed = 0; -// double oblateSpheroidEquatorialRadius = 6378.0E3; -// double oblateSpheroidFlattening = 1.0 / 300.0; - -// if( testCase% 2 == 0 ) -// { -// isOblateSpheroidUsed = 1; -// bodySettings.at( "Earth" )->shapeModelSettings = -// std::make_shared< simulation_setup::OblateSphericalBodyShapeSettings >( -// oblateSpheroidEquatorialRadius, oblateSpheroidFlattening ); -// } - -// // Create Earth object -// simulation_setup::SystemOfBodies bodies = simulation_setup::createSystemOfBodies( bodySettings ); - -// // Create vehicle objects. -// bodies.createEmptyBody( "Apollo" ); - -// // Create vehicle aerodynamic coefficients -// bodies.at( "Apollo" )->setAerodynamicCoefficientInterface( -// unit_tests::getApolloCoefficientInterface( ) ); -// bodies.at( "Apollo" )->setConstantBodyMass( 5.0E3 ); -// bodies.at( "Apollo" )->setRotationalEphemeris( -// createRotationModel( -// std::make_shared< PitchTrimRotationSettings >( "Earth", "ECLIPJ2000", "VehicleFixed" ), -// "Apollo", bodies ) ); - -// std::shared_ptr< system_models::VehicleSystems > vehicleSystems = -// std::make_shared< system_models::VehicleSystems >( ); - -// double noseRadius = 0.7; -// double wallEmissivity = 0.7; -// vehicleSystems->setNoseRadius( noseRadius ); -// vehicleSystems->setWallEmissivity( wallEmissivity ); - -// bodies.at( "Apollo" )->setVehicleSystems( vehicleSystems ); - -// // Define propagator settings variables. -// SelectedAccelerationMap accelerationMap; -// std::vector< std::string > bodiesToPropagate; -// std::vector< std::string > centralBodies; - -// // Define acceleration model settings. -// std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfApollo; -// accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); -// accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) ); -// accelerationsOfApollo[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); -// accelerationMap[ "Apollo" ] = accelerationsOfApollo; - -// bodiesToPropagate.push_back( "Apollo" ); -// centralBodies.push_back( "Earth" ); - -// // Set initial state -// Eigen::Vector6d systemInitialState = apolloInitialState; - -// // Define list of dependent variables to save. -// std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( mach_number_dependent_variable, "Apollo" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( altitude_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( relative_distance_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( relative_speed_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleAccelerationDependentVariableSaveSettings >( -// point_mass_gravity, "Apollo", "Earth", 1 ) ); - -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( total_aerodynamic_g_load_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( stagnation_point_heat_flux_dependent_variable, -// "Apollo", "Earth" ) ); - -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( local_temperature_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( geodetic_latitude_dependent_variable, -// "Apollo", "Earth" ) ); - -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( local_density_dependent_variable, -// "Apollo", "Earth" ) ); - -// dependentVariables.push_back( -// std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( -// "Apollo", reference_frames::latitude_angle ) ); -// dependentVariables.push_back( -// std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( -// "Apollo", reference_frames::longitude_angle ) ); - -// dependentVariables.push_back( -// std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( -// "Apollo", reference_frames::angle_of_attack ) ); -// dependentVariables.push_back( -// std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( -// "Apollo", reference_frames::angle_of_sideslip ) ); -// dependentVariables.push_back( -// std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( -// "Apollo", reference_frames::bank_angle ) ); - - - - -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( relative_position_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( relative_velocity_dependent_variable, -// "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleAccelerationDependentVariableSaveSettings >( -// point_mass_gravity, "Apollo", "Earth", 0 ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// total_acceleration_dependent_variable, "Apollo" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// aerodynamic_moment_coefficients_dependent_variable, "Apollo" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// aerodynamic_force_coefficients_dependent_variable, "Apollo" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleAccelerationDependentVariableSaveSettings >( -// aerodynamic, "Apollo", "Earth", 0 ) ); -// dependentVariables.push_back( -// std::make_shared< SingleAccelerationDependentVariableSaveSettings >( -// point_mass_gravity, "Apollo", "Moon", 0 ) ); -// dependentVariables.push_back( -// std::make_shared< SingleAccelerationDependentVariableSaveSettings >( -// third_body_point_mass_gravity, "Apollo", "Moon", 0 ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// keplerian_state_dependent_variable, "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// modified_equinocial_state_dependent_variable, "Apollo", "Earth" ) ); -// dependentVariables.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// body_fixed_relative_cartesian_position, "Apollo", "Earth" ) ); - - -// // Create acceleration models and propagation settings. -// basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( -// bodies, accelerationMap, bodiesToPropagate, centralBodies ); - -// std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings; -// if( propagatorType == 0 ) -// { -// propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > > -// ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, -// std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), cowell, -// dependentVariables ); -// } -// else -// { -// propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > > -// ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, -// std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), gauss_modified_equinoctial, -// dependentVariables ); -// } - -// std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesToAdd; -// dependentVariablesToAdd.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// body_fixed_relative_spherical_position, "Apollo", "Earth" ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// rsw_to_inertial_frame_rotation_dependent_variable, "Apollo", "Earth" ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< CustomDependentVariableSaveSettings >( -// std::bind( &customDependentVariable1, bodies ), 3 ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< CustomDependentVariableSaveSettings >( -// std::bind( &customDependentVariable2, bodies ), 1 ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// gravity_field_potential_dependent_variable, "Apollo", "Earth" ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// gravity_field_potential_dependent_variable, "Apollo", "Moon" ) ); -// dependentVariablesToAdd.push_back( -// std::make_shared< SingleDependentVariableSaveSettings >( -// body_fixed_relative_cartesian_position, "Apollo", "Moon" ) ); - -// addDepedentVariableSettings< double, double >( dependentVariablesToAdd, propagatorSettings ); - -// std::shared_ptr< IntegratorSettings< > > integratorSettings = -// std::make_shared< IntegratorSettings< > > -// ( rungeKutta4, simulationStartEpoch, fixedStepSize ); - -// // Create simulation object and propagate dynamics. -// SingleArcDynamicsSimulator< > dynamicsSimulator( -// bodies, integratorSettings, propagatorSettings, true, false, false ); - -// std::cout<<"Propagation finished "< > numericalSolution = -// dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); -// std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > rawNumericalSolution = -// dynamicsSimulator.getEquationsOfMotionNumericalSolutionRaw( ); -// std::map< double, Eigen::VectorXd > dependentVariableSolution = -// dynamicsSimulator.getDependentVariableHistory( ); - -// // Iterate over results for dependent variables, and check against manually retrieved values. -// Eigen::Vector6d currentStateDerivative; -// Eigen::Vector3d manualCentralGravity; -// std::shared_ptr< ephemerides::RotationalEphemeris > earthRotationModel = -// bodies.at( "Earth" )->getRotationalEphemeris( ); -// std::shared_ptr< aerodynamics::AtmosphereModel > earthAtmosphereModel = -// bodies.at( "Earth" )->getAtmosphereModel( ); -// std::shared_ptr< gravitation::GravityFieldModel > earthGravityModel = -// bodies.at( "Earth" )->getGravityFieldModel( ); -// std::shared_ptr< gravitation::GravityFieldModel > moonGravityModel = -// bodies.at( "Moon" )->getGravityFieldModel( ); - -// std::shared_ptr< aerodynamics::AtmosphericFlightConditions > apolloFlightConditions = -// std::dynamic_pointer_cast< aerodynamics::AtmosphericFlightConditions >( -// bodies.at( "Apollo" )->getFlightConditions( ) ); -// std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > apolloCoefficientInterface = -// bodies.at( "Apollo" )->getAerodynamicCoefficientInterface( ); -// bodies.at( "Apollo" )->setIsBodyInPropagation( true ); - -// std::cout<<"Number of points: "<::iterator variableIterator = dependentVariableSolution.begin( ); -// variableIterator != dependentVariableSolution.end( ); variableIterator++ ) -// { -// Eigen::VectorXd currentDependentVariables = variableIterator->second; - -// double machNumber = currentDependentVariables( 0 ); -// double altitude = currentDependentVariables( 1 ); -// double relativeDistance = currentDependentVariables( 2 ); -// double relativeSpeed= currentDependentVariables( 3 ); -// double gravitationalAccelerationNorm = currentDependentVariables( 4 ); -// double gLoad = currentDependentVariables( 5 ); -// double stagnationPointHeatFlux = currentDependentVariables( 6 ); -// double freestreamTemperature = currentDependentVariables( 7 ); -// double geodeticLatitude = currentDependentVariables( 8 ); -// double freestreamDensity = currentDependentVariables( 9 ); -// double latitude = currentDependentVariables( 10 ); -// double longitude = currentDependentVariables( 11 ); -// double angleOfAttack = currentDependentVariables( 12 ); -// double sideslipAngle = currentDependentVariables( 13 ); -// double bankAngle = currentDependentVariables( 14 ); - -// Eigen::Vector3d relativePosition = currentDependentVariables.segment( 15, 3 ); -// Eigen::Vector3d computedBodyFixedPosition = -// earthRotationModel->getRotationToTargetFrame( variableIterator->first ) * relativePosition; -// Eigen::Vector3d computedSphericalBodyFixedPosition = -// coordinate_conversions::convertCartesianToSpherical( computedBodyFixedPosition ); - -// Eigen::Vector3d relativeVelocity = currentDependentVariables.segment( 18, 3 ); -// Eigen::Vector3d gravitationalAcceleration = currentDependentVariables.segment( 21, 3 ); -// Eigen::Vector3d totalAcceleration = currentDependentVariables.segment( 24, 3 ); -// Eigen::Vector3d momentCoefficients = currentDependentVariables.segment( 27, 3 ); -// Eigen::Vector3d forceCoefficients = currentDependentVariables.segment( 30, 3 ); -// Eigen::Vector3d aerodynamicAcceleration = currentDependentVariables.segment( 33, 3 ); -// Eigen::Vector3d moonAcceleration1 = currentDependentVariables.segment( 36, 3 ); -// Eigen::Vector3d moonAcceleration2 = currentDependentVariables.segment( 39, 3 ); - -// Eigen::Vector6d keplerElements = currentDependentVariables.segment( 42, 6 ); -// Eigen::Vector6d modifiedEquinoctialElements = currentDependentVariables.segment( 48, 6 ); -// Eigen::Vector3d bodyFixedCartesianPosition = currentDependentVariables.segment( 54, 3 ); -// Eigen::Vector3d bodyFixedSphericalPosition = currentDependentVariables.segment( 57, 3 ); -// Eigen::Matrix3d rswToInertialRotationMatrix = -// propagators::getMatrixFromVectorRotationRepresentation( currentDependentVariables.segment( 60, 9 ) ); -// Eigen::Vector3d customVariable1 = currentDependentVariables.segment( 69, 3 ); -// Eigen::Vector3d customVariable2 = currentDependentVariables.segment( 72, 1 ); - -// double earthGravitationalPotential = variableIterator->second( 73 ); -// double moonGravitationalPotential = variableIterator->second( 74 ); -// Eigen::Vector3d moonBodyFixedCartesianPosition = variableIterator->second.segment( 75, 3 ); - -// currentStateDerivative = dynamicsSimulator.getDynamicsStateDerivative( )->computeStateDerivative( -// variableIterator->first, rawNumericalSolution.at( variableIterator->first ) ); - -// // Manually compute central gravity. -// manualCentralGravity = -// -bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) * -// relativePosition / -// std::pow( relativePosition.norm( ), 3 ); - -// // Check output time consistency -// BOOST_CHECK_EQUAL( numericalSolution.count( variableIterator->first ), 1 ); - -// // Check relative position and velocity against state -// for( unsigned int i = 0; i < 3; i++ ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( numericalSolution.at( variableIterator->first )( i ) - -// relativePosition( i ) ), 2.0E-5 ); -// BOOST_CHECK_SMALL( -// std::fabs( numericalSolution.at( variableIterator->first )( 3 + i ) - -// relativeVelocity( i ) ), 5.0E-11 ); -// } - -// // Check central gravity acceleration -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( -// manualCentralGravity.segment( 0, 3 ), -// gravitationalAcceleration, ( 6.0 * std::numeric_limits< double >::epsilon( ) ) ); - -// if( propagatorType == 0 ) -// { -// // Check total acceleration (tolerance is not epsilon due to numerical root finding for trim) -// for( unsigned int i = 0; i < 3; i++ ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( currentStateDerivative( 3 + i ) - totalAcceleration( i ) ), 1.0E-13 ); -// } -// cowellAcceleration[ variableIterator->first ] = totalAcceleration; -// } -// else if( variableIterator->first < 100.0 ) -// { -// for( unsigned int i = 0; i < 3; i++ ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( cowellAcceleration[ variableIterator->first ]( i ) - totalAcceleration( i ) ), 1.0E-8 ); -// } -// } - -// // Check relative position and velocity norm. -// BOOST_CHECK_SMALL( -// std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 0, 3 ) ).norm( ) - -// relativeDistance ), 2.0E-5 ); -// BOOST_CHECK_SMALL( -// std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 3, 3 ) ).norm( ) - -// relativeSpeed ), 2.0E-11 ); - -// // Check central gravity acceleration norm -// BOOST_CHECK_CLOSE_FRACTION( -// manualCentralGravity.norm( ), -// gravitationalAccelerationNorm, 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check Mach number -// BOOST_CHECK_CLOSE_FRACTION( -// apolloFlightConditions->getCurrentAirspeed( ) / -// apolloFlightConditions->getCurrentSpeedOfSound( ), -// machNumber , std::numeric_limits< double >::epsilon( ) ); - -// // Check altitude. -// BOOST_CHECK_CLOSE_FRACTION( -// apolloFlightConditions->getCurrentAltitude( ), -// altitude, std::numeric_limits< double >::epsilon( ) ); - -// // Check latitude and longitude -// BOOST_CHECK_SMALL( -// std::fabs( mathematical_constants::PI / 2.0 - computedSphericalBodyFixedPosition( 1 ) - latitude ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_SMALL( -// std::fabs( computedSphericalBodyFixedPosition( 2 ) - longitude ), -// 8.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check geodetic latitude. -// if( !isOblateSpheroidUsed ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( latitude - geodeticLatitude ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// } -// else -// { -// double computedGeodeticLatitude = coordinate_conversions::calculateGeodeticLatitude( -// computedBodyFixedPosition, oblateSpheroidEquatorialRadius, oblateSpheroidFlattening, 1.0E-4 ); -// BOOST_CHECK_SMALL( -// std::fabs( computedGeodeticLatitude - geodeticLatitude ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// } -// BOOST_CHECK_SMALL( -// std::fabs( geodeticLatitude - apolloFlightConditions->getCurrentGeodeticLatitude( ) ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check sideslip anf bank angle -// BOOST_CHECK_SMALL( -// std::fabs( sideslipAngle ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_SMALL( -// std::fabs( bankAngle ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check g-load -// BOOST_CHECK_CLOSE_FRACTION( -// gLoad, aerodynamicAcceleration.norm( ) / physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION, -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check density and temperature -// BOOST_CHECK_CLOSE_FRACTION( -// freestreamDensity, -// earthAtmosphereModel->getDensity( altitude, longitude, latitude, variableIterator->first ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_CLOSE_FRACTION( -// freestreamDensity, -// apolloFlightConditions->getCurrentDensity( ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_CLOSE_FRACTION( -// freestreamTemperature, -// earthAtmosphereModel->getTemperature( altitude, longitude, latitude, variableIterator->first ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_CLOSE_FRACTION( -// freestreamTemperature, -// apolloFlightConditions->getCurrentFreestreamTemperature( ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check aerodynamic force coefficients -// std::vector< double > aerodynamicCoefficientInput; -// aerodynamicCoefficientInput.push_back( machNumber ); -// aerodynamicCoefficientInput.push_back( angleOfAttack ); -// aerodynamicCoefficientInput.push_back( sideslipAngle ); -// apolloCoefficientInterface->updateCurrentCoefficients( aerodynamicCoefficientInput ); -// Eigen::Vector3d computedForceCoefficients = apolloCoefficientInterface->getCurrentForceCoefficients( ); - -// BOOST_CHECK_SMALL( std::fabs( computedForceCoefficients( 1 ) ), 1.0E-14 ); -// for( unsigned int i = 0; i < 3; i ++ ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( computedForceCoefficients( i ) - forceCoefficients( i ) ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); -// } - -// // Check heat flux -// double expectedHeatFlux = aerodynamics::computeEquilibriumFayRiddellHeatFlux( -// freestreamDensity, apolloFlightConditions->getCurrentAirspeed( ), -// freestreamTemperature, machNumber, noseRadius, wallEmissivity ); -// BOOST_CHECK_CLOSE_FRACTION( -// expectedHeatFlux, stagnationPointHeatFlux, -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check trimmed condition (y-term)/symmetric vehicle shape (x- and z-term). -// BOOST_CHECK_SMALL( -// std::fabs( momentCoefficients( 0 ) ), 1.0E-14 ); -// BOOST_CHECK_SMALL( -// std::fabs( momentCoefficients( 1 ) ), 1.0E-10 ); -// BOOST_CHECK_SMALL( -// std::fabs( momentCoefficients( 2 ) ), 1.0E-14 ); - -// // Check if third-body gravity saving is done correctly -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( moonAcceleration1, moonAcceleration2, -// ( 2.0 * std::numeric_limits< double >::epsilon( ) ) ); - -// Eigen::Vector6d expectedKeplerElements = -// tudat::orbital_element_conversions::convertCartesianToKeplerianElements( -// Eigen::Vector6d( numericalSolution.at( variableIterator->first ) ), earthGravitationalParameter ); - -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerElements, keplerElements, -// ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); - -// Eigen::Vector6d expectedModifiedEquinoctialElements = -// tudat::orbital_element_conversions::convertCartesianToModifiedEquinoctialElements( -// Eigen::Vector6d( numericalSolution.at( variableIterator->first ) ), earthGravitationalParameter ); - -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedModifiedEquinoctialElements, modifiedEquinoctialElements, -// ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); - -// for( unsigned int i = 0; i < 3; i ++ ) -// { -// BOOST_CHECK_SMALL( -// std::fabs( computedBodyFixedPosition( i ) - bodyFixedCartesianPosition( i ) ), -// 1.0E-8 ); -// } -// BOOST_CHECK_SMALL( -// std::fabs( computedSphericalBodyFixedPosition( 0 ) - bodyFixedSphericalPosition( 0 ) ), -// 1.0E-8 ); -// BOOST_CHECK_SMALL( -// std::fabs( tudat::mathematical_constants::PI / 2.0 - computedSphericalBodyFixedPosition( 1 ) - -// bodyFixedSphericalPosition( 1 ) ), -// 10.0 * std::numeric_limits< double >::epsilon( ) ); -// BOOST_CHECK_SMALL( -// std::fabs( computedSphericalBodyFixedPosition( 2 ) - bodyFixedSphericalPosition( 2 ) ), -// 10.0 * std::numeric_limits< double >::epsilon( ) ); -// Eigen::Matrix3d computedRswRotationMatrix = -// tudat::reference_frames::getRswSatelliteCenteredToInertialFrameRotationMatrix( numericalSolution.at( variableIterator->first ) ); - -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( rswToInertialRotationMatrix, computedRswRotationMatrix, -// ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( customVariable1, ( relativePosition / 2.0 ), -// ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); -// BOOST_CHECK_CLOSE_FRACTION( customVariable2( 0 ), ( relativePosition.norm( ) / 2.0 ), -// ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); - - -// // Check central body gravitational potential -// BOOST_CHECK_CLOSE_FRACTION( -// earthGravitationalPotential, -// earthGravityModel->getGravitationalPotential( computedBodyFixedPosition ), -// 6.0 * std::numeric_limits< double >::epsilon( ) ); - -// // Check 3rd body gravitational potential - not sure why the tolerance needs to be so large -// BOOST_CHECK_CLOSE_FRACTION( -// moonGravitationalPotential, -// moonGravityModel->getGravitationalPotential( moonBodyFixedCartesianPosition ), -// 1e-8 ); -// } -// } -// } -//} +//! Propagate entry of Apollo capsule, and save a list of dependent variables during entry. The saved dependent variables +//! are compared against theoretical/manual values in this test. +BOOST_AUTO_TEST_CASE( testDependentVariableOutput ) +{ + // Load Spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + // Set simulation start epoch. + const double simulationStartEpoch = 0.0; + + // Set simulation end epoch. + const double simulationEndEpoch = 3300.0; + + // Set numerical integration fixed step size. + const double fixedStepSize = 1.0; + + + // Set Keplerian elements for Capsule. + Eigen::Vector6d apolloInitialStateInKeplerianElements; + apolloInitialStateInKeplerianElements( semiMajorAxisIndex ) = spice_interface::getAverageRadius( "Earth" ) + 120.0E3; + apolloInitialStateInKeplerianElements( eccentricityIndex ) = 0.005; + apolloInitialStateInKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 85.3 ); + apolloInitialStateInKeplerianElements( argumentOfPeriapsisIndex ) + = unit_conversions::convertDegreesToRadians( 235.7 ); + apolloInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex ) + = unit_conversions::convertDegreesToRadians( 23.4 ); + apolloInitialStateInKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 139.87 ); + + // Convert apollo state from Keplerian elements to Cartesian elements. + const double earthGravitationalParameter = getBodyGravitationalParameter( "Earth" ); + const Eigen::Vector6d apolloInitialState = convertKeplerianToCartesianElements( + apolloInitialStateInKeplerianElements, + getBodyGravitationalParameter( "Earth" ) ); + +#if TUDAT_BUILD_WITH_NRLMSISE + unsigned int maximumTestCase = 3; +#else + unsigned int maximumTestCase = 2; +#endif + for( unsigned int testCase = 0; testCase < maximumTestCase; testCase++ ) + { + std::map< double, Eigen::Vector3d > cowellAcceleration; + for( unsigned int propagatorType = 0; propagatorType < 2; propagatorType++ ) + { + // Define simulation body settings. + BodyListSettings bodySettings = + getDefaultBodySettings( { "Earth", "Moon" }, simulationStartEpoch - 10.0 * fixedStepSize, + simulationEndEpoch + 10.0 * fixedStepSize, "Earth", "ECLIPJ2000" ); + bodySettings.at( "Earth" )->gravityFieldSettings = + std::make_shared< simulation_setup::GravityFieldSettings >( central_spice ); + + if( testCase >= 2 ) + { + bodySettings.at( "Earth" )->atmosphereSettings = + std::make_shared< simulation_setup::AtmosphereSettings >( nrlmsise00 ); + } + + bool isOblateSpheroidUsed = 0; + double oblateSpheroidEquatorialRadius = 6378.0E3; + double oblateSpheroidFlattening = 1.0 / 300.0; + + if( testCase% 2 == 0 ) + { + isOblateSpheroidUsed = 1; + bodySettings.at( "Earth" )->shapeModelSettings = + std::make_shared< simulation_setup::OblateSphericalBodyShapeSettings >( + oblateSpheroidEquatorialRadius, oblateSpheroidFlattening ); + } + + // Create Earth object + simulation_setup::SystemOfBodies bodies = simulation_setup::createSystemOfBodies( bodySettings ); + + // Create vehicle objects. + bodies.createEmptyBody( "Apollo" ); + + // Create vehicle aerodynamic coefficients + bodies.at( "Apollo" )->setAerodynamicCoefficientInterface( + unit_tests::getApolloCoefficientInterface( ) ); + bodies.at( "Apollo" )->setConstantBodyMass( 5.0E3 ); + bodies.at( "Apollo" )->setRotationalEphemeris( + createRotationModel( + std::make_shared< PitchTrimRotationSettings >( "Earth", "ECLIPJ2000", "VehicleFixed" ), + "Apollo", bodies ) ); + + std::shared_ptr< system_models::VehicleSystems > vehicleSystems = + std::make_shared< system_models::VehicleSystems >( ); + + double noseRadius = 0.7; + double wallEmissivity = 0.7; + vehicleSystems->setNoseRadius( noseRadius ); + vehicleSystems->setWallEmissivity( wallEmissivity ); + + bodies.at( "Apollo" )->setVehicleSystems( vehicleSystems ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define acceleration model settings. + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfApollo; + accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); + accelerationsOfApollo[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( aerodynamic ) ); + accelerationsOfApollo[ "Moon" ].push_back( std::make_shared< AccelerationSettings >( point_mass_gravity ) ); + accelerationMap[ "Apollo" ] = accelerationsOfApollo; + + bodiesToPropagate.push_back( "Apollo" ); + centralBodies.push_back( "Earth" ); + + // Set initial state + Eigen::Vector6d systemInitialState = apolloInitialState; + + // Define list of dependent variables to save. + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( mach_number_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( altitude_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( relative_distance_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( relative_speed_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleAccelerationDependentVariableSaveSettings >( + point_mass_gravity, "Apollo", "Earth", 1 ) ); + + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( total_aerodynamic_g_load_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( stagnation_point_heat_flux_dependent_variable, + "Apollo", "Earth" ) ); + + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( local_temperature_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( geodetic_latitude_dependent_variable, + "Apollo", "Earth" ) ); + + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( local_density_dependent_variable, + "Apollo", "Earth" ) ); + + dependentVariables.push_back( + std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( + "Apollo", reference_frames::latitude_angle ) ); + dependentVariables.push_back( + std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( + "Apollo", reference_frames::longitude_angle ) ); + + dependentVariables.push_back( + std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( + "Apollo", reference_frames::angle_of_attack ) ); + dependentVariables.push_back( + std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( + "Apollo", reference_frames::angle_of_sideslip ) ); + dependentVariables.push_back( + std::make_shared< BodyAerodynamicAngleVariableSaveSettings >( + "Apollo", reference_frames::bank_angle ) ); + + + + + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( relative_position_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( relative_velocity_dependent_variable, + "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleAccelerationDependentVariableSaveSettings >( + point_mass_gravity, "Apollo", "Earth", 0 ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + total_acceleration_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + aerodynamic_moment_coefficients_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + aerodynamic_force_coefficients_dependent_variable, "Apollo" ) ); + dependentVariables.push_back( + std::make_shared< SingleAccelerationDependentVariableSaveSettings >( + aerodynamic, "Apollo", "Earth", 0 ) ); + dependentVariables.push_back( + std::make_shared< SingleAccelerationDependentVariableSaveSettings >( + point_mass_gravity, "Apollo", "Moon", 0 ) ); + dependentVariables.push_back( + std::make_shared< SingleAccelerationDependentVariableSaveSettings >( + third_body_point_mass_gravity, "Apollo", "Moon", 0 ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + keplerian_state_dependent_variable, "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + modified_equinocial_state_dependent_variable, "Apollo", "Earth" ) ); + dependentVariables.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + body_fixed_relative_cartesian_position, "Apollo", "Earth" ) ); + + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToPropagate, centralBodies ); + + std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings; + if( propagatorType == 0 ) + { + propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, + std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), cowell, + dependentVariables ); + } + else + { + propagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, + std::make_shared< propagators::PropagationTimeTerminationSettings >( 3200.0 ), gauss_modified_equinoctial, + dependentVariables ); + } + + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariablesToAdd; + dependentVariablesToAdd.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + body_fixed_relative_spherical_position, "Apollo", "Earth" ) ); + dependentVariablesToAdd.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + rsw_to_inertial_frame_rotation_dependent_variable, "Apollo", "Earth" ) ); + dependentVariablesToAdd.push_back( + std::make_shared< CustomDependentVariableSaveSettings >( + std::bind( &customDependentVariable1, bodies ), 3 ) ); + dependentVariablesToAdd.push_back( + std::make_shared< CustomDependentVariableSaveSettings >( + std::bind( &customDependentVariable2, bodies ), 1 ) ); + dependentVariablesToAdd.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + gravity_field_potential_dependent_variable, "Apollo", "Earth" ) ); + dependentVariablesToAdd.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + gravity_field_potential_dependent_variable, "Apollo", "Moon" ) ); + dependentVariablesToAdd.push_back( + std::make_shared< SingleDependentVariableSaveSettings >( + body_fixed_relative_cartesian_position, "Apollo", "Moon" ) ); + + addDepedentVariableSettings< double, double >( dependentVariablesToAdd, propagatorSettings ); + + + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, simulationStartEpoch, fixedStepSize ); + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodies, integratorSettings, propagatorSettings, true, false, false ); + + std::cout<<"Propagation finished "< > numericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolution( ); + std::map< double, Eigen::Matrix< double, Eigen::Dynamic, 1 > > rawNumericalSolution = + dynamicsSimulator.getEquationsOfMotionNumericalSolutionRaw( ); + std::map< double, Eigen::VectorXd > dependentVariableSolution = + dynamicsSimulator.getDependentVariableHistory( ); + + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > originalDependentVariables = + dynamicsSimulator.getSingleArcPropagationResults( )->getOriginalDependentVariableSettings( ); + + std::map , std::string> dependentVariableId = + dynamicsSimulator.getSingleArcPropagationResults( )->getDependentVariableId( ); + + std::map< std::pair< int, int >, std::shared_ptr< SingleDependentVariableSaveSettings > > orderedDependentVariableSettings = + dynamicsSimulator.getSingleArcPropagationResults( )-> getOrderedDependentVariableSettings( ); + + BOOST_CHECK_EQUAL( + originalDependentVariables.size( ), propagatorSettings->getDependentVariablesToSave( ).size( ) ); + for( unsigned int k = 0; k < originalDependentVariables.size( ); k++ ) + { + BOOST_CHECK_EQUAL( originalDependentVariables.at( k ), propagatorSettings->getDependentVariablesToSave( ).at( k ) ); + } + + + // Iterate over results for dependent variables, and check against manually retrieved values. + Eigen::Vector6d currentStateDerivative; + Eigen::Vector3d manualCentralGravity; + std::shared_ptr< ephemerides::RotationalEphemeris > earthRotationModel = + bodies.at( "Earth" )->getRotationalEphemeris( ); + std::shared_ptr< aerodynamics::AtmosphereModel > earthAtmosphereModel = + bodies.at( "Earth" )->getAtmosphereModel( ); + std::shared_ptr< gravitation::GravityFieldModel > earthGravityModel = + bodies.at( "Earth" )->getGravityFieldModel( ); + std::shared_ptr< gravitation::GravityFieldModel > moonGravityModel = + bodies.at( "Moon" )->getGravityFieldModel( ); + + std::shared_ptr< aerodynamics::AtmosphericFlightConditions > apolloFlightConditions = + std::dynamic_pointer_cast< aerodynamics::AtmosphericFlightConditions >( + bodies.at( "Apollo" )->getFlightConditions( ) ); + std::shared_ptr< aerodynamics::AerodynamicCoefficientInterface > apolloCoefficientInterface = + bodies.at( "Apollo" )->getAerodynamicCoefficientInterface( ); + bodies.at( "Apollo" )->setIsBodyInPropagation( true ); + + std::cout<<"Number of points: "<::iterator variableIterator = dependentVariableSolution.begin( ); + variableIterator != dependentVariableSolution.end( ); variableIterator++ ) + { + Eigen::VectorXd currentDependentVariables = variableIterator->second; + + double machNumber = currentDependentVariables( 0 ); + double altitude = currentDependentVariables( 1 ); + double relativeDistance = currentDependentVariables( 2 ); + double relativeSpeed= currentDependentVariables( 3 ); + double gravitationalAccelerationNorm = currentDependentVariables( 4 ); + double gLoad = currentDependentVariables( 5 ); + double stagnationPointHeatFlux = currentDependentVariables( 6 ); + double freestreamTemperature = currentDependentVariables( 7 ); + double geodeticLatitude = currentDependentVariables( 8 ); + double freestreamDensity = currentDependentVariables( 9 ); + double latitude = currentDependentVariables( 10 ); + double longitude = currentDependentVariables( 11 ); + double angleOfAttack = currentDependentVariables( 12 ); + double sideslipAngle = currentDependentVariables( 13 ); + double bankAngle = currentDependentVariables( 14 ); + + Eigen::Vector3d relativePosition = currentDependentVariables.segment( 15, 3 ); + Eigen::Vector3d computedBodyFixedPosition = + earthRotationModel->getRotationToTargetFrame( variableIterator->first ) * relativePosition; + Eigen::Vector3d computedSphericalBodyFixedPosition = + coordinate_conversions::convertCartesianToSpherical( computedBodyFixedPosition ); + + Eigen::Vector3d relativeVelocity = currentDependentVariables.segment( 18, 3 ); + Eigen::Vector3d gravitationalAcceleration = currentDependentVariables.segment( 21, 3 ); + Eigen::Vector3d totalAcceleration = currentDependentVariables.segment( 24, 3 ); + Eigen::Vector3d momentCoefficients = currentDependentVariables.segment( 27, 3 ); + Eigen::Vector3d forceCoefficients = currentDependentVariables.segment( 30, 3 ); + Eigen::Vector3d aerodynamicAcceleration = currentDependentVariables.segment( 33, 3 ); + Eigen::Vector3d moonAcceleration1 = currentDependentVariables.segment( 36, 3 ); + Eigen::Vector3d moonAcceleration2 = currentDependentVariables.segment( 39, 3 ); + + Eigen::Vector6d keplerElements = currentDependentVariables.segment( 42, 6 ); + Eigen::Vector6d modifiedEquinoctialElements = currentDependentVariables.segment( 48, 6 ); + Eigen::Vector3d bodyFixedCartesianPosition = currentDependentVariables.segment( 54, 3 ); + Eigen::Vector3d bodyFixedSphericalPosition = currentDependentVariables.segment( 57, 3 ); + Eigen::Matrix3d rswToInertialRotationMatrix = + propagators::getMatrixFromVectorRotationRepresentation( currentDependentVariables.segment( 60, 9 ) ); + Eigen::Vector3d customVariable1 = currentDependentVariables.segment( 69, 3 ); + Eigen::Vector3d customVariable2 = currentDependentVariables.segment( 72, 1 ); + + double earthGravitationalPotential = variableIterator->second( 73 ); + double moonGravitationalPotential = variableIterator->second( 74 ); + Eigen::Vector3d moonBodyFixedCartesianPosition = variableIterator->second.segment( 75, 3 ); + + currentStateDerivative = dynamicsSimulator.getDynamicsStateDerivative( )->computeStateDerivative( + variableIterator->first, rawNumericalSolution.at( variableIterator->first ) ); + + // Manually compute central gravity. + manualCentralGravity = + -bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ) * + relativePosition / + std::pow( relativePosition.norm( ), 3 ); + + // Check output time consistency + BOOST_CHECK_EQUAL( numericalSolution.count( variableIterator->first ), 1 ); + + // Check relative position and velocity against state + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + std::fabs( numericalSolution.at( variableIterator->first )( i ) - + relativePosition( i ) ), 2.0E-5 ); + BOOST_CHECK_SMALL( + std::fabs( numericalSolution.at( variableIterator->first )( 3 + i ) - + relativeVelocity( i ) ), 5.0E-11 ); + } + + // Check central gravity acceleration + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( + manualCentralGravity.segment( 0, 3 ), + gravitationalAcceleration, ( 6.0 * std::numeric_limits< double >::epsilon( ) ) ); + + if( propagatorType == 0 ) + { + // Check total acceleration (tolerance is not epsilon due to numerical root finding for trim) + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + std::fabs( currentStateDerivative( 3 + i ) - totalAcceleration( i ) ), 1.0E-13 ); + } + cowellAcceleration[ variableIterator->first ] = totalAcceleration; + } + else if( variableIterator->first < 100.0 ) + { + for( unsigned int i = 0; i < 3; i++ ) + { + BOOST_CHECK_SMALL( + std::fabs( cowellAcceleration[ variableIterator->first ]( i ) - totalAcceleration( i ) ), 1.0E-8 ); + } + } + + // Check relative position and velocity norm. + BOOST_CHECK_SMALL( + std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 0, 3 ) ).norm( ) - + relativeDistance ), 2.0E-5 ); + BOOST_CHECK_SMALL( + std::fabs( ( numericalSolution.at( variableIterator->first ).segment( 3, 3 ) ).norm( ) - + relativeSpeed ), 2.0E-11 ); + + // Check central gravity acceleration norm + BOOST_CHECK_CLOSE_FRACTION( + manualCentralGravity.norm( ), + gravitationalAccelerationNorm, 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check Mach number + BOOST_CHECK_CLOSE_FRACTION( + apolloFlightConditions->getCurrentAirspeed( ) / + apolloFlightConditions->getCurrentSpeedOfSound( ), + machNumber , std::numeric_limits< double >::epsilon( ) ); + + // Check altitude. + BOOST_CHECK_CLOSE_FRACTION( + apolloFlightConditions->getCurrentAltitude( ), + altitude, std::numeric_limits< double >::epsilon( ) ); + + // Check latitude and longitude + BOOST_CHECK_SMALL( + std::fabs( mathematical_constants::PI / 2.0 - computedSphericalBodyFixedPosition( 1 ) - latitude ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_SMALL( + std::fabs( computedSphericalBodyFixedPosition( 2 ) - longitude ), + 8.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check geodetic latitude. + if( !isOblateSpheroidUsed ) + { + BOOST_CHECK_SMALL( + std::fabs( latitude - geodeticLatitude ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + } + else + { + double computedGeodeticLatitude = coordinate_conversions::calculateGeodeticLatitude( + computedBodyFixedPosition, oblateSpheroidEquatorialRadius, oblateSpheroidFlattening, 1.0E-4 ); + BOOST_CHECK_SMALL( + std::fabs( computedGeodeticLatitude - geodeticLatitude ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + } + BOOST_CHECK_SMALL( + std::fabs( geodeticLatitude - apolloFlightConditions->getCurrentGeodeticLatitude( ) ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check sideslip anf bank angle + BOOST_CHECK_SMALL( + std::fabs( sideslipAngle ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_SMALL( + std::fabs( bankAngle ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check g-load + BOOST_CHECK_CLOSE_FRACTION( + gLoad, aerodynamicAcceleration.norm( ) / physical_constants::SEA_LEVEL_GRAVITATIONAL_ACCELERATION, + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check density and temperature + BOOST_CHECK_CLOSE_FRACTION( + freestreamDensity, + earthAtmosphereModel->getDensity( altitude, longitude, latitude, variableIterator->first ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( + freestreamDensity, + apolloFlightConditions->getCurrentDensity( ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( + freestreamTemperature, + earthAtmosphereModel->getTemperature( altitude, longitude, latitude, variableIterator->first ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_CLOSE_FRACTION( + freestreamTemperature, + apolloFlightConditions->getCurrentFreestreamTemperature( ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check aerodynamic force coefficients + std::vector< double > aerodynamicCoefficientInput; + aerodynamicCoefficientInput.push_back( machNumber ); + aerodynamicCoefficientInput.push_back( angleOfAttack ); + aerodynamicCoefficientInput.push_back( sideslipAngle ); + apolloCoefficientInterface->updateCurrentCoefficients( aerodynamicCoefficientInput ); + Eigen::Vector3d computedForceCoefficients = apolloCoefficientInterface->getCurrentForceCoefficients( ); + + BOOST_CHECK_SMALL( std::fabs( computedForceCoefficients( 1 ) ), 1.0E-14 ); + for( unsigned int i = 0; i < 3; i ++ ) + { + BOOST_CHECK_SMALL( + std::fabs( computedForceCoefficients( i ) - forceCoefficients( i ) ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + } + + // Check heat flux + double expectedHeatFlux = aerodynamics::computeEquilibriumFayRiddellHeatFlux( + freestreamDensity, apolloFlightConditions->getCurrentAirspeed( ), + freestreamTemperature, machNumber, noseRadius, wallEmissivity ); + BOOST_CHECK_CLOSE_FRACTION( + expectedHeatFlux, stagnationPointHeatFlux, + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check trimmed condition (y-term)/symmetric vehicle shape (x- and z-term). + BOOST_CHECK_SMALL( + std::fabs( momentCoefficients( 0 ) ), 1.0E-14 ); + BOOST_CHECK_SMALL( + std::fabs( momentCoefficients( 1 ) ), 1.0E-10 ); + BOOST_CHECK_SMALL( + std::fabs( momentCoefficients( 2 ) ), 1.0E-14 ); + + // Check if third-body gravity saving is done correctly + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( moonAcceleration1, moonAcceleration2, + ( 2.0 * std::numeric_limits< double >::epsilon( ) ) ); + + Eigen::Vector6d expectedKeplerElements = + tudat::orbital_element_conversions::convertCartesianToKeplerianElements( + Eigen::Vector6d( numericalSolution.at( variableIterator->first ) ), earthGravitationalParameter ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedKeplerElements, keplerElements, + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + + Eigen::Vector6d expectedModifiedEquinoctialElements = + tudat::orbital_element_conversions::convertCartesianToModifiedEquinoctialElements( + Eigen::Vector6d( numericalSolution.at( variableIterator->first ) ), earthGravitationalParameter ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( expectedModifiedEquinoctialElements, modifiedEquinoctialElements, + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + + for( unsigned int i = 0; i < 3; i ++ ) + { + BOOST_CHECK_SMALL( + std::fabs( computedBodyFixedPosition( i ) - bodyFixedCartesianPosition( i ) ), + 1.0E-8 ); + } + BOOST_CHECK_SMALL( + std::fabs( computedSphericalBodyFixedPosition( 0 ) - bodyFixedSphericalPosition( 0 ) ), + 1.0E-8 ); + BOOST_CHECK_SMALL( + std::fabs( tudat::mathematical_constants::PI / 2.0 - computedSphericalBodyFixedPosition( 1 ) - + bodyFixedSphericalPosition( 1 ) ), + 10.0 * std::numeric_limits< double >::epsilon( ) ); + BOOST_CHECK_SMALL( + std::fabs( computedSphericalBodyFixedPosition( 2 ) - bodyFixedSphericalPosition( 2 ) ), + 10.0 * std::numeric_limits< double >::epsilon( ) ); + Eigen::Matrix3d computedRswRotationMatrix = + tudat::reference_frames::getRswSatelliteCenteredToInertialFrameRotationMatrix( numericalSolution.at( variableIterator->first ) ); + + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( rswToInertialRotationMatrix, computedRswRotationMatrix, + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( customVariable1, ( relativePosition / 2.0 ), + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + BOOST_CHECK_CLOSE_FRACTION( customVariable2( 0 ), ( relativePosition.norm( ) / 2.0 ), + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + + + // Check central body gravitational potential + BOOST_CHECK_CLOSE_FRACTION( + earthGravitationalPotential, + earthGravityModel->getGravitationalPotential( computedBodyFixedPosition ), + 6.0 * std::numeric_limits< double >::epsilon( ) ); + + // Check 3rd body gravitational potential - not sure why the tolerance needs to be so large + BOOST_CHECK_CLOSE_FRACTION( + moonGravitationalPotential, + moonGravityModel->getGravitationalPotential( moonBodyFixedCartesianPosition ), + 1e-8 ); + } + } + } +} //! Function to test whether separate spherical harmonic acceleration contributions are correctly saved. BOOST_AUTO_TEST_CASE( testSphericalHarmonicDependentVariableOutput ) @@ -1684,216 +1702,216 @@ std::pair< int, double > getClosestSatelliteDistance( return std::make_pair( minimumDistanceIndex, distances[ minimumDistanceIndex ] ); } -//std::tuple< int, double, double > getClosestStationSatelliteDistance( -// const SystemOfBodies& bodies, -// const std::shared_ptr< ground_stations::GroundStation > groundStation, -// const std::vector< std::string >& bodyList, -// const double time ) -//{ -// std::vector< double > distances; -// std::vector< double > elevationAngles; -// std::vector< int > indices; - - -// Eigen::Vector6d stationState = getLinkEndCompleteEphemerisFunction( -// bodies.at( "Earth" ), std::make_pair( "Earth", groundStation->getStationId( ) ) )( time ); - -// for( unsigned int i = 0; i < bodyList.size( ); i++ ) -// { -// Eigen::Vector3d relativePosition = -// bodies.at( bodyList.at( i ) )->getEphemeris( )->getCartesianPosition( time ) - -// stationState.segment( 0, 3 ); -// double elevationAngle = groundStation->getPointingAnglesCalculator( )->calculateElevationAngle( -// relativePosition, time ); -// if( elevationAngle > 0.0 ) -// { -// elevationAngles.push_back( elevationAngle ); -// distances.push_back( relativePosition.norm( ) ); -// indices.push_back( i ); -// } -// } - -// if( elevationAngles.size( ) > 0 ) -// { -// int minimumDistanceIndex = std::distance(std::begin(distances), std::min_element(std::begin(distances), std::end(distances))); -// return std::make_tuple( indices.at( minimumDistanceIndex ), distances.at( minimumDistanceIndex ), elevationAngles.at( minimumDistanceIndex ) ); -// } -// else -// { -// return std::make_tuple( -1, TUDAT_NAN, TUDAT_NAN ); -// } - -//} - -//BOOST_AUTO_TEST_CASE( test_ConstellationVariables ) -//{ -// // Load Spice kernels. -// spice_interface::loadStandardSpiceKernels( ); - -// // Set simulation end epoch. -// const double simulationStartEpoch = 0.0 * tudat::physical_constants::JULIAN_DAY; -// const double simulationEndEpoch = 7.0 * tudat::physical_constants::JULIAN_DAY; - -// // Set numerical integration fixed step size. -// const double fixedStepSize = 120.0; - -// // Define body settings for simulation. -// BodyListSettings bodySettings =getDefaultBodySettings( -// {"Earth"}, "Earth", "J2000" ); - -// // Create Earth object -// SystemOfBodies bodies = createSystemOfBodies( bodySettings ); - -// // Create spacecraft object. -// bodies.createEmptyBody( "Satellite1" ); -// bodies.createEmptyBody( "Satellite2" ); -// bodies.createEmptyBody( "Satellite3" ); -// bodies.createEmptyBody( "Satellite4" ); -// bodies.createEmptyBody( "Satellite5" ); - -// // Define propagator settings variables. -// SelectedAccelerationMap accelerationMap; -// std::vector< std::string > bodiesToPropagate; -// std::vector< std::string > centralBodies; - -// // Define propagation settings. -// std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSatellite1; -// std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSatellite2; -// accelerationsOfSatellite1[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( -// 2, 2 ) ); -//// accelerationsOfSatellite1[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( -//// aerodynamic ) ); -// accelerationsOfSatellite2[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( -// 2, 2 ) ); -// accelerationMap[ "Satellite1" ] = accelerationsOfSatellite1; -// accelerationMap[ "Satellite2" ] = accelerationsOfSatellite1; - -// bodiesToPropagate.push_back( "Satellite1" ); -// bodiesToPropagate.push_back( "Satellite2" ); - -// centralBodies.push_back( "Earth" ); -// centralBodies.push_back( "Earth" ); - -// // Create acceleration models and propagation settings. -// basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( -// bodies, accelerationMap, bodiesToPropagate, centralBodies ); - -// double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); - -// // Set Keplerian elements for Asterix. -// Eigen::Vector6d satellite1InitialState; -// satellite1InitialState( semiMajorAxisIndex ) = 6800.0E3; -// satellite1InitialState( eccentricityIndex ) = 0.1; -// satellite1InitialState( inclinationIndex ) = convertDegreesToRadians( 85.3 ); -// satellite1InitialState( argumentOfPeriapsisIndex ) = convertDegreesToRadians( 235.7 ); -// satellite1InitialState( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 23.4 ); -// satellite1InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 ); - -// Eigen::Vector6d satellite2InitialState = satellite1InitialState; -// satellite2InitialState( eccentricityIndex ) = 0.0; -// satellite2InitialState( inclinationIndex ) = convertDegreesToRadians( 86.3 ); -// satellite2InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 141.87 ); - -// Eigen::Vector6d satellite3InitialState = satellite1InitialState; -// satellite3InitialState( eccentricityIndex ) = 0.0; -// satellite3InitialState( inclinationIndex ) = convertDegreesToRadians( 86.3 ); -// satellite3InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 137.87 ); -// bodies.at( "Satellite3" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( -// satellite3InitialState, 0.0,earthGravitationalParameter, "Earth", "J2000" ), -// "Satellite3" ) ); - -// Eigen::Vector6d satellite4InitialState = satellite1InitialState; -// satellite4InitialState( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 24.4 ); -// bodies.at( "Satellite4" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( -// satellite4InitialState, 0.0,earthGravitationalParameter, "Earth", "J2000" ), -// "Satellite4" ) ); - -// Eigen::Vector6d satellite5InitialState = satellite1InitialState; -// satellite5InitialState( semiMajorAxisIndex ) = 6810.0E3; -// bodies.at( "Satellite5" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( -// satellite5InitialState, 0.0, earthGravitationalParameter, "Earth", "J2000" ), -// "Satellite5" ) ); - -// createGroundStation( bodies.at( "Earth" ), "Station", ( Eigen::Vector3d( ) << 0.0, 1.5, 2.1 ).finished( ), -// coordinate_conversions::geodetic_position ); - -// // Convert Asterix state from Keplerian elements to Cartesian elements. -// Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 12 ); -// systemInitialState.segment( 0, 6 ) = convertKeplerianToCartesianElements( -// satellite1InitialState, -// earthGravitationalParameter ); -// systemInitialState.segment( 6, 6 ) = convertKeplerianToCartesianElements( -// satellite2InitialState, -// earthGravitationalParameter ); - -// std::shared_ptr< IntegratorSettings< > > integratorSettings = -// std::make_shared< IntegratorSettings< > > -// ( rungeKutta4, simulationStartEpoch, fixedStepSize ); - - -// std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; - -// dependentVariables.push_back( -// std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( -// "Satellite1", std::vector< std::string >( { "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ) ) ); -// dependentVariables.push_back( -// std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( -// "Satellite2", std::vector< std::string >( { "Satellite1", "Satellite3", "Satellite4", "Satellite5" } ) ) ); -// dependentVariables.push_back( -// std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( -// "Satellite3", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite4", "Satellite5" } ) ) ); -// dependentVariables.push_back( -// std::make_shared< MinimumConstellationStationDistanceDependentVariableSaveSettings >( -// "Earth", "Station", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), 0.0 ) ); - -// std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = -// std::make_shared< TranslationalStatePropagatorSettings< double > > -// ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationEndEpoch, cowell, dependentVariables ); - - -// // Create simulation object and propagate dynamics. -// SingleArcDynamicsSimulator< > dynamicsSimulator( -// bodies, integratorSettings, propagatorSettings, true, false, true ); -// std::map< double, Eigen::VectorXd > dependentVariableResults = dynamicsSimulator.getDependentVariableHistory( ); -// std::pair< int, double > testPair; -// std::tuple< int, double, double > testTuple; -// for( auto it : dependentVariableResults ) -// { -// testPair = getClosestSatelliteDistance( -// bodies, "Satellite1", std::vector< std::string >( { "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), it.first ); -// BOOST_CHECK_CLOSE_FRACTION( it.second( 0 ), testPair.second, 1.0E-12 ); -// BOOST_CHECK_EQUAL( it.second( 1 ), testPair.first ); - -// testPair = getClosestSatelliteDistance( -// bodies, "Satellite2", std::vector< std::string >( { "Satellite1", "Satellite3", "Satellite4", "Satellite5" } ), it.first ); -// BOOST_CHECK_CLOSE_FRACTION( it.second( 2 ), testPair.second, 1.0E-12 ); -// BOOST_CHECK_EQUAL( it.second( 3 ), testPair.first ); - -// testPair = getClosestSatelliteDistance( -// bodies, "Satellite3", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite4", "Satellite5" } ), it.first ); -// BOOST_CHECK_CLOSE_FRACTION( it.second( 4 ), testPair.second, 1.0E-12 ); -// BOOST_CHECK_EQUAL( it.second( 5 ), testPair.first ); - -// testTuple = getClosestStationSatelliteDistance( -// bodies, bodies.at( "Earth" )->getGroundStation( "Station" ), -// std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), -// it.first ); -// if( std::get< 0 >( testTuple ) == -1 ) -// { -// BOOST_CHECK_EQUAL( it.second( 7 ), -1 ); -// BOOST_CHECK_EQUAL( ( it.second( 6 ) != it.second( 6 ) ), true ); -// BOOST_CHECK_EQUAL( ( it.second( 8 ) != it.second( 8 ) ), true ); -// } -// else -// { -// BOOST_CHECK_EQUAL( it.second( 7 ), std::get< 0 >( testTuple ) ); -// BOOST_CHECK_CLOSE_FRACTION( it.second( 6 ), std::get< 1 >( testTuple ), 1.0E-12 ); -// BOOST_CHECK_CLOSE_FRACTION( it.second( 8 ), std::get< 2 >( testTuple ), 1.0E-12 ); -// } - -// } - -//} +std::tuple< int, double, double > getClosestStationSatelliteDistance( + const SystemOfBodies& bodies, + const std::shared_ptr< ground_stations::GroundStation > groundStation, + const std::vector< std::string >& bodyList, + const double time ) +{ + std::vector< double > distances; + std::vector< double > elevationAngles; + std::vector< int > indices; + + + Eigen::Vector6d stationState = getLinkEndCompleteEphemerisFunction( + bodies.at( "Earth" ), observation_models::LinkEndId( "Earth", groundStation->getStationId( ) ) )( time ); + + for( unsigned int i = 0; i < bodyList.size( ); i++ ) + { + Eigen::Vector3d relativePosition = + bodies.at( bodyList.at( i ) )->getEphemeris( )->getCartesianPosition( time ) - + stationState.segment( 0, 3 ); + double elevationAngle = groundStation->getPointingAnglesCalculator( )->calculateElevationAngle( + relativePosition, time ); + if( elevationAngle > 0.0 ) + { + elevationAngles.push_back( elevationAngle ); + distances.push_back( relativePosition.norm( ) ); + indices.push_back( i ); + } + } + + if( elevationAngles.size( ) > 0 ) + { + int minimumDistanceIndex = std::distance(std::begin(distances), std::min_element(std::begin(distances), std::end(distances))); + return std::make_tuple( indices.at( minimumDistanceIndex ), distances.at( minimumDistanceIndex ), elevationAngles.at( minimumDistanceIndex ) ); + } + else + { + return std::make_tuple( -1, TUDAT_NAN, TUDAT_NAN ); + } + +} + +BOOST_AUTO_TEST_CASE( test_ConstellationVariables ) +{ + // Load Spice kernels. + spice_interface::loadStandardSpiceKernels( ); + + // Set simulation end epoch. + const double simulationStartEpoch = 0.0 * tudat::physical_constants::JULIAN_DAY; + const double simulationEndEpoch = 7.0 * tudat::physical_constants::JULIAN_DAY; + + // Set numerical integration fixed step size. + const double fixedStepSize = 120.0; + + // Define body settings for simulation. + BodyListSettings bodySettings =getDefaultBodySettings( + {"Earth"}, "Earth", "J2000" ); + + // Create Earth object + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + + // Create spacecraft object. + bodies.createEmptyBody( "Satellite1" ); + bodies.createEmptyBody( "Satellite2" ); + bodies.createEmptyBody( "Satellite3" ); + bodies.createEmptyBody( "Satellite4" ); + bodies.createEmptyBody( "Satellite5" ); + + // Define propagator settings variables. + SelectedAccelerationMap accelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define propagation settings. + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSatellite1; + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfSatellite2; + accelerationsOfSatellite1[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( + 2, 2 ) ); +// accelerationsOfSatellite1[ "Earth" ].push_back( std::make_shared< AccelerationSettings >( +// aerodynamic ) ); + accelerationsOfSatellite2[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( + 2, 2 ) ); + accelerationMap[ "Satellite1" ] = accelerationsOfSatellite1; + accelerationMap[ "Satellite2" ] = accelerationsOfSatellite1; + + bodiesToPropagate.push_back( "Satellite1" ); + bodiesToPropagate.push_back( "Satellite2" ); + + centralBodies.push_back( "Earth" ); + centralBodies.push_back( "Earth" ); + + // Create acceleration models and propagation settings. + basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap( + bodies, accelerationMap, bodiesToPropagate, centralBodies ); + + double earthGravitationalParameter = bodies.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( ); + + // Set Keplerian elements for Asterix. + Eigen::Vector6d satellite1InitialState; + satellite1InitialState( semiMajorAxisIndex ) = 6800.0E3; + satellite1InitialState( eccentricityIndex ) = 0.1; + satellite1InitialState( inclinationIndex ) = convertDegreesToRadians( 85.3 ); + satellite1InitialState( argumentOfPeriapsisIndex ) = convertDegreesToRadians( 235.7 ); + satellite1InitialState( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 23.4 ); + satellite1InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 ); + + Eigen::Vector6d satellite2InitialState = satellite1InitialState; + satellite2InitialState( eccentricityIndex ) = 0.0; + satellite2InitialState( inclinationIndex ) = convertDegreesToRadians( 86.3 ); + satellite2InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 141.87 ); + + Eigen::Vector6d satellite3InitialState = satellite1InitialState; + satellite3InitialState( eccentricityIndex ) = 0.0; + satellite3InitialState( inclinationIndex ) = convertDegreesToRadians( 86.3 ); + satellite3InitialState( trueAnomalyIndex ) = convertDegreesToRadians( 137.87 ); + bodies.at( "Satellite3" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( + satellite3InitialState, 0.0,earthGravitationalParameter, "Earth", "J2000" ), + "Satellite3" ) ); + + Eigen::Vector6d satellite4InitialState = satellite1InitialState; + satellite4InitialState( longitudeOfAscendingNodeIndex ) = convertDegreesToRadians( 24.4 ); + bodies.at( "Satellite4" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( + satellite4InitialState, 0.0,earthGravitationalParameter, "Earth", "J2000" ), + "Satellite4" ) ); + + Eigen::Vector6d satellite5InitialState = satellite1InitialState; + satellite5InitialState( semiMajorAxisIndex ) = 6810.0E3; + bodies.at( "Satellite5" )->setEphemeris( createBodyEphemeris( std::make_shared< KeplerEphemerisSettings >( + satellite5InitialState, 0.0, earthGravitationalParameter, "Earth", "J2000" ), + "Satellite5" ) ); + + createGroundStation( bodies.at( "Earth" ), "Station", ( Eigen::Vector3d( ) << 0.0, 1.5, 2.1 ).finished( ), + coordinate_conversions::geodetic_position ); + + // Convert Asterix state from Keplerian elements to Cartesian elements. + Eigen::VectorXd systemInitialState = Eigen::VectorXd::Zero( 12 ); + systemInitialState.segment( 0, 6 ) = convertKeplerianToCartesianElements( + satellite1InitialState, + earthGravitationalParameter ); + systemInitialState.segment( 6, 6 ) = convertKeplerianToCartesianElements( + satellite2InitialState, + earthGravitationalParameter ); + + std::shared_ptr< IntegratorSettings< > > integratorSettings = + std::make_shared< IntegratorSettings< > > + ( rungeKutta4, simulationStartEpoch, fixedStepSize ); + + + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > dependentVariables; + + dependentVariables.push_back( + std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( + "Satellite1", std::vector< std::string >( { "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ) ) ); + dependentVariables.push_back( + std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( + "Satellite2", std::vector< std::string >( { "Satellite1", "Satellite3", "Satellite4", "Satellite5" } ) ) ); + dependentVariables.push_back( + std::make_shared< MinimumConstellationDistanceDependentVariableSaveSettings >( + "Satellite3", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite4", "Satellite5" } ) ) ); + dependentVariables.push_back( + std::make_shared< MinimumConstellationStationDistanceDependentVariableSaveSettings >( + "Earth", "Station", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), 0.0 ) ); + + std::shared_ptr< TranslationalStatePropagatorSettings< double > > propagatorSettings = + std::make_shared< TranslationalStatePropagatorSettings< double > > + ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState, simulationEndEpoch, cowell, dependentVariables ); + + + // Create simulation object and propagate dynamics. + SingleArcDynamicsSimulator< > dynamicsSimulator( + bodies, integratorSettings, propagatorSettings, true, false, true ); + std::map< double, Eigen::VectorXd > dependentVariableResults = dynamicsSimulator.getDependentVariableHistory( ); + std::pair< int, double > testPair; + std::tuple< int, double, double > testTuple; + for( auto it : dependentVariableResults ) + { + testPair = getClosestSatelliteDistance( + bodies, "Satellite1", std::vector< std::string >( { "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), it.first ); + BOOST_CHECK_CLOSE_FRACTION( it.second( 0 ), testPair.second, 1.0E-12 ); + BOOST_CHECK_EQUAL( it.second( 1 ), testPair.first ); + + testPair = getClosestSatelliteDistance( + bodies, "Satellite2", std::vector< std::string >( { "Satellite1", "Satellite3", "Satellite4", "Satellite5" } ), it.first ); + BOOST_CHECK_CLOSE_FRACTION( it.second( 2 ), testPair.second, 1.0E-12 ); + BOOST_CHECK_EQUAL( it.second( 3 ), testPair.first ); + + testPair = getClosestSatelliteDistance( + bodies, "Satellite3", std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite4", "Satellite5" } ), it.first ); + BOOST_CHECK_CLOSE_FRACTION( it.second( 4 ), testPair.second, 1.0E-12 ); + BOOST_CHECK_EQUAL( it.second( 5 ), testPair.first ); + + testTuple = getClosestStationSatelliteDistance( + bodies, bodies.at( "Earth" )->getGroundStation( "Station" ), + std::vector< std::string >( { "Satellite1", "Satellite2", "Satellite3", "Satellite4", "Satellite5" } ), + it.first ); + if( std::get< 0 >( testTuple ) == -1 ) + { + BOOST_CHECK_EQUAL( it.second( 7 ), -1 ); + BOOST_CHECK_EQUAL( ( it.second( 6 ) != it.second( 6 ) ), true ); + BOOST_CHECK_EQUAL( ( it.second( 8 ) != it.second( 8 ) ), true ); + } + else + { + BOOST_CHECK_EQUAL( it.second( 7 ), std::get< 0 >( testTuple ) ); + BOOST_CHECK_CLOSE_FRACTION( it.second( 6 ), std::get< 1 >( testTuple ), 1.0E-12 ); + BOOST_CHECK_CLOSE_FRACTION( it.second( 8 ), std::get< 2 >( testTuple ), 1.0E-12 ); + } + + } + +} BOOST_AUTO_TEST_SUITE_END( ) diff --git a/tests/src/astro/propagators/unitTestDependentVariablesInterface.cpp b/tests/src/astro/propagators/unitTestDependentVariablesInterface.cpp index 53d4ae7306..370b066e7a 100644 --- a/tests/src/astro/propagators/unitTestDependentVariablesInterface.cpp +++ b/tests/src/astro/propagators/unitTestDependentVariablesInterface.cpp @@ -140,7 +140,7 @@ BOOST_AUTO_TEST_CASE( testSingleArcDependentVariablesInterface ) std::make_shared< TranslationalStatePropagatorSettings< double > >( centralBodies, accelerationModelMap, bodiesToPropagate, initialState, simulationStartEpoch, integratorSettings, terminationSettings, cowell, dependentVariables ); - propagatorSettings->getOutputSettings()->setCreateDependentVariablesInterface( true ); + propagatorSettings->getOutputSettings()->setUpdateDependentVariableInterpolator( true ); // Define single-arc dynamics simulator SingleArcDynamicsSimulator< > simulator = SingleArcDynamicsSimulator< >( @@ -314,7 +314,7 @@ BOOST_AUTO_TEST_CASE( testMultiArcDependentVariablesInterface ) std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPropagatorSettings = std::make_shared >( propagatorSettingsList ); - multiArcPropagatorSettings->getOutputSettings( )->setCreateDependentVariablesInterface( true ); + multiArcPropagatorSettings->getOutputSettings( )->setUpdateDependentVariableInterpolator( true ); // Define single-arc dynamics simulator MultiArcDynamicsSimulator< > simulator = MultiArcDynamicsSimulator< >( @@ -376,213 +376,216 @@ BOOST_AUTO_TEST_CASE( testMultiArcDependentVariablesInterface ) std::numeric_limits< double >::epsilon( ) ); } } - } -//BOOST_AUTO_TEST_CASE( testHybridArcDependentVariablesInterface ) -//{ -// -// // Set simulation time settings. -// const double simulationStartEpoch = 0.0; -// const double simulationEndEpoch = 3.0 * tudat::physical_constants::JULIAN_DAY; -// -// // Load spice kernel. -// spice_interface::loadStandardSpiceKernels( ); -// -// // Define body settings for simulation. -// std::vector< std::string > bodiesToCreate; -// bodiesToCreate.push_back( "Sun" ); -// bodiesToCreate.push_back( "Earth" ); -// bodiesToCreate.push_back( "Mars" ); -// -// // Create system of bodies -// BodyListSettings bodySettings = getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 ); -// std::shared_ptr< BodySettings > phobosSettings = std::make_shared< BodySettings >( ); -// phobosSettings->ephemerisSettings = getDefaultEphemerisSettings( "Phobos" ); -// phobosSettings->gravityFieldSettings = getDefaultGravityFieldSettings( "Phobos", simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 ); -// bodySettings.addSettings( phobosSettings, "Phobos" ); -// -// SystemOfBodies bodies = createSystemOfBodies( bodySettings ); -// -// // Create spacecraft object. -// bodies.createEmptyBody( "AlienSpaceship" ); -// bodies.at( "AlienSpaceship" )->setConstantBodyMass( 400.0 ); -// bodies.at( "AlienSpaceship" )->setEphemeris( std::make_shared< MultiArcEphemeris >( -// std::map< double, std::shared_ptr< Ephemeris > >( ), "Mars", "ECLIPJ2000" ) ); -// -// // Create radiation pressure settings -// double referenceAreaRadiation = 4.0; -// double radiationPressureCoefficient = 1.2; -// std::vector< std::string > occultingBodies; -// occultingBodies.push_back( "Mars" ); -// std::shared_ptr< RadiationPressureInterfaceSettings > radiationPressureSettings = std::make_shared< CannonBallRadiationPressureInterfaceSettings >( -// "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); -// -// // Create and set radiation pressure settings -// bodies.at( "AlienSpaceship" )->setRadiationPressureInterface( -// "Sun", createRadiationPressureInterface( radiationPressureSettings, "AlienSpaceship", bodies ) ); -// -// // Define propagator settings variables. -// SelectedAccelerationMap phobosAccelerationMap; -// std::vector< std::string > bodiesToPropagate; -// std::vector< std::string > centralBodies; -// -// // Define propagation settings. -// std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > phobosAccelerations; -// phobosAccelerations[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); -// -// phobosAccelerationMap[ "Phobos" ] = phobosAccelerations; -// bodiesToPropagate.push_back( "Phobos" ); -// centralBodies.push_back( "Mars" ); -// -// basic_astrodynamics::AccelerationMap phobosAccelerationModelMap = createAccelerationModelsMap( -// bodies, phobosAccelerationMap, bodiesToPropagate, centralBodies ); -// -// // Define propagator settings variables. -// SelectedAccelerationMap spacecraftAccelerationMap; -// std::vector< std::string > multiArcBodiesToPropagate; -// std::vector< std::string > multiArcCentralBodies; -// -// // Define propagation settings. -// std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > spacecraftAccelerations; -// spacecraftAccelerations[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) ); -// spacecraftAccelerations[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); -// spacecraftAccelerations[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); -// spacecraftAccelerations[ "Phobos" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); -// spacecraftAccelerations[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::cannon_ball_radiation_pressure ) ); -// -// spacecraftAccelerationMap[ "AlienSpaceship" ] = spacecraftAccelerations; -// multiArcBodiesToPropagate.push_back( "AlienSpaceship" ); -// multiArcCentralBodies.push_back( "Mars" ); -// -// basic_astrodynamics::AccelerationMap spacecraftAccelerationModelMap = createAccelerationModelsMap( -// bodies, spacecraftAccelerationMap, multiArcBodiesToPropagate, multiArcCentralBodies ); -// -// // Set initial state for Phobos -// Eigen::Vector6d initialStatePhobos = bodies.at( "Phobos" )->getEphemeris( )->getCartesianState( simulationStartEpoch ); -// -// // Set Keplerian elements for spacecraft. -// Eigen::Vector6d initialKeplerianElements; -// initialKeplerianElements( semiMajorAxisIndex ) = 1000.0E3; -// initialKeplerianElements( eccentricityIndex ) = 0.0; -// initialKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 26.04 ); -// initialKeplerianElements( argumentOfPeriapsisIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); -// initialKeplerianElements( longitudeOfAscendingNodeIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); -// initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); -// -// std::vector< Eigen::Vector6d > arcWiseKeplerianStates; -// arcWiseKeplerianStates.push_back( initialKeplerianElements ); -// initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 60.0 ); -// arcWiseKeplerianStates.push_back( initialKeplerianElements ); -// initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 120.0 ); -// arcWiseKeplerianStates.push_back( initialKeplerianElements ); -// -// unsigned int nbArcs = arcWiseKeplerianStates.size( ); -// -// std::vector< double > arcStartTimes, arcEndTimes; -// std::vector< Eigen::Vector6d > arcWiseStates; -// double marsGravitationalParameter = bodies.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( ); -// for ( unsigned int i = 0 ; i < nbArcs ; i++ ) -// { -// arcWiseStates.push_back( convertKeplerianToCartesianElements( arcWiseKeplerianStates.at( i ), marsGravitationalParameter ) ); -// arcStartTimes.push_back( simulationStartEpoch + i * physical_constants::JULIAN_DAY ); -// arcEndTimes.push_back( simulationStartEpoch + ( i + 1 ) * physical_constants::JULIAN_DAY - 3600.0 ); -// } -// -// // Define list of dependent variables to save. -// std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > multiArcDependentVariables; -// multiArcDependentVariables.push_back( std::make_shared< SingleDependentVariableSaveSettings >( -// total_acceleration_dependent_variable, "AlienSpaceship" ) ); -// multiArcDependentVariables.push_back( std::make_shared< SingleDependentVariableSaveSettings >( -// keplerian_state_dependent_variable, "AlienSpaceship", "Mars" ) ); -// -// const double fixedStepSize = 10.0; -// std::shared_ptr< IntegratorSettings< > > integratorSettings = rungeKutta4Settings( fixedStepSize ); -// -// std::vector< std::shared_ptr< SingleArcPropagatorSettings< > > > propagatorSettingsList; -// for ( unsigned int i = 0 ; i < nbArcs ; i++ ) -// { -// propagatorSettingsList.push_back( std::make_shared< TranslationalStatePropagatorSettings< double > >( -// multiArcCentralBodies, spacecraftAccelerationModelMap, multiArcBodiesToPropagate, -// arcWiseStates.at( i ), arcStartTimes.at( i ), integratorSettings, -// std::make_shared< propagators::PropagationTimeTerminationSettings >( arcEndTimes.at( i ) ), cowell, multiArcDependentVariables ) ); -// } -// -// std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPropagatorSettings = -// std::make_shared >( propagatorSettingsList ); -// -// std::shared_ptr< TranslationalStatePropagatorSettings< double > > singleArcPropagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >( -// centralBodies, phobosAccelerationModelMap, bodiesToPropagate, initialStatePhobos, simulationStartEpoch, integratorSettings, -// std::make_shared< propagators::PropagationTimeTerminationSettings >( simulationEndEpoch ), cowell ); -// -// std::shared_ptr< HybridArcPropagatorSettings< > > hybridArcPropagatorSettings = std::make_shared< HybridArcPropagatorSettings< > >( -// singleArcPropagatorSettings, multiArcPropagatorSettings ); -// hybridArcPropagatorSettings->getOutputSettings( )->setCreateDependentVariablesInterface( true ); -// -// -// // Define single-arc dynamics simulator -// HybridArcDynamicsSimulator< > simulator = HybridArcDynamicsSimulator< >( bodies, hybridArcPropagatorSettings ); -// -// -// // Retrieve dependent variables history. -// std::map< double, Eigen::VectorXd > singleArcDependentVariablesHistory = simulator.getSingleArcDynamicsSimulator( )->getDependentVariableHistory( ); -// std::vector< std::map< double, Eigen::VectorXd > > multiArcDependentVariablesHistory = simulator.getMultiArcDynamicsSimulator( )->getDependentVariableHistory( ); -// -// // Retrieve dependent variables interface. -// std::shared_ptr< HybridArcDependentVariablesInterface< > > dependentVariablesInterface = -// std::dynamic_pointer_cast< HybridArcDependentVariablesInterface< > >( simulator.getDependentVariablesInterface( ) ); -// -// // Create dependent variables interpolator. -// for ( unsigned int i = 0 ; i < nbArcs ; i++ ) -// { -// std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::VectorXd > > dependentVariablesInterpolator -// = std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::VectorXd > >( -// utilities::createVectorFromMapKeys< Eigen::VectorXd, double >( multiArcDependentVariablesHistory.at( i ) ), -// utilities::createVectorFromMapValues< Eigen::VectorXd, double >( multiArcDependentVariablesHistory.at( i ) ), 4 ); -// -// std::vector< double > testEpochs; -// testEpochs.push_back( arcStartTimes.at( i ) + 10.0 ); -// testEpochs.push_back( arcStartTimes.at( i ) + ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 4.0 ); -// testEpochs.push_back( arcStartTimes.at( i ) + ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 2.0 ); -// testEpochs.push_back( arcStartTimes.at( i ) + 3.0 * ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 4.0 ); -// testEpochs.push_back( arcEndTimes.at( i ) - 10.0 ); -// -// // Check consistency between interpolator results and interface results. -// for ( unsigned int j = 0 ; j < testEpochs.size( ) ; j++ ) -// { -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( dependentVariablesInterpolator->interpolate( testEpochs[ j ] ), -// dependentVariablesInterface->getDependentVariables( testEpochs[ j ] ), -// std::numeric_limits< double >::epsilon( ) ); -// } -// -// -// std::map< double, Eigen::VectorXd > totalAccelerationHistory; -// for ( auto itr : multiArcDependentVariablesHistory.at( i ) ) -// { -// totalAccelerationHistory[ itr.first ] = itr.second.segment( 0, 3 ); -// } -// -// // Create total acceleration history interpolator. -// std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::VectorXd > > totalAccelerationInterpolator -// = std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::VectorXd > >( -// utilities::createVectorFromMapKeys< Eigen::VectorXd, double >( totalAccelerationHistory ), -// utilities::createVectorFromMapValues< Eigen::VectorXd, double >( totalAccelerationHistory ), 4 ); -// -// // Total acceleration dependent variable settings. -// std::shared_ptr< SingleDependentVariableSaveSettings > totalAccelerationDependentVariable -// = std::make_shared< SingleDependentVariableSaveSettings >( total_acceleration_dependent_variable, "AlienSpaceship" ); -// -// -// // Check consistency between interpolator results and interface results, for a single dependent variable. -// for ( unsigned int j = 0 ; j < testEpochs.size( ) ; j++ ) -// { -// TUDAT_CHECK_MATRIX_CLOSE_FRACTION( totalAccelerationInterpolator->interpolate( testEpochs[ j ] ), -// dependentVariablesInterface->getSingleDependentVariable( totalAccelerationDependentVariable, testEpochs[ j ] ), -// std::numeric_limits< double >::epsilon( ) ); -// } -// } -// -//} +BOOST_AUTO_TEST_CASE( testHybridArcDependentVariablesInterface ) +{ + + // Set simulation time settings. + const double simulationStartEpoch = 0.0; + const double simulationEndEpoch = 3.0 * tudat::physical_constants::JULIAN_DAY; + + // Load spice kernel. + spice_interface::loadStandardSpiceKernels( ); + + // Define body settings for simulation. + std::vector< std::string > bodiesToCreate; + bodiesToCreate.push_back( "Sun" ); + bodiesToCreate.push_back( "Earth" ); + bodiesToCreate.push_back( "Mars" ); + + // Create system of bodies + BodyListSettings bodySettings = getDefaultBodySettings( bodiesToCreate, simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 ); + std::shared_ptr< BodySettings > phobosSettings = std::make_shared< BodySettings >( ); + phobosSettings->ephemerisSettings = getDefaultEphemerisSettings( "Phobos" ); + phobosSettings->gravityFieldSettings = getDefaultGravityFieldSettings( "Phobos", simulationStartEpoch - 300.0, simulationEndEpoch + 300.0 ); + bodySettings.addSettings( phobosSettings, "Phobos" ); + + SystemOfBodies bodies = createSystemOfBodies( bodySettings ); + + // Create spacecraft object. + bodies.createEmptyBody( "AlienSpaceship" ); + bodies.at( "AlienSpaceship" )->setConstantBodyMass( 400.0 ); + bodies.at( "AlienSpaceship" )->setEphemeris( std::make_shared< MultiArcEphemeris >( + std::map< double, std::shared_ptr< Ephemeris > >( ), "Mars", "ECLIPJ2000" ) ); + + // Create radiation pressure settings + double referenceAreaRadiation = 4.0; + double radiationPressureCoefficient = 1.2; + std::vector< std::string > occultingBodies; + occultingBodies.push_back( "Mars" ); + std::shared_ptr< RadiationPressureInterfaceSettings > radiationPressureSettings = std::make_shared< CannonBallRadiationPressureInterfaceSettings >( + "Sun", referenceAreaRadiation, radiationPressureCoefficient, occultingBodies ); + + // Create and set radiation pressure settings + bodies.at( "AlienSpaceship" )->setRadiationPressureInterface( + "Sun", createRadiationPressureInterface( radiationPressureSettings, "AlienSpaceship", bodies ) ); + + // Define propagator settings variables. + SelectedAccelerationMap phobosAccelerationMap; + std::vector< std::string > bodiesToPropagate; + std::vector< std::string > centralBodies; + + // Define propagation settings. + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > phobosAccelerations; + phobosAccelerations[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); + + phobosAccelerationMap[ "Phobos" ] = phobosAccelerations; + bodiesToPropagate.push_back( "Phobos" ); + centralBodies.push_back( "Mars" ); + + basic_astrodynamics::AccelerationMap phobosAccelerationModelMap = createAccelerationModelsMap( + bodies, phobosAccelerationMap, bodiesToPropagate, centralBodies ); + + // Define propagator settings variables. + SelectedAccelerationMap spacecraftAccelerationMap; + std::vector< std::string > multiArcBodiesToPropagate; + std::vector< std::string > multiArcCentralBodies; + + // Define propagation settings. + std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > spacecraftAccelerations; + spacecraftAccelerations[ "Earth" ].push_back( std::make_shared< SphericalHarmonicAccelerationSettings >( 5, 5 ) ); + spacecraftAccelerations[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); + spacecraftAccelerations[ "Mars" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); + spacecraftAccelerations[ "Phobos" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::point_mass_gravity ) ); + spacecraftAccelerations[ "Sun" ].push_back( std::make_shared< AccelerationSettings >( basic_astrodynamics::cannon_ball_radiation_pressure ) ); + + spacecraftAccelerationMap[ "AlienSpaceship" ] = spacecraftAccelerations; + multiArcBodiesToPropagate.push_back( "AlienSpaceship" ); + multiArcCentralBodies.push_back( "Mars" ); + + basic_astrodynamics::AccelerationMap spacecraftAccelerationModelMap = createAccelerationModelsMap( + bodies, spacecraftAccelerationMap, multiArcBodiesToPropagate, multiArcCentralBodies ); + + // Set initial state for Phobos + Eigen::Vector6d initialStatePhobos = bodies.at( "Phobos" )->getEphemeris( )->getCartesianState( simulationStartEpoch ); + + // Set Keplerian elements for spacecraft. + Eigen::Vector6d initialKeplerianElements; + initialKeplerianElements( semiMajorAxisIndex ) = 1000.0E3; + initialKeplerianElements( eccentricityIndex ) = 0.0; + initialKeplerianElements( inclinationIndex ) = unit_conversions::convertDegreesToRadians( 26.04 ); + initialKeplerianElements( argumentOfPeriapsisIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); + initialKeplerianElements( longitudeOfAscendingNodeIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); + initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 0.0 ); + + std::vector< Eigen::Vector6d > arcWiseKeplerianStates; + arcWiseKeplerianStates.push_back( initialKeplerianElements ); + initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 60.0 ); + arcWiseKeplerianStates.push_back( initialKeplerianElements ); + initialKeplerianElements( trueAnomalyIndex ) = unit_conversions::convertDegreesToRadians( 120.0 ); + arcWiseKeplerianStates.push_back( initialKeplerianElements ); + + unsigned int nbArcs = arcWiseKeplerianStates.size( ); + + std::vector< double > arcStartTimes, arcEndTimes; + std::vector< Eigen::Vector6d > arcWiseStates; + double marsGravitationalParameter = bodies.at( "Mars" )->getGravityFieldModel( )->getGravitationalParameter( ); + for ( unsigned int i = 0 ; i < nbArcs ; i++ ) + { + arcWiseStates.push_back( convertKeplerianToCartesianElements( arcWiseKeplerianStates.at( i ), marsGravitationalParameter ) ); + arcStartTimes.push_back( simulationStartEpoch + i * physical_constants::JULIAN_DAY ); + arcEndTimes.push_back( simulationStartEpoch + ( i + 1 ) * physical_constants::JULIAN_DAY - 3600.0 ); + } + + // Define list of dependent variables to save. + std::vector< std::shared_ptr< SingleDependentVariableSaveSettings > > multiArcDependentVariables; + multiArcDependentVariables.push_back( std::make_shared< SingleDependentVariableSaveSettings >( + total_acceleration_dependent_variable, "AlienSpaceship" ) ); + multiArcDependentVariables.push_back( std::make_shared< SingleDependentVariableSaveSettings >( + keplerian_state_dependent_variable, "AlienSpaceship", "Mars" ) ); + + const double fixedStepSize = 10.0; + std::shared_ptr< IntegratorSettings< > > integratorSettings = rungeKutta4Settings( fixedStepSize ); + + std::vector< std::shared_ptr< SingleArcPropagatorSettings< > > > propagatorSettingsList; + for ( unsigned int i = 0 ; i < nbArcs ; i++ ) + { + propagatorSettingsList.push_back( std::make_shared< TranslationalStatePropagatorSettings< double > >( + multiArcCentralBodies, spacecraftAccelerationModelMap, multiArcBodiesToPropagate, + arcWiseStates.at( i ), arcStartTimes.at( i ), integratorSettings, + std::make_shared< propagators::PropagationTimeTerminationSettings >( arcEndTimes.at( i ) ), cowell, multiArcDependentVariables ) ); + } + + std::shared_ptr< MultiArcPropagatorSettings< > > multiArcPropagatorSettings = + std::make_shared >( propagatorSettingsList ); + + std::shared_ptr< TranslationalStatePropagatorSettings< double > > singleArcPropagatorSettings = std::make_shared< TranslationalStatePropagatorSettings< double > >( + centralBodies, phobosAccelerationModelMap, bodiesToPropagate, initialStatePhobos, simulationStartEpoch, integratorSettings, + std::make_shared< propagators::PropagationTimeTerminationSettings >( simulationEndEpoch ), cowell ); + + std::shared_ptr< HybridArcPropagatorSettings< > > hybridArcPropagatorSettings = std::make_shared< HybridArcPropagatorSettings< > >( + singleArcPropagatorSettings, multiArcPropagatorSettings ); + hybridArcPropagatorSettings->getOutputSettings( )->setUpdateDependentVariableInterpolator( true ); + + + // Define single-arc dynamics simulator + HybridArcDynamicsSimulator< > simulator = HybridArcDynamicsSimulator< >( bodies, hybridArcPropagatorSettings ); + + + // Retrieve dependent variables history. + std::map< double, Eigen::VectorXd > singleArcDependentVariablesHistory = simulator.getSingleArcDynamicsSimulator( )->getDependentVariableHistory( ); + std::vector< std::map< double, Eigen::VectorXd > > multiArcDependentVariablesHistory = simulator.getMultiArcDynamicsSimulator( )->getDependentVariableHistory( ); + + // Retrieve dependent variables interface. + std::shared_ptr< HybridArcDependentVariablesInterface< > > dependentVariablesInterface = + std::dynamic_pointer_cast< HybridArcDependentVariablesInterface< > >( simulator.getDependentVariablesInterface( ) ); + + std::cout<<( dependentVariablesInterface == nullptr )< > dependentVariablesInterpolator + = std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::VectorXd > >( + utilities::createVectorFromMapKeys< Eigen::VectorXd, double >( multiArcDependentVariablesHistory.at( i ) ), + utilities::createVectorFromMapValues< Eigen::VectorXd, double >( multiArcDependentVariablesHistory.at( i ) ), 4 ); + + std::vector< double > testEpochs; + testEpochs.push_back( arcStartTimes.at( i ) + 10.0 ); + testEpochs.push_back( arcStartTimes.at( i ) + ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 4.0 ); + testEpochs.push_back( arcStartTimes.at( i ) + ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 2.0 ); + testEpochs.push_back( arcStartTimes.at( i ) + 3.0 * ( arcEndTimes.at( i ) - arcStartTimes.at( i ) ) / 4.0 ); + testEpochs.push_back( arcEndTimes.at( i ) - 10.0 ); + + bool exceptionCaught = false; + try + { + dependentVariablesInterface->getDependentVariables( testEpochs[ 0 ] ); + } + catch( std::runtime_error const& ) + { + exceptionCaught = true; + } + BOOST_CHECK_EQUAL( exceptionCaught, true ); + + + std::map< double, Eigen::VectorXd > totalAccelerationHistory; + for ( auto itr : multiArcDependentVariablesHistory.at( i ) ) + { + totalAccelerationHistory[ itr.first ] = itr.second.segment( 0, 3 ); + } + + // Create total acceleration history interpolator. + std::shared_ptr< interpolators::OneDimensionalInterpolator< double, Eigen::VectorXd > > totalAccelerationInterpolator + = std::make_shared< interpolators::LagrangeInterpolator< double, Eigen::VectorXd > >( + utilities::createVectorFromMapKeys< Eigen::VectorXd, double >( totalAccelerationHistory ), + utilities::createVectorFromMapValues< Eigen::VectorXd, double >( totalAccelerationHistory ), 4 ); + + // Total acceleration dependent variable settings. + std::shared_ptr< SingleDependentVariableSaveSettings > totalAccelerationDependentVariable + = std::make_shared< SingleDependentVariableSaveSettings >( total_acceleration_dependent_variable, "AlienSpaceship" ); + + + // Check consistency between interpolator results and interface results, for a single dependent variable. + for ( unsigned int j = 0 ; j < testEpochs.size( ) ; j++ ) + { + TUDAT_CHECK_MATRIX_CLOSE_FRACTION( totalAccelerationInterpolator->interpolate( testEpochs[ j ] ), + dependentVariablesInterface->getSingleDependentVariable( totalAccelerationDependentVariable, testEpochs[ j ] ), + ( 10.0 * std::numeric_limits< double >::epsilon( ) ) ); + } + } + +} BOOST_AUTO_TEST_SUITE_END( ) diff --git a/tests/src/astro/propagators/unitTestHybridArcVariationalEquations.cpp b/tests/src/astro/propagators/unitTestHybridArcVariationalEquations.cpp index aaf2c128d5..432d55d00b 100644 --- a/tests/src/astro/propagators/unitTestHybridArcVariationalEquations.cpp +++ b/tests/src/astro/propagators/unitTestHybridArcVariationalEquations.cpp @@ -71,9 +71,6 @@ executeHybridArcMarsAndOrbiterSensitivitySimulation( const double arcOverlap = 5.0E3 ) { - //Load spice kernels. - spice_interface::loadStandardSpiceKernels( ); - std::vector< std::string > bodyNames; bodyNames.push_back( "Sun" ); bodyNames.push_back( "Mars" ); @@ -334,6 +331,10 @@ executeHybridArcMarsAndOrbiterSensitivitySimulation( BOOST_AUTO_TEST_CASE( testMarsAndOrbiterHybridArcVariationalEquationCalculation ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; // Define variables for numerical differentiation diff --git a/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp b/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp index 4e5e88d482..1d2183dbf9 100644 --- a/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp +++ b/tests/src/astro/propagators/unitTestMultiArcVariationalEquations.cpp @@ -71,10 +71,6 @@ executeMultiArcEarthMoonSimulation( const double arcDuration = 5.0E5, const double arcOverlap = 5.0E3 ) { - - //Load spice kernels. - spice_interface::loadStandardSpiceKernels( ); - // Define std::vector< std::string > bodyNames; bodyNames.push_back( "Earth" ); @@ -294,6 +290,10 @@ executeMultiArcEarthMoonSimulation( BOOST_AUTO_TEST_CASE( testEarthMoonMultiArcVariationalEquationCalculation ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; std::vector< std::vector< std::string > > centralBodiesSet; diff --git a/tests/src/astro/propagators/unitTestVariationalEquations.cpp b/tests/src/astro/propagators/unitTestVariationalEquations.cpp index 0759760384..389bbe668d 100644 --- a/tests/src/astro/propagators/unitTestVariationalEquations.cpp +++ b/tests/src/astro/propagators/unitTestVariationalEquations.cpp @@ -68,10 +68,6 @@ executeEarthMoonSimulation( const Eigen::Vector3d parameterPerturbation = Eigen::Vector3d::Zero( ), const bool propagateVariationalEquations = 1 ) { - - //Load spice kernels. - spice_interface::loadStandardSpiceKernels( ); - // Define std::vector< std::string > bodyNames; bodyNames.push_back( "Earth" ); @@ -230,6 +226,10 @@ executeEarthMoonSimulation( */ BOOST_AUTO_TEST_CASE( testEarthMoonVariationalEquationCalculation ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; std::vector< std::vector< std::string > > centralBodiesSet; @@ -349,8 +349,6 @@ executeOrbiterSimulation( { int numberOfParametersToEstimate = 10; - //Load spice kernels. - spice_interface::loadStandardSpiceKernels( ); // Define bodies in simulation std::vector< std::string > bodyNames; bodyNames.push_back( "Earth" ); @@ -530,6 +528,9 @@ executeOrbiterSimulation( */ BOOST_AUTO_TEST_CASE( testEarthOrbiterVariationalEquationCalculation ) { + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; // Define variables for numerical differentiation @@ -875,6 +876,8 @@ executePhobosRotationSimulation( BOOST_AUTO_TEST_CASE( testPhobosRotationVariationalEquationCalculation ) { + + //Load spice kernels. spice_interface::loadStandardSpiceKernels( ); std::pair< std::vector< Eigen::MatrixXd >, std::vector< Eigen::VectorXd > > currentOutput; @@ -1051,6 +1054,8 @@ BOOST_AUTO_TEST_CASE( testPhobosRotationVariationalEquationCalculation ) BOOST_AUTO_TEST_CASE( testMassRateVariationalEquations ) { + + //Load spice kernels. spice_interface::loadStandardSpiceKernels( ); // Set simulation time settings. diff --git a/tests/src/interface/spice/unitTestSpiceInterface.cpp b/tests/src/interface/spice/unitTestSpiceInterface.cpp index 8ab1376563..70b85c40f8 100644 --- a/tests/src/interface/spice/unitTestSpiceInterface.cpp +++ b/tests/src/interface/spice/unitTestSpiceInterface.cpp @@ -477,7 +477,7 @@ BOOST_AUTO_TEST_CASE( testSpiceWrappers_7 ) spiceKernelsLoaded = getTotalCountOfKernelsLoaded( ); // Loaded kernels should be 4. - BOOST_CHECK_EQUAL( spiceKernelsLoaded, 12 ); + BOOST_CHECK_EQUAL( spiceKernelsLoaded, 13 ); // Clear all Spice kernels. clearSpiceKernels( ); diff --git a/tests/src/math/integrators/unitTestIntegratorOrders.cpp b/tests/src/math/integrators/unitTestIntegratorOrders.cpp index 451c7085d1..e19cd9c84a 100644 --- a/tests/src/math/integrators/unitTestIntegratorOrders.cpp +++ b/tests/src/math/integrators/unitTestIntegratorOrders.cpp @@ -44,9 +44,6 @@ Eigen::Vector6d getFinalIntegrationError( const double numberOfOrbits, const double eccentricity) { - //Load spice kernels. - spice_interface::loadStandardSpiceKernels( ); - // Define bodies in simulation. std::vector< std::string > bodyNames; bodyNames.push_back( "Earth" ); @@ -116,6 +113,10 @@ Eigen::Vector6d getFinalIntegrationError( //! where the time step is expected to behave well. BOOST_AUTO_TEST_CASE( testPureFixedMultiStageNumericalIntegratorOrder ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + // List of schemes for which to test std::vector< CoefficientSets > coefficients = { rungeKutta4Classic, @@ -186,6 +187,10 @@ BOOST_AUTO_TEST_CASE( testPureFixedMultiStageNumericalIntegratorOrder ) //! where the time step is expected to behave well. BOOST_AUTO_TEST_CASE( testFixedMultiStageNumericalIntegratorOrder ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::vector< CoefficientSets > coefficients = { // heunEuler, // rungeKuttaFehlberg12, @@ -287,6 +292,10 @@ BOOST_AUTO_TEST_CASE( testFixedMultiStageNumericalIntegratorOrder ) //! substantial tuning (for a 10th order method, a factor 2 in time step will result in a factor 1024 in error) BOOST_AUTO_TEST_CASE( testFixedBulirschStoerNumericalIntegratorOrder ) { + + //Load spice kernels. + spice_interface::loadStandardSpiceKernels( ); + std::vector timeSteps = { 400.0, 350.0, 300.0, 250.0, 200.0 }; // Run test for different sequences diff --git a/version b/version index df87cda348..9e17525f0d 100644 --- a/version +++ b/version @@ -1 +1 @@ -2.12.1.dev24 +2.12.1.dev30