From adea6198d28d04b506b4216967613f091234ac92 Mon Sep 17 00:00:00 2001 From: Dominic Dirkx Date: Fri, 13 Oct 2023 14:02:46 +0200 Subject: [PATCH] Added fix for non-ordered observations --- .../estimation_setup/observations.h | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h index 1206836338..c4583b4f41 100644 --- a/include/tudat/simulation/estimation_setup/observations.h +++ b/include/tudat/simulation/estimation_setup/observations.h @@ -69,6 +69,7 @@ class SingleObservationSet throw std::runtime_error( "Error when creating SingleObservationSet, ObservationDependentVariableCalculator has incompatible link ends " ); } } + if( observations_.size( ) != observationTimes_.size( ) ) { throw std::runtime_error( "Error when making SingleObservationSet, input sizes are inconsistent." + @@ -82,6 +83,26 @@ class SingleObservationSet throw std::runtime_error( "Error when making SingleObservationSet, input observables not of consistent size." ); } } + + if( !std::is_sorted( observations_.begin( ), observations_.end( ) ) ) + { + std::map< TimeType, Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > > observationsMap; + for( unsigned int i = 0; i < observations_.size( ); i++ ) + { + observationsMap[ observationTimes_.at( i ) ] = observations_.at( i ); + } + observationTimes_ = utilities::createVectorFromMapKeys( observationsMap ); + observations_ = utilities::createVectorFromMapValues( observationsMap ); + if( observationsDependentVariables_.size( ) > 0 ) + { + std::map< TimeType, Eigen::VectorXd > observationsDependentVariablesMap; + for( unsigned int i = 0; i < observationsDependentVariables_.size( ); i++ ) + { + observationsDependentVariablesMap[ observationTimes_.at( i ) ] = observationsDependentVariables_.at( i ); + } + observationsDependentVariables_ = utilities::createVectorFromMapValues( observationsDependentVariablesMap ); + } + } }