diff --git a/src/behaviours/behaviour-follow-robot.cpp b/src/behaviours/behaviour-follow-robot.cpp
@@ -33,7 +33,7 @@ void BehaviourFollowRobot::atAlignedPositionState() noexcept {
     if (std::abs(*m_pDetectionAngle) < ULTRASONIC_ANGLE_THRESHOLD)
         estimated_distance = *m_pFrontDistance;
     else
-        estimated_distance = *m_pDetectionDistance;
+        estimated_distance = (*m_pDetectionDistance);
 
     if (estimated_distance < PREFERRED_DISTANCE - MARGIN) {
         setState(&BehaviourFollowRobot::followBackward);
@@ -44,15 +44,29 @@ void BehaviourFollowRobot::atAlignedPositionState() noexcept {
 
 void BehaviourFollowRobot::followForward() noexcept {
     m_pedalLogic = 1;
-    if (*m_pDetectionDistance < PREFERRED_DISTANCE + MARGIN) {
+
+    double estimated_distance;
+    if (std::abs(*m_pDetectionAngle) < ULTRASONIC_ANGLE_THRESHOLD)
+        estimated_distance = *m_pFrontDistance;
+    else
+        estimated_distance = (*m_pDetectionDistance);
+
+    if (estimated_distance < PREFERRED_DISTANCE + MARGIN) {
         setState(&BehaviourFollowRobot::atAlignedPositionState);
     }
 }
 
 void BehaviourFollowRobot::followBackward() noexcept {
     m_pedalLogic = -1;
     m_groundSteeringAngle = (-1) * m_groundSteeringAngle; // Flip direction of steering because we're going in reverse. Steering is calculated every turn in the universal state.
-    if (*m_pDetectionDistance > PREFERRED_DISTANCE - MARGIN) {
+
+    double estimated_distance;
+    if (std::abs(*m_pDetectionAngle) < ULTRASONIC_ANGLE_THRESHOLD)
+        estimated_distance = *m_pFrontDistance;
+    else
+        estimated_distance = (*m_pDetectionDistance);
+
+    if (estimated_distance > PREFERRED_DISTANCE - MARGIN) {
         setState(&BehaviourFollowRobot::atAlignedPositionState); // Stop reversing as soon as we are within the margin of preferred distance
     }
 }
diff --git a/stalker.conf b/stalker.conf
@@ -23,5 +23,5 @@
 #_STALKER_OBLIGATORY_
 #MARGIN=0.2
 #PREFERRED_DISTANCE=0.5
-#SMOOTH_STEERING_THRESHOLD=0.5236
-#ULTRASONIC_ANGLE_THRESHOLD=0.2
+#SMOOTH_STEERING_THRESHOLD=1
+#ULTRASONIC_ANGLE_THRESHOLD=0.3