diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 09cb634742..d388bb1014 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -3,8 +3,10 @@
   <modules main_freq="512">
     <load name="servo_switch.xml"/>
     <load name="rotorcraft_cam.xml"/>
-    <!--load name="sonar_maxbotix_booz.xml"/-->
-    <!--load name="adc_generic_booz.xml"/-->
+    <load name="sonar_maxbotix_booz.xml"/>
+    <!--load name="adc_generic.xml">
+      <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
+    </load-->
     <!--load name="sys_mon.xml"/-->
   </modules>
 
@@ -34,8 +36,7 @@
     </subsystem>
     <subsystem name="stabilization"   type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_euler"/>
-    <subsystem name="ins"           type="hff"/>
-    <!--subsystem name="ins"           type="extended"/-->
+    <subsystem name="ins"           type="extended"/>
   </firmware>
 
   <servos driver="Asctec">
@@ -164,9 +165,10 @@
 
   <section name="INS" prefix="INS_">
     <define name="BARO_SENS" value="15." integer="16"/>
-    <!--define name="SONAR_SENS" value="0.0128633" integer="16"/--> <!-- 3.3V supply -->
-    <define name="SONAR_SENS" value="0.00833333" integer="16"/> <!-- 5V supply -->
-    <define name="SONAR_MAX_RANGE" value="4.0"/>
+    <define name="SONAR_SENS" value="0.00650498" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply -->
+    <define name="SONAR_MAX_RANGE" value="5.0"/>
+    <define name="SONAR_MIN_RANGE" value="0.25"/>
+    <!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/boards/booz_1.0.makefile b/conf/boards/booz_1.0.makefile
index a8f499e3e2..20058f06b2 100644
--- a/conf/boards/booz_1.0.makefile
+++ b/conf/boards/booz_1.0.makefile
@@ -21,11 +21,11 @@ endif
 #
 # default LED configuration
 #
-RADIO_CONTROL_LED  ?= 2
-BARO_LED           ?= none
+RADIO_CONTROL_LED  ?= 1
+BARO_LED           ?= 2
 AHRS_ALIGNER_LED   ?= 3
 GPS_LED            ?= 4
-SYS_TIME_LED       ?= 1
+SYS_TIME_LED       ?= none
 
 RADIO_CONTROL_LINK = UART0
 
diff --git a/sw/airborne/subsystems/ins/ins_int_extended.c b/sw/airborne/subsystems/ins/ins_int_extended.c
index f6dc8c6178..64c97707f6 100644
--- a/sw/airborne/subsystems/ins/ins_int_extended.c
+++ b/sw/airborne/subsystems/ins/ins_int_extended.c
@@ -102,6 +102,7 @@ void ins_init() {
 
   ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
   ins_ltp_def.hmsl = NAV_ALT0;
+  stateSetLocalOrigin_i(&ins_ltp_def);
 #else
   ins_ltp_initialised  = FALSE;
 #endif
@@ -165,6 +166,7 @@ void ins_propagate() {
   ins_ltp_accel.y = accel_meas_ltp.y;
 #endif /* USE_HFF */
 
+  INS_NED_TO_STATE();
 }
 
 void ins_update_baro() {
@@ -199,6 +201,7 @@ void ins_update_gps(void) {
       ins_ltp_def.lla.alt = gps.lla_pos.alt;
       ins_ltp_def.hmsl = gps.hmsl;
       ins_ltp_initialised = TRUE;
+      stateSetLocalOrigin_i(&ins_ltp_def);
     }
     ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
     ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
@@ -263,6 +266,9 @@ void ins_update_sonar() {
 
   /* update filter assuming a flat ground */
   if (sonar < INS_SONAR_MAX_RANGE
+#ifdef INS_SONAR_MIN_RANGE
+      && sonar > INS_SONAR_MIN_RANGE
+#endif
 #ifdef INS_SONAR_THROTTLE_THRESHOLD
       && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
 #endif