Added fix for non-ordered observations

tudat-team · Oct 13, 2023 · adea619 · adea619
1 parent c68ba30
commit adea619
Showing 1 changed file with 21 additions and 0 deletions.
diff --git 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 );
+            }
+        }
     }