diff --git a/conf/airframes/booz2.xml b/conf/airframes/booz2.xml
index 1055b961f2..ec3e445d5f 100644
--- a/conf/airframes/booz2.xml
+++ b/conf/airframes/booz2.xml
@@ -65,7 +65,6 @@
     <define name="GYRO_Y_NEUTRAL" value="32768"/>
     <define name="GYRO_Z_NEUTRAL" value="32768"/>
 
-
     <define name="GYRO_X_SENS" value="(-4541.3261)"/>
     <define name="GYRO_Y_SENS" value="(-4651.1628)"/>
     <define name="GYRO_Z_SENS" value="( 4752.8517)"/>
diff --git a/conf/autopilot/conf_booz2.makefile b/conf/autopilot/conf_booz2.makefile
index 7592ed27aa..5c6debd9f0 100644
--- a/conf/autopilot/conf_booz2.makefile
+++ b/conf/autopilot/conf_booz2.makefile
@@ -28,6 +28,7 @@ FLASH_MODE = IAP
 
 BOOZ=booz
 BOOZ_PRIV=booz_priv
+BOOZ_PRIV_ARCH=booz_priv/arm7
 BOOZ_ARCH=booz/arm7
 
 #
@@ -70,4 +71,27 @@ sim.srcs += $(BOOZ_PRIV)/booz_estimator_int.c
 sim.srcs += $(BOOZ_PRIV)/booz_cmp_flt_quat_int.c
 sim.srcs += $(BOOZ_PRIV)/booz_guidance_int.c
 sim.srcs += $(BOOZ_PRIV)/booz_stabilization_int.c
-sim.srcs += $(BOOZ_PRIV)/booz_supervision_int.c
\ No newline at end of file
+sim.srcs += $(BOOZ_PRIV)/booz_supervision_int.c
+
+
+
+# test imu
+test_imu.ARCHDIR = $(ARCHI)
+test_imu.ARCH = arm7tdmi
+test_imu.TARGET = test_imu
+test_imu.TARGETDIR = test_imu
+
+test_imu.CFLAGS += -DCONFIG=\"tiny_1_1.h\" -I$(BOOZ_PRIV_ARCH)
+test_imu.srcs += $(BOOZ_PRIV)/test_imu.c
+test_imu.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./1000.))' -DTIME_LED=1
+test_imu.CFLAGS += -DLED
+test_imu.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
+
+test_imu.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B57600
+test_imu.srcs += $(SRC_ARCH)/uart_hw.c
+
+test_imu.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 
+test_imu.srcs += downlink.c pprz_transport.c
+
+test_imu.CFLAGS += -DFLOAT_T=float
+test_imu.srcs += $(BOOZ_PRIV)/booz_imu_int.c $(BOOZ_PRIV_ARCH)/booz_imu_int_hw.c
\ No newline at end of file
diff --git a/conf/messages.xml b/conf/messages.xml
index a235de92bf..bca300cc26 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -871,14 +871,21 @@
    <field name="gr"    type="int32"/>
  </message>
 
+ <message name="BOOZ2_ACCEL" ID="132">   
+   <field name="ax"    type="int32"/>
+   <field name="ay"    type="int32"/>
+   <field name="az"    type="int32"/>
+ </message>
 
- <message name="BOOZ2_FILTER" ID="132">   
+ <message name="BOOZ2_FILTER" ID="133">   
+   <field name="gp"    type="int32"/>
+   <field name="gq"    type="int32"/>
+   <field name="gr"    type="int32"/>
    <field name="bp"    type="int32"/>
    <field name="bq"    type="int32"/>
    <field name="br"    type="int32"/>
  </message>
 
-
 <!--
   <message name="DC_MC_STATUS" ID="255">
       <field name="time_sec" type="uint16"/>
diff --git a/sw/simulator/booz_sensors_model_params.h b/sw/simulator/booz_sensors_model_params.h
index 2cda7e577f..53d3387147 100644
--- a/sw/simulator/booz_sensors_model_params.h
+++ b/sw/simulator/booz_sensors_model_params.h
@@ -74,7 +74,7 @@
 #define BSM_GYRO_NOISE_STD_DEV_Q  RadOfDeg(.5)
 #define BSM_GYRO_NOISE_STD_DEV_R  RadOfDeg(.5)
 
-#define BSM_GYRO_BIAS_INITIAL_P  RadOfDeg(  .5)
+#define BSM_GYRO_BIAS_INITIAL_P  RadOfDeg(  .0)
 #define BSM_GYRO_BIAS_INITIAL_Q  RadOfDeg(-0.5)
 #define BSM_GYRO_BIAS_INITIAL_R  RadOfDeg(  .25)
 
diff --git a/sw/simulator/booz_sensors_model_params_booz2.h b/sw/simulator/booz_sensors_model_params_booz2.h
index 9c090fb8bf..00b9ce4f9f 100644
--- a/sw/simulator/booz_sensors_model_params_booz2.h
+++ b/sw/simulator/booz_sensors_model_params_booz2.h
@@ -37,13 +37,13 @@
 #define BSM_ACCEL_NEUTRAL_Y       32768
 #define BSM_ACCEL_NEUTRAL_Z       32768
 /* m2s-4 */
-#define BSM_ACCEL_NOISE_STD_DEV_X 1e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Y 1e-1
-#define BSM_ACCEL_NOISE_STD_DEV_Z 1e-1
+#define BSM_ACCEL_NOISE_STD_DEV_X 0
+#define BSM_ACCEL_NOISE_STD_DEV_Y 0
+#define BSM_ACCEL_NOISE_STD_DEV_Z 0
 /* ms-2 */
-#define BSM_ACCEL_BIAS_X          1e-3
-#define BSM_ACCEL_BIAS_Y          1e-3
-#define BSM_ACCEL_BIAS_Z          1e-3
+#define BSM_ACCEL_BIAS_X          0
+#define BSM_ACCEL_BIAS_Y          0
+#define BSM_ACCEL_BIAS_Z          0
 /* s */
 #define BSM_ACCEL_DT              (1./1000.)
 
@@ -53,10 +53,6 @@
  * Gyrometer 
  */
 #define BSM_GYRO_RESOLUTION       65536
-/* degres/s - nominal 300 */
-//#define BSM_GYRO_SENSITIVITY_PP   65536. / (2.*RadOfDeg(-413.41848));
-//#define BSM_GYRO_SENSITIVITY_QQ   65536. / (2.*RadOfDeg(-403.65564));
-//#define BSM_GYRO_SENSITIVITY_RR   65536. / (2.*RadOfDeg( 395.01929));
 
 #define BSM_GYRO_SENSITIVITY_PP   (-4541.3261)
 #define BSM_GYRO_SENSITIVITY_QQ   (-4651.1628)
@@ -66,17 +62,17 @@
 #define BSM_GYRO_NEUTRAL_Q        32768
 #define BSM_GYRO_NEUTRAL_R        32768
 
-#define BSM_GYRO_NOISE_STD_DEV_P  RadOfDeg(.5)
-#define BSM_GYRO_NOISE_STD_DEV_Q  RadOfDeg(.5)
-#define BSM_GYRO_NOISE_STD_DEV_R  RadOfDeg(.5)
+#define BSM_GYRO_NOISE_STD_DEV_P  RadOfDeg(0.5)
+#define BSM_GYRO_NOISE_STD_DEV_Q  RadOfDeg(0.)
+#define BSM_GYRO_NOISE_STD_DEV_R  RadOfDeg(0.)
 
-#define BSM_GYRO_BIAS_INITIAL_P  RadOfDeg(  .0)
+#define BSM_GYRO_BIAS_INITIAL_P  RadOfDeg( 0.0)
 #define BSM_GYRO_BIAS_INITIAL_Q  RadOfDeg(  .0)
 #define BSM_GYRO_BIAS_INITIAL_R  RadOfDeg(  .0)
 
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(5.e-1)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(5.e-1)
-#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(5.e-1)
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_P RadOfDeg(0.5)
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_Q RadOfDeg(0.)
+#define BSM_GYRO_BIAS_RANDOM_WALK_STD_DEV_R RadOfDeg(0.)
 
 #define BSM_GYRO_DT (1./1000.)