diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index d3a1f27697..8017f92a94 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -5,6 +5,7 @@
     <load name="rotorcraft_cam.xml"/>
     <load name="sonar_adc.xml">
       <configure name="ADC_SONAR" value="ADC_0"/>
+      <define name="USE_SONAR"/>
       <!--define name="SENSOR_SYNC_SEND_SONAR"/-->
     </load>
     <!--load name="baro_mpl3115.xml">
@@ -24,6 +25,7 @@
   </modules>
 
   <firmware name="rotorcraft">
+    <define name="DEBUG_VFF_EXTENDED"/>
     <define name="USE_INS_NAV_INIT"/>
     <!--define name="GUIDANCE_H_USE_REF"/-->
 
@@ -185,7 +187,6 @@
   <section name="SONAR">
     <!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply -->
     <define name="SONAR_SENS" value="0.016775" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
-    <define name="USE_SONAR"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/settings/estimation/ins_sonar.xml b/conf/settings/estimation/ins_sonar.xml
index 0c016fa593..e709d2c6f5 100644
--- a/conf/settings/estimation/ins_sonar.xml
+++ b/conf/settings/estimation/ins_sonar.xml
@@ -3,7 +3,7 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="sonar">
-      <dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
+      <dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_impl.update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 4373e6fa80..7e30c91127 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/sw/airborne/boards/ardrone/navdata.c
@@ -64,10 +64,9 @@ measures_t navdata;
  *  Sensor sensitivity in m/adc (float)
  */
 #ifndef SONAR_SCALE
-#define SONAR_SCALE 0.00034
+#define SONAR_SCALE 0.00047
 #endif
 
-
 // FIXME(ben): there must be a better home for these
 ssize_t full_write(int fd, const uint8_t *buf, size_t count)
 {
@@ -482,7 +481,7 @@ void navdata_update()
         // Check if there is a new sonar measurement and update the sonar
         if (navdata.ultrasound >> 15)
         {
-          float sonar_meas = (navdata.ultrasound & 0x7FFF);
+          float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
           AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
         }
 #endif
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 32143b5a4d..93b94a28be 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -230,11 +230,12 @@ void ins_propagate(void) {
 }
 
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
-  if (!ins_impl.baro_initialized) {
+  if (!ins_impl.baro_initialized && *pressure > 1e-7) {
+    // wait for a first positive value
     ins_impl.qfe = *pressure;
     ins_impl.baro_initialized = TRUE;
   }
-  if (ins_impl.vf_reset) {
+  if (ins_impl.vf_reset && ins_impl.baro_initialized) {
     ins_impl.vf_reset = FALSE;
     ins_impl.qfe = *pressure;
     vff_realign(0.);
@@ -323,7 +324,7 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis
 
 #ifdef INS_SONAR_VARIANCE_THRESHOLD
   /* compute variance of error between sonar and baro alt */
-  float err = distance + ins_impl.baro_z; // sonar positive up, baro positive down !!!!
+  float err = *distance + ins_impl.baro_z; // sonar positive up, baro positive down !!!!
   var_err[var_idx] = err;
   var_idx = (var_idx + 1) % VAR_ERR_MAX;
   float var = variance_float(var_err, VAR_ERR_MAX);