Additional observation dependent variables working

include/tudat/simulation/estimation_setup/observationOutputSettings.h

@@ -115,13 +115,14 @@ class InterlinkObservationDependentVariableSettings: public ObservationDependent
             const std::string relativeBody = "" ):
     ObservationDependentVariableSettings( variableType ),
         startLinkEnd_( startLinkEnd ), endLinkEnd_( endLinkEnd ),
-        integratedObservableHandling_( integratedObservableHandling ){ }
+        integratedObservableHandling_( integratedObservableHandling ),
+        relativeBody_( relativeBody ){ }
 
     ~InterlinkObservationDependentVariableSettings( ){ }
 
     std::string getIdentifier( )
     {
-        std::string identifier = ", link from " + observation_models::getLinkEndTypeString( startLinkEnd_ ) +
+        std::string identifier = ", link from " + observation_models::getLinkEndTypeString( startLinkEnd_ ) + " to " +
             observation_models::getLinkEndTypeString( endLinkEnd_ );
         if( relativeBody_ != "" )
         {

src/astro/observation_models/observationViabilityCalculator.cpp

@@ -80,6 +80,7 @@ double computeMinimumLinkDistanceToPoint( const Eigen::Vector3d& observingBody,
     Eigen::Vector3d observerToPoint = relativePoint - observingBody;
     Eigen::Vector3d transmitterToPoint = relativePoint - transmittingBody;
     Eigen::Vector3d transmitterToObserver = observingBody - transmittingBody;
+    Eigen::Vector3d observerToTransmitter = transmittingBody - observingBody;
 
     double transmitterAngle = transmitterToObserver.dot( transmitterToPoint );
     double observerAngle = transmitterToObserver.dot( observerToPoint );
@@ -91,18 +92,20 @@ double computeMinimumLinkDistanceToPoint( const Eigen::Vector3d& observingBody,
     {
         if( std::fabs( transmitterAngle ) < std::fabs( observerAngle ) )
         {
-            minimumDistance = transmitterAngle;
+            minimumDistance = transmitterToPoint.norm( );
         }
         else
         {
-            minimumDistance = observerAngle;
+            minimumDistance = observerToPoint.norm( );
         }
     }
 
     // Minimum distance is impact parameter
     else
     {
-        minimumDistance = ( transmitterToObserver.cross( transmitterToPoint ) ).norm( ) / transmitterToObserver.norm( );
+        // Compute as average value with both points as reference, to minimuze numerical errors
+        minimumDistance = ( ( transmitterToObserver.cross( transmitterToPoint ) ).norm( ) / transmitterToObserver.norm( ) +
+            ( observerToTransmitter.cross( observerToPoint ) ).norm( ) / observerToTransmitter.norm( ) ) / 2.0;
     }
 
     return minimumDistance;

src/simulation/estimation_setup/observationOutput.cpp

@@ -225,7 +225,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
     }
     case body_avoidance_angle_variable:
     {
-        if ( bodies.count( variableSettings->relativeBody_ ) != 0 )
+        if ( bodies.count( variableSettings->relativeBody_ ) == 0 )
         {
             throw std::runtime_error( "Error when parsing body avoidance observation dependent variable w.r.t. " +
                                       variableSettings->relativeBody_ + ", body is not defined" );
@@ -246,7 +246,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
                 linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ),
                 linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ),
                 bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris<double, double>(
-                    ( linkEndIndicesToUse.first + linkEndIndicesToUse.second ) / 2.0 ).segment( 0, 3 ));
+                    ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 ));
             if ( std::fabs( cosineOfAvoidanceAngle ) >= 1.0 )
             {
                 return ( Eigen::Vector1d( )
@@ -261,7 +261,7 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
     }
     case link_body_center_distance:
     {
-        if ( bodies.count( variableSettings->relativeBody_ ) != 0 )
+        if ( bodies.count( variableSettings->relativeBody_ ) == 0 )
         {
             throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " +
                                       variableSettings->relativeBody_ + ", body is not defined" );
@@ -282,20 +282,24 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
                 linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ),
                 linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ),
                 bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris<double, double>(
-                    ( linkEndIndicesToUse.first + linkEndIndicesToUse.second ) / 2.0 ).segment( 0, 3 ));
+                    ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 ));
             return ( Eigen::Vector1d( ) << minimumDistance ).finished( );
         };
         break;
     }
     case link_limb_distance:
     {
-        std::shared_ptr< InterlinkObservationDependentVariableSettings > bodyCenterDistanceSettings =
-            std::make_shared< InterlinkObservationDependentVariableSettings >(
-                link_body_center_distance,
-                variableSettings->startLinkEnd_, variableSettings->endLinkEnd_,
-                variableSettings->integratedObservableHandling_, variableSettings->relativeBody_ );
-        ObservationDependentVariableFunction linkCenterFunction = getInterlinkObservationVariableFunction(
-            bodies, variableSettings, observableType, linkEnds );
+        if ( bodies.count( variableSettings->relativeBody_ ) == 0 )
+        {
+            throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " +
+                                      variableSettings->relativeBody_ + ", body is not defined" );
+        }
+
+        if ( bodies.at( variableSettings->relativeBody_ )->getEphemeris( ) == nullptr )
+        {
+            throw std::runtime_error( "Error when parsing link-body distance observation dependent variable w.r.t. " +
+                                      variableSettings->relativeBody_ + ", body has no ephemeris" );
+        }
 
         if ( bodies.at( variableSettings->relativeBody_ )->getShapeModel( ) == nullptr )
         {
@@ -308,14 +312,18 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
                                 const Eigen::VectorXd &observationValue,
                                 const std::shared_ptr<observation_models::ObservationAncilliarySimulationSettings> ancilliarySimulationSettings )
         {
-            return linkCenterFunction( linkEndTimes, linkEndStates, observationValue, ancilliarySimulationSettings ) -
-                ( Eigen::Vector1d( ) << shapeModel->getAverageRadius( ) ).finished( );
+            double minimumDistance = observation_models::computeMinimumLinkDistanceToPoint(
+                linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 ),
+                linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ),
+                bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris<double, double>(
+                    ( linkEndTimes.at( linkEndIndicesToUse.first ) + linkEndTimes.at( linkEndIndicesToUse.second ) ) / 2.0 ).segment( 0, 3 ));
+            return ( Eigen::Vector1d( ) << minimumDistance - shapeModel->getAverageRadius( ) ).finished( );
         };
         break;
     }
     case link_angle_with_orbital_plane:
     {
-        if ( bodies.count( variableSettings->relativeBody_ ) != 0 )
+        if ( bodies.count( variableSettings->relativeBody_ ) == 0 )
         {
             throw std::runtime_error( "Error when parsing link-orbital plane angle observation dependent variable w.r.t. " +
                                       variableSettings->relativeBody_ + ", body is not defined" );
@@ -336,9 +344,10 @@ ObservationDependentVariableFunction getInterlinkObservationVariableFunction(
                 linkEndStates.at( linkEndIndicesToUse.second ).segment( 0, 3 ) -
                 linkEndStates.at( linkEndIndicesToUse.first ).segment( 0, 3 );
             Eigen::Vector6d targetStateWrtCentralBody = linkEndStates.at( linkEndIndicesToUse.second ) -
-                bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris<double, double>( linkEndIndicesToUse.second );
+                bodies.at( variableSettings->relativeBody_ )->getStateInBaseFrameFromEphemeris<double, double>(
+                    linkEndTimes.at( linkEndIndicesToUse.second ) );
             Eigen::Vector3d orbitNormal = targetStateWrtCentralBody.segment< 3 >( 0 ).cross( targetStateWrtCentralBody.segment< 3 >( 3 ) );
-            return ( Eigen::Vector1d( ) << linear_algebra::computeAngleBetweenVectors( orbitNormal, vectorToTarget ) ).finished( );
+            return ( Eigen::Vector1d( ) << linear_algebra::computeAngleBetweenVectors( orbitNormal, vectorToTarget ) - mathematical_constants::PI / 2.0 ).finished( );
 
         };
         break;

src/simulation/estimation_setup/observationOutputSettings.cpp

@@ -73,7 +73,7 @@ std::string getObservationDependentVariableName(
     }
     case link_angle_with_orbital_plane:
     {
-        dependentVariableName = "Angle between link vector and orbit normal vector ";
+        dependentVariableName = "Angle between link vector and orbital plane ";
         break;
     }
     case doppler_integration_time_dependent_variable:
@@ -89,15 +89,15 @@ std::string getObservationDependentVariableName(
     default:
         throw std::runtime_error( "Error when checking observation dependent variable. Type " +
                                   std::to_string( variableType ) +
-                                  " not found when checking fif variable is vectorial." );
+                                  " not found when retrieving variable name." );
     }
     return dependentVariableName;
 }
 
 std::string getObservationDependentVariableId(
         const std::shared_ptr< ObservationDependentVariableSettings > variableSettings )
 {
-    return getObservationDependentVariableName( variableSettings->variableType_ ) + ". " +
+    return getObservationDependentVariableName( variableSettings->variableType_ ) +
             variableSettings->getIdentifier( );
 }
 
@@ -126,10 +126,16 @@ bool isObservationDependentVariableVectorial(
     case link_body_center_distance:
         isVariableVectorial = false;
         break;
+    case link_limb_distance:
+        isVariableVectorial = false;
+        break;
+    case link_angle_with_orbital_plane:
+        isVariableVectorial = false;
+        break;
     default:
         throw std::runtime_error( "Error when checking observation dependent variable. Type " +
                                   getObservationDependentVariableId( variableSettings ) +
-                                  " not found when checking fif variable is vectorial." );
+                                  " not found when checking if variable is vectorial." );
 
     }
     return isVariableVectorial;