diff --git a/conf/TUDELFT/tudelft_rm_conf.xml b/conf/TUDELFT/tudelft_rm_conf.xml
index 6d2f46b2f6..444c6cf9d0 100644
--- a/conf/TUDELFT/tudelft_rm_conf.xml
+++ b/conf/TUDELFT/tudelft_rm_conf.xml
@@ -5,9 +5,20 @@
    airframe="airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml"
    radio="radios/dummy.xml"
    telemetry="telemetry/default_rotorcraft.xml"
+   flight_plan="flight_plans/rotorcraft_optitrack.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_indi.xml] settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml"
+   settings_modules=""
+   gui_color="red"
+  />
+  <aircraft
+   name="ardrone2_follow_me"
+   ac_id="2"
+   airframe="airframes/TUDELFT/tudelft_selfie.xml"
+   radio="radios/dummy.xml"
+   telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_oa_avoid.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_indi.xml] settings/control/stabilization_rate.xml settings/control/stabilization_att_int_quat.xml"
-   settings_modules="modules/obstacle_avoidance.xml"
+   settings_modules=""
    gui_color="red"
   />
   <aircraft
@@ -16,7 +27,7 @@
    airframe="airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml"
    radio="radios/cockpitSX.xml"
    telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/rotorcraft_optitrack_stereoavoid.xml"
+   flight_plan="flight_plans/rotorcraft_optitrack.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
    settings_modules=""
    gui_color="blue"
diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml
index 0ccf73558a..629f5812b8 100644
--- a/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml
+++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga.xml
@@ -195,8 +195,6 @@
  <modules main_freq="512">
    <load name="gps_ubx_ucenter.xml"/>
    <load name="send_imu_mag_current.xml"/>
-   <load name="stereocam_nav_line_avoid.xml"/>
-   <load name="mission_rotorcraft.xml"/>
  </modules>
 
   <firmware name="rotorcraft">
diff --git a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
index e2306c2ace..312fe4ad0d 100644
--- a/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
+++ b/conf/airframes/BR/ladybird_kit_bart_bluegiga_optitrack.xml
@@ -79,7 +79,7 @@
    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
-   <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
+   <define name="MODE_AUTO2" value="AP_MODE_HOVER_Z_HOLD"/>
 
     <define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
@@ -210,12 +210,11 @@
 
  <modules main_freq="512">
    <load name="send_imu_mag_current.xml"/>
-   <load name="stereocam_nav_line_avoid.xml"/>
-   <load name="mission_rotorcraft.xml"/>
  <load name="stereocam.xml">
 	<configure name="STEREO_UART" value="UART1"/>
 	<configure name="STEREO_BAUD" value="B115200"/>
 </load>
+<load name="stereocam_forward_velocity.xml"/>
  </modules>
 
   <firmware name="rotorcraft">
@@ -227,11 +226,6 @@
       <define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
     </target>
 
-    <target name="nps" board="pc">
-      <subsystem name="fdm" type="jsbsim"/>
-      <subsystem name="radio_control" type="ppm"/>
-    </target>
-
     <subsystem name="motor_mixing"/>
     <subsystem name="actuators" type="pwm">
       <!-- This airframe is using the servo PWM outputs directly to drive mosfets, which in turn directly drive brushed motors.
diff --git a/conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml b/conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml
index 1bf7facb52..4dc0330d71 100644
--- a/conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml
+++ b/conf/airframes/TUDELFT/tudelft_ardrone2_oa_clint_roland.xml
@@ -18,6 +18,8 @@
     <subsystem name="motor_mixing"/>
     <subsystem name="actuators" type="ardrone2"/>
     <subsystem name="imu" type="ardrone2"/>
+
+    <subsystem name="gps" type="datalink"/>
     <subsystem name="stabilization" type="int_quat"/>
     <subsystem name="ahrs" type="int_cmpl_quat"/>
     <subsystem name="ins" type="extended"/>
@@ -27,9 +29,9 @@
     <load name="bat_voltage_ardrone2.xml"/>
     <load name="stereocam.xml">
 	<configure name="STEREO_UART" value="UART1"/>
-	<configure name="STEREO_BAUD" value="B1000000"/>
+	<configure name="STEREO_BAUD" value="B115200"/>
     </load>
-    <load name="obstacle_avoidance.xml"/>
+    <load name="stereocam_forward_velocity.xml"/>
   </modules>
 
   <commands>
diff --git a/conf/airframes/TUDELFT/tudelft_selfie.xml b/conf/airframes/TUDELFT/tudelft_selfie.xml
index 37ff4b0dbc..16598b3b60 100644
--- a/conf/airframes/TUDELFT/tudelft_selfie.xml
+++ b/conf/airframes/TUDELFT/tudelft_selfie.xml
@@ -8,8 +8,9 @@
     </target>
 
     <define name="FAILSAFE_DESCENT_SPEED" value="0.5"/>
-    <define name="USE_SONAR" value="TRUE"/>
-    <define name="NO_GPS_NEEDED_FOR_NAV" value="TRUE"/>
+    <define name="USE_SONAR" value="FALSE"/>
+ <define name="USE_GPS_ALT" value="1"/>
+
     <configure name="USE_MAGNETOMETER" value="TRUE"/>
 
 	<!-- Subsystem section -->
diff --git a/conf/messages.xml b/conf/messages.xml
index 26e11c916e..7756ace64b 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -887,7 +887,14 @@
     <field name="servo" type="uint16[]"/>
   </message>
 
-  <!--112 is free -->
+
+  <message name="STEREO_VELOCITY" id="112">
+    <field name="disparity" type="uint8"/>
+    <field name="distance" type="float"/>
+    <field name="velocity" type="float"/>
+    <field name="velocitySideways" type="float"/>
+    <field name="velocityUpDown" type="float"/>
+  </message>
 
   <message name="MLX_SERIAL" id="113">
     <field name="serial0" type="uint32"/>
diff --git a/conf/modules/stereocam_follow_me.xml b/conf/modules/stereocam_follow_me.xml
index 2c4415d6e1..feb92eae24 100644
--- a/conf/modules/stereocam_follow_me.xml
+++ b/conf/modules/stereocam_follow_me.xml
@@ -2,7 +2,10 @@
 
 <module name="stereocam_follow_me" dir="stereocam">
   <doc>
-    <description>follows based on stereo</description>
+    <description> Follows a person using the reference given by the stereocam.
+  This module does so by changing the yaw angle and roll angle alternatively.
+  This way the drone does not drift away, and keeps looking at the person it tries to follow.
+ </description>
   </doc>
   <header>
     <file name="stereocam_follow_me/follow_me.h"/>
diff --git a/conf/telemetry/default_rotorcraft.xml b/conf/telemetry/default_rotorcraft.xml
index b358163be4..a06e2b97b9 100644
--- a/conf/telemetry/default_rotorcraft.xml
+++ b/conf/telemetry/default_rotorcraft.xml
@@ -26,6 +26,7 @@
       <message name="AIR_DATA"               period="1.3"/>
       <message name="SURVEY"                 period="2.5"/>
       <message name="OPTIC_FLOW_EST"         period="0.25"/>
+ <message name="STEREO_IMG"         period="0.25"/>
       <message name="VECTORNAV_INFO"      period="0.5"/>
     </mode>
 
diff --git a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c
index 27723756ef..8efa9e398b 100644
--- a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c
+++ b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.c
@@ -7,7 +7,9 @@
 /**
  * @file modules/stereocam/stereocam_follow_me/stereocam_follow_me.c
  * @author Roland
- * follows a person using the reference given by the stereocam.
+ * Follows a person using the reference given by the stereocam.
+ * This module does so by changing the yaw angle and roll angle alternatively.
+ * This way the drone does not drift away, and keeps looking at the person it tries to follow.
  */
 
 #include "modules/stereocam/stereocam_follow_me/follow_me.h"
diff --git a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h
index eb7d87b153..be6293f73b 100644
--- a/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h
+++ b/sw/airborne/modules/stereocam/stereocam_follow_me/follow_me.h
@@ -5,9 +5,11 @@
  *
  */
 /**
- * @file "modules/stereocam/stereocam_follow_me/stereocam_follow_me.h"
+ * @file modules/stereocam/stereocam_follow_me/stereocam_follow_me.h
  * @author Roland
- * follows a person using the reference given by the stereocam.
+ * Follows a person using the reference given by the stereocam.
+ * This module does so by changing the yaw angle and roll angle alternatively.
+ * This way the drone does not drift away, and keeps looking at the person it tries to follow.
  */
 
 #ifndef FOLLOW_ME_H
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index 0750518d7b..1b67b3c6fa 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/sw/airborne/subsystems/abi_sender_ids.h
@@ -230,5 +230,11 @@
 #define PX4FLOW_VELOCITY_ID 17
 #endif
 
+#ifndef STEREO_VELOCITY_ID
+#define STEREO_VELOCITY_ID 18
+#endif
+
+
+
 
 #endif /* ABI_SENDER_IDS_H */