Added functionality fot tudatpy issue 113

tudat-team · Sep 30, 2023 · e697f15 · e697f15
1 parent 9780f2d
commit e697f15
Showing 1 changed file with 34 additions and 0 deletions.
diff --git a/include/tudat/simulation/estimation_setup/observations.h b/include/tudat/simulation/estimation_setup/observations.h
@@ -368,6 +368,24 @@ class ObservationCollection
         return concatenatedLinkEndIdNames_;
     }
 
+    std::map< ObservableType, std::vector< LinkDefinition > > getLinkDefinitionsPerObservable( )
+    {
+        return linkDefinitionsPerObservable_;
+    }
+
+    std::vector< LinkDefinition > getLinkDefinitionsForSingleObservable(
+        const ObservableType observableType )
+    {
+        if( linkDefinitionsPerObservable_.count( observableType ) > 0 )
+        {
+            return linkDefinitionsPerObservable_.at( observableType );
+        }
+        else
+        {
+            return std::vector< LinkDefinition >( );
+        }
+    }
+
 
     std::map< ObservableType, std::map< int, std::vector< std::shared_ptr< SingleObservationSet< ObservationScalarType, TimeType > > > > > getSortedObservationSets( )
     {
@@ -448,6 +466,14 @@ class ObservationCollection
             for( auto linkEndIterator : observationIterator.second )
             {
                 LinkEnds currentLinkEnds = linkEndIterator.first;
+                LinkDefinition firstLinkDefinition;
+
+                if( linkEndIterator.second.size( ) > 0 )
+                {
+                    firstLinkDefinition = linkEndIterator.second.at( 0 )->getLinkEnds( );
+                    linkDefinitionsPerObservable_[ currentObservableType].push_back( firstLinkDefinition );
+                }
+
                 if( linkEndIds_.count( currentLinkEnds ) == 0 )
                 {
                     linkEndIds_[ currentLinkEnds ] = maximumStationId;
@@ -462,6 +488,12 @@ class ObservationCollection
 
                 for( unsigned int i = 0; i < linkEndIterator.second.size( ); i++ )
                 {
+                    LinkDefinition currentLinkDefinition = linkEndIterator.second.at( i )->getLinkEnds( );
+                    if( !( currentLinkDefinition == firstLinkDefinition ) )
+                    {
+                        throw std::runtime_error( "Error when creating ObservationCollection, link definitions of same link ends are not equal " );
+                    }
+
                     std::pair< int, int > startAndSize =
                             observationSetStartAndSize_.at( currentObservableType ).at( currentLinkEnds ).at( i );
                     Eigen::Matrix< ObservationScalarType, Eigen::Dynamic, 1 > currentObservables =
@@ -506,6 +538,8 @@ class ObservationCollection
 
     std::vector< LinkEnds > concatenatedLinkEndIdNames_;
 
+    std::map< ObservableType, std::vector< LinkDefinition > > linkDefinitionsPerObservable_;
+
     std::map< observation_models::LinkEnds, int > linkEndIds_;
 
     std::map< int, observation_models::LinkEnds > inverseLinkEndIds_;