diff --git a/conf/airframes/booz2_flixr.xml b/conf/airframes/booz2_flixr.xml
index c523bde362..ffa72b5e44 100644
--- a/conf/airframes/booz2_flixr.xml
+++ b/conf/airframes/booz2_flixr.xml
@@ -204,6 +204,9 @@
 
 		include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
 
+		#enable horizontal filter
+		ap.CFLAGS += -DUSE_HFF
+
   </makefile>
 
 </airframe>
diff --git a/conf/autopilot/booz2_autopilot.makefile b/conf/autopilot/booz2_autopilot.makefile
index ba896d55f1..ae0b2ada0e 100644
--- a/conf/autopilot/booz2_autopilot.makefile
+++ b/conf/autopilot/booz2_autopilot.makefile
@@ -130,7 +130,9 @@ ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
 
 ap.srcs += $(SRC_BOOZ)/booz2_ins.c
 ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
+# horizontal filter float version
 ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
+ap.CFLAGS += -DDT_HFILTER="(1./512)"
 #  vertical filter float version
 ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
 ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index e1755d7459..8c43837bca 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -29,13 +29,18 @@
 
 #include "airframe.h"
 #include "math/pprz_algebra_int.h"
+#include "math/pprz_algebra_float.h"
+
+#include "booz_ahrs.h"
 
 #ifdef USE_VFF
 #include "ins/booz2_vf_float.h"
 #endif
 
-#include "booz_ahrs.h"
+#ifdef USE_HFF
 #include "ins/booz2_hf_float.h"
+#endif
+
 
 #include "math/pprz_geodetic_int.h"
 
@@ -70,41 +75,59 @@ void booz_ins_init() {
   booz_ins_vff_realign = FALSE;
   b2_vff_init(0., 0., 0.);
 #endif
-  //#ifdef SITL
-  b2ins_init();
-  //#endif
+#ifdef USE_HFF
+  b2_hff_init(0., 0., 0., 0., 0., 0.);
+#endif
+  INT32_VECT3_ZERO(booz_ins_ltp_pos);
+  INT32_VECT3_ZERO(booz_ins_ltp_speed);
+  INT32_VECT3_ZERO(booz_ins_ltp_accel);
   INT32_VECT3_ZERO(booz_ins_enu_pos);
   INT32_VECT3_ZERO(booz_ins_enu_speed);
   INT32_VECT3_ZERO(booz_ins_enu_accel);
 }
 
 void booz_ins_propagate() {
+#ifdef BOOZ_INS_UNTILT_ACCEL
+  struct Int32Vect3 accel_body;
+  INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel);
+  struct Int32Vect3 accel_ltp;
+  INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body);
+#ifdef USE_HFF
+  float x_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.x);
+  float y_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.y);
+#endif
+  float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
+#else /* BOOZ_INS_UNTILT_ACCELS */
+#ifdef USE_HFF
+  float x_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.x);
+  float y_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.y);
+#endif
+  float z_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
+#endif /* BOOZ_INS_UNTILT_ACCELS */
 
 #ifdef USE_VFF
   if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING && booz_ins_baro_initialised) {
-#ifdef BOOZ_INS_UNTILT_ACCEL
-    struct Int32Vect3 accel_body;
-    INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel);
-    struct Int32Vect3 accel_ltp;
-    INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body);
-    float accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
-#else /* BOOZ_INS_UNTILT_ACCELS */
-    float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
-#endif /* BOOZ_INS_UNTILT_ACCELS */
-    b2_vff_propagate(accel_float);
+    b2_vff_propagate(z_accel_float);
     booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
     booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
     booz_ins_ltp_pos.z   = POS_BFP_OF_REAL(b2_vff_z);
-    booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
-    booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
-    booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
   }
-#endif
+#endif /* USE_VFF */
+
 #ifdef USE_HFF
-  if (booz_ahrs.status == BOOZ_AHRS_RUNNING &&
-      booz_gps_state.fix == BOOZ2_GPS_FIX_3D && booz_ins_ltp_initialised )
-    b2ins_propagate();
-#endif
+  if (booz_ahrs.status == BOOZ_AHRS_RUNNING && booz_gps_state.fix == BOOZ2_GPS_FIX_3D && booz_ins_ltp_initialised ) {
+    b2_hff_propagate(x_accel_float, y_accel_float);
+	booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
+	booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
+    booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
+	booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
+    booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_x);
+	booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_y);
+  }
+#endif /* USE_HFF */
+  INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
+  INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
+  INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
 }
 
 void booz_ins_update_baro() {
@@ -121,6 +144,12 @@ void booz_ins_update_baro() {
       booz_ins_vff_realign = FALSE;
       booz_ins_qfe = booz2_analog_baro_value;
       b2_vff_realign(0.);
+	  booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
+	  booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
+	  booz_ins_ltp_pos.z   = POS_BFP_OF_REAL(b2_vff_z);
+	  booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
+	  booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
+	  booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
     }
     b2_vff_update(alt_float);
   }
@@ -138,17 +167,36 @@ void booz_ins_update_gps(void) {
     ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
     ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
 #ifdef USE_HFF
-    b2ins_update_gps();
-    VECT2_SDIV(booz_ins_ltp_pos, b2ins_pos_ltp, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)));
-    VECT2_SDIV(booz_ins_ltp_speed, b2ins_speed_ltp, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
-#else
-    INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
-			INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
-    INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
-			INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
-    VECT3_COPY(booz_ins_ltp_pos,   b2ins_meas_gps_pos_ned);
-    VECT3_COPY(booz_ins_ltp_speed, b2ins_meas_gps_speed_ned);
+	struct FloatVect2 gps_float;
+#ifdef B2_HFF_UPDATE_POS
+	VECT2_ASSIGN(gps_float, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
+	VECT2_SDIV(gps_float, gps_float, 100.);
+	b2_hff_update_pos(gps_float.x, gps_float.y);
 #endif
+#ifdef B2_HFF_UPDATE_SPEED
+	VECT2_ASSIGN(gps_float, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
+	VECT2_SDIV(gps_float, gps_float, 100.);
+	b2_hff_update_v(gps_float.x, gps_float.y);
+#endif
+	booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
+	booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
+    booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
+	booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
+    booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_x);
+	booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_y);
+#ifndef USE_VFF /* only hf */
+	booz_ins_ltp_pos.z =  (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
+	booz_ins_ltp_speed.z =  (booz_ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
+#endif /* only hf */
+#else /* hf not used */
+	INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+    INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#ifndef USE_VFF /* neither hf nor vf used */
+    INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
+    INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
+#endif /* neither hf nor vf used */
+#endif /* USE_HFF */
+	
     INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
     INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
     INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
diff --git a/sw/airborne/booz/booz2_ins.h b/sw/airborne/booz/booz2_ins.h
index ad7ca800b5..638c30dcbf 100644
--- a/sw/airborne/booz/booz2_ins.h
+++ b/sw/airborne/booz/booz2_ins.h
@@ -50,7 +50,6 @@ extern struct EnuCoor_i booz_ins_enu_pos;
 extern struct EnuCoor_i booz_ins_enu_speed;
 extern struct EnuCoor_i booz_ins_enu_accel;
 
-
 extern void booz_ins_init( void );
 extern void booz_ins_propagate( void );
 extern void booz_ins_update_baro( void );
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
new file mode 100644
index 0000000000..756f330002
--- /dev/null
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -0,0 +1,363 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ * Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#include "booz2_hf_float.h"
+
+/*
+
+X_x = [ x xdot xbias ]
+X_y = [ y ydot ybias ]
+
+
+*/
+/* initial covariance diagonal */
+#define INIT_PXX 1.
+/* process noise (is the same for x and y)*/
+#define ACCEL_NOISE 0.5
+#define Q       ACCEL_NOISE/512./512./2.
+#define Qdotdot ACCEL_NOISE/512.
+#define Qbiasbias 1e-7
+#define R 1.
+
+float b2_hff_x;
+float b2_hff_xbias;
+float b2_hff_xdot;
+float b2_hff_xdotdot;
+
+float b2_hff_y;
+float b2_hff_ybias;
+float b2_hff_ydot;
+float b2_hff_ydotdot;
+
+float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+
+float b2_hff_x_meas;
+float b2_hff_y_meas;
+
+
+static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias);
+static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias);
+
+static inline void b2_hff_propagate_x(float xaccel);
+static inline void b2_hff_propagate_y(float yaccel);
+
+static inline void b2_hff_update_x(float x_meas);
+static inline void b2_hff_update_y(float y_meas);
+
+static inline void b2_hff_update_xdot(float v);
+static inline void b2_hff_update_ydot(float v);
+
+
+
+void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias) {
+    b2_hff_init_x(init_x, init_xdot, init_xbias);
+    b2_hff_init_y(init_y, init_ydot, init_ybias);
+}
+
+static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias) {
+  b2_hff_x     = init_x;
+  b2_hff_xdot  = init_xdot;
+  b2_hff_xbias = init_xbias;
+  int i, j;
+  for (i=0; i<B2_HFF_STATE_SIZE; i++) {
+    for (j=0; j<B2_HFF_STATE_SIZE; j++)
+      b2_hff_xP[i][j] = 0.;
+    b2_hff_xP[i][i] = INIT_PXX;
+  }
+
+}
+
+static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias) {
+  b2_hff_y     = init_y;
+  b2_hff_ydot  = init_ydot;
+  b2_hff_ybias = init_ybias;
+  int i, j;
+  for (i=0; i<B2_HFF_STATE_SIZE; i++) {
+    for (j=0; j<B2_HFF_STATE_SIZE; j++)
+      b2_hff_yP[i][j] = 0.;
+    b2_hff_yP[i][i] = INIT_PXX;
+  }
+
+}
+
+
+/*
+
+ F = [ 1 dt -dt^2/2
+       0  1 -dt
+       0  0   1     ];
+
+ B = [ dt^2/2 dt 0]';
+
+ Q = [ 0.01  0     0
+       0     0.01  0
+       0     0     0.001 ];
+
+ Xk1 = F * Xk0 + B * accel;
+
+ Pk1 = F * Pk0 * F' + Q;
+
+*/
+void b2_hff_propagate(float xaccel, float yaccel) {
+    b2_hff_propagate_x(xaccel);
+    b2_hff_propagate_y(yaccel);
+}
+
+static inline void b2_hff_propagate_x(float xaccel) {
+  /* update state */
+  b2_hff_xdotdot = xaccel - b2_hff_xbias;
+  b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot;
+  b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
+  /* update covariance */
+  const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] + b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] );
+  const float FPF01 = b2_hff_xP[0][1] + DT_HFILTER * ( b2_hff_xP[1][1] - b2_hff_xP[0][2] - DT_HFILTER * b2_hff_xP[1][2] );
+  const float FPF02 = b2_hff_xP[0][2] + DT_HFILTER * ( b2_hff_xP[1][2] );
+  const float FPF10 = b2_hff_xP[1][0] + DT_HFILTER * (-b2_hff_xP[2][0] + b2_hff_xP[1][1] - DT_HFILTER * b2_hff_xP[2][1] );
+  const float FPF11 = b2_hff_xP[1][1] + DT_HFILTER * (-b2_hff_xP[2][1] - b2_hff_xP[1][2] + DT_HFILTER * b2_hff_xP[2][2] );
+  const float FPF12 = b2_hff_xP[1][2] + DT_HFILTER * (-b2_hff_xP[2][2] );
+  const float FPF20 = b2_hff_xP[2][0] + DT_HFILTER * ( b2_hff_xP[2][1] );
+  const float FPF21 = b2_hff_xP[2][1] + DT_HFILTER * (-b2_hff_xP[2][2] );
+  const float FPF22 = b2_hff_xP[2][2];
+
+  b2_hff_xP[0][0] = FPF00 + Q;
+  b2_hff_xP[0][1] = FPF01;
+  b2_hff_xP[0][2] = FPF02;
+  b2_hff_xP[1][0] = FPF10;
+  b2_hff_xP[1][1] = FPF11 + Qdotdot;
+  b2_hff_xP[1][2] = FPF12;
+  b2_hff_xP[2][0] = FPF20;
+  b2_hff_xP[2][1] = FPF21;
+  b2_hff_xP[2][2] = FPF22 + Qbiasbias;
+
+}
+
+static inline void b2_hff_propagate_y(float yaccel) {
+  /* update state */
+  b2_hff_ydotdot = yaccel - b2_hff_ybias;
+  b2_hff_y = b2_hff_y + DT_HFILTER * b2_hff_ydot;
+  b2_hff_ydot = b2_hff_ydot + DT_HFILTER * b2_hff_ydotdot;
+  /* update covariance */
+  const float FPF00 = b2_hff_yP[0][0] + DT_HFILTER * ( b2_hff_yP[1][0] + b2_hff_yP[0][1] + DT_HFILTER * b2_hff_yP[1][1] );
+  const float FPF01 = b2_hff_yP[0][1] + DT_HFILTER * ( b2_hff_yP[1][1] - b2_hff_yP[0][2] - DT_HFILTER * b2_hff_yP[1][2] );
+  const float FPF02 = b2_hff_yP[0][2] + DT_HFILTER * ( b2_hff_yP[1][2] );
+  const float FPF10 = b2_hff_yP[1][0] + DT_HFILTER * (-b2_hff_yP[2][0] + b2_hff_yP[1][1] - DT_HFILTER * b2_hff_yP[2][1] );
+  const float FPF11 = b2_hff_yP[1][1] + DT_HFILTER * (-b2_hff_yP[2][1] - b2_hff_yP[1][2] + DT_HFILTER * b2_hff_yP[2][2] );
+  const float FPF12 = b2_hff_yP[1][2] + DT_HFILTER * (-b2_hff_yP[2][2] );
+  const float FPF20 = b2_hff_yP[2][0] + DT_HFILTER * ( b2_hff_yP[2][1] );
+  const float FPF21 = b2_hff_yP[2][1] + DT_HFILTER * (-b2_hff_yP[2][2] );
+  const float FPF22 = b2_hff_yP[2][2];
+
+  b2_hff_yP[0][0] = FPF00 + Q;
+  b2_hff_yP[0][1] = FPF01;
+  b2_hff_yP[0][2] = FPF02;
+  b2_hff_yP[1][0] = FPF10;
+  b2_hff_yP[1][1] = FPF11 + Qdotdot;
+  b2_hff_yP[1][2] = FPF12;
+  b2_hff_yP[2][0] = FPF20;
+  b2_hff_yP[2][1] = FPF21;
+  b2_hff_yP[2][2] = FPF22 + Qbiasbias;
+
+}
+
+
+
+/*
+  H = [1 0 0];
+  R = 0.1;
+  // state residual
+  y = pos_measurement - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*y;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+void b2_hff_update_pos (float posx, float posy) {
+    b2_hff_update_x(posx);
+    b2_hff_update_y(posy);
+}
+
+static inline void b2_hff_update_x(float x_meas) {
+  b2_hff_x_meas = x_meas;
+
+  const float y = x_meas - b2_hff_x;
+  const float S = b2_hff_xP[0][0] + R;
+  const float K1 = b2_hff_xP[0][0] * 1/S;
+  const float K2 = b2_hff_xP[1][0] * 1/S;
+  const float K3 = b2_hff_xP[2][0] * 1/S;
+
+  b2_hff_x     = b2_hff_x     + K1 * y;
+  b2_hff_xdot  = b2_hff_xdot  + K2 * y;
+  b2_hff_xbias = b2_hff_xbias + K3 * y;
+
+  const float P11 = (1. - K1) * b2_hff_xP[0][0];
+  const float P12 = (1. - K1) * b2_hff_xP[0][1];
+  const float P13 = (1. - K1) * b2_hff_xP[0][2];
+  const float P21 = -K2 * b2_hff_xP[0][0] + b2_hff_xP[1][0];
+  const float P22 = -K2 * b2_hff_xP[0][1] + b2_hff_xP[1][1];
+  const float P23 = -K2 * b2_hff_xP[0][2] + b2_hff_xP[1][2];
+  const float P31 = -K3 * b2_hff_xP[0][0] + b2_hff_xP[2][0];
+  const float P32 = -K3 * b2_hff_xP[0][1] + b2_hff_xP[2][1];
+  const float P33 = -K3 * b2_hff_xP[0][2] + b2_hff_xP[2][2];
+
+  b2_hff_xP[0][0] = P11;
+  b2_hff_xP[0][1] = P12;
+  b2_hff_xP[0][2] = P13;
+  b2_hff_xP[1][0] = P21;
+  b2_hff_xP[1][1] = P22;
+  b2_hff_xP[1][2] = P23;
+  b2_hff_xP[2][0] = P31;
+  b2_hff_xP[2][1] = P32;
+  b2_hff_xP[2][2] = P33;
+
+}
+
+static inline void b2_hff_update_y(float y_meas) {
+  b2_hff_y_meas = y_meas;
+
+  const float y = y_meas - b2_hff_y;
+  const float S = b2_hff_yP[0][0] + R;
+  const float K1 = b2_hff_yP[0][0] * 1/S;
+  const float K2 = b2_hff_yP[1][0] * 1/S;
+  const float K3 = b2_hff_yP[2][0] * 1/S;
+
+  b2_hff_y     = b2_hff_y     + K1 * y;
+  b2_hff_ydot  = b2_hff_ydot  + K2 * y;
+  b2_hff_ybias = b2_hff_ybias + K3 * y;
+
+  const float P11 = (1. - K1) * b2_hff_yP[0][0];
+  const float P12 = (1. - K1) * b2_hff_yP[0][1];
+  const float P13 = (1. - K1) * b2_hff_yP[0][2];
+  const float P21 = -K2 * b2_hff_yP[0][0] + b2_hff_yP[1][0];
+  const float P22 = -K2 * b2_hff_yP[0][1] + b2_hff_yP[1][1];
+  const float P23 = -K2 * b2_hff_yP[0][2] + b2_hff_yP[1][2];
+  const float P31 = -K3 * b2_hff_yP[0][0] + b2_hff_yP[2][0];
+  const float P32 = -K3 * b2_hff_yP[0][1] + b2_hff_yP[2][1];
+  const float P33 = -K3 * b2_hff_yP[0][2] + b2_hff_yP[2][2];
+
+  b2_hff_yP[0][0] = P11;
+  b2_hff_yP[0][1] = P12;
+  b2_hff_yP[0][2] = P13;
+  b2_hff_yP[1][0] = P21;
+  b2_hff_yP[1][1] = P22;
+  b2_hff_yP[1][2] = P23;
+  b2_hff_yP[2][0] = P31;
+  b2_hff_yP[2][1] = P32;
+  b2_hff_yP[2][2] = P33;
+
+}
+
+
+
+
+/*
+  H = [0 1 0];
+  R = 0.1;
+  // state residual
+  yd = vx - H * Xm;
+  // covariance residual
+  S = H*Pm*H' + R;
+  // kalman gain
+  K = Pm*H'*inv(S);
+  // update state
+  Xp = Xm + K*yd;
+  // update covariance
+  Pp = Pm - K*H*Pm;
+*/
+void b2_hff_update_v(float xspeed, float yspeed) {
+    b2_hff_update_xdot(xspeed);
+    b2_hff_update_ydot(yspeed);
+}
+
+static inline void b2_hff_update_xdot(float v) {
+  const float yd = v - b2_hff_xdot;
+  const float S = b2_hff_xP[1][1] + R;
+  const float K1 = b2_hff_xP[0][1] * 1/S;
+  const float K2 = b2_hff_xP[1][1] * 1/S;
+  const float K3 = b2_hff_xP[2][1] * 1/S;
+
+  b2_hff_x     = b2_hff_x     + K1 * yd;
+  b2_hff_xdot  = b2_hff_xdot  + K2 * yd;
+  b2_hff_xbias = b2_hff_xbias + K3 * yd;
+
+  const float P11 = -K1 * b2_hff_xP[1][0] + b2_hff_xP[0][0];
+  const float P12 = -K1 * b2_hff_xP[1][1] + b2_hff_xP[0][1];
+  const float P13 = -K1 * b2_hff_xP[1][2] + b2_hff_xP[0][2];
+  const float P21 = (1. - K2) * b2_hff_xP[1][0];
+  const float P22 = (1. - K2) * b2_hff_xP[1][1];
+  const float P23 = (1. - K2) * b2_hff_xP[1][2];
+  const float P31 = -K3 * b2_hff_xP[1][0] + b2_hff_xP[2][0];
+  const float P32 = -K3 * b2_hff_xP[1][1] + b2_hff_xP[2][1];
+  const float P33 = -K3 * b2_hff_xP[1][2] + b2_hff_xP[2][2];
+
+  b2_hff_xP[0][0] = P11;
+  b2_hff_xP[0][1] = P12;
+  b2_hff_xP[0][2] = P13;
+  b2_hff_xP[1][0] = P21;
+  b2_hff_xP[1][1] = P22;
+  b2_hff_xP[1][2] = P23;
+  b2_hff_xP[2][0] = P31;
+  b2_hff_xP[2][1] = P32;
+  b2_hff_xP[2][2] = P33;
+
+}
+
+static inline void b2_hff_update_ydot(float v) {
+  const float yd = v - b2_hff_ydot;
+  const float S = b2_hff_yP[1][1] + R;
+  const float K1 = b2_hff_yP[0][1] * 1/S;
+  const float K2 = b2_hff_yP[1][1] * 1/S;
+  const float K3 = b2_hff_yP[2][1] * 1/S;
+
+  b2_hff_y     = b2_hff_y     + K1 * yd;
+  b2_hff_ydot  = b2_hff_ydot  + K2 * yd;
+  b2_hff_ybias = b2_hff_ybias + K3 * yd;
+
+  const float P11 = -K1 * b2_hff_yP[1][0] + b2_hff_yP[0][0];
+  const float P12 = -K1 * b2_hff_yP[1][1] + b2_hff_yP[0][1];
+  const float P13 = -K1 * b2_hff_yP[1][2] + b2_hff_yP[0][2];
+  const float P21 = (1. - K2) * b2_hff_yP[1][0];
+  const float P22 = (1. - K2) * b2_hff_yP[1][1];
+  const float P23 = (1. - K2) * b2_hff_yP[1][2];
+  const float P31 = -K3 * b2_hff_yP[1][0] + b2_hff_yP[2][0];
+  const float P32 = -K3 * b2_hff_yP[1][1] + b2_hff_yP[2][1];
+  const float P33 = -K3 * b2_hff_yP[1][2] + b2_hff_yP[2][2];
+
+  b2_hff_yP[0][0] = P11;
+  b2_hff_yP[0][1] = P12;
+  b2_hff_yP[0][2] = P13;
+  b2_hff_yP[1][0] = P21;
+  b2_hff_yP[1][1] = P22;
+  b2_hff_yP[1][2] = P23;
+  b2_hff_yP[2][0] = P31;
+  b2_hff_yP[2][1] = P32;
+  b2_hff_yP[2][2] = P33;
+
+}
+
+
diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h
new file mode 100644
index 0000000000..bb3f9979b4
--- /dev/null
+++ b/sw/airborne/booz/ins/booz2_hf_float.h
@@ -0,0 +1,54 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef BOOZ2_HF_FLOAT_H
+#define BOOZ2_HF_FLOAT_H
+
+#define B2_HFF_STATE_SIZE 3
+
+#define B2_HFF_UPDATE_SPEED
+#define B2_HFF_UPDATE_POS
+
+extern float b2_hff_x;
+extern float b2_hff_xbias;
+extern float b2_hff_xdot;
+extern float b2_hff_xdotdot;
+
+extern float b2_hff_y;
+extern float b2_hff_ybias;
+extern float b2_hff_ydot;
+extern float b2_hff_ydotdot;
+
+extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
+
+extern float b2_hff_x_meas;
+extern float b2_hff_y_meas;
+
+extern void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias);
+extern void b2_hff_propagate(float xaccel, float yaccel);
+extern void b2_hff_update_pos(float xpos, float ypos);
+extern void b2_hff_update_v(float xspeed, float yspeed);
+
+#endif /* BOOZ2_HF_FLOAT_H */
+
diff --git a/sw/airborne/math/pprz_algebra_float.h b/sw/airborne/math/pprz_algebra_float.h
index 5325574f28..2b4a9fd95b 100644
--- a/sw/airborne/math/pprz_algebra_float.h
+++ b/sw/airborne/math/pprz_algebra_float.h
@@ -73,6 +73,9 @@ struct FloatRates {
   }
 
 
+/*
+ * Dimension 3 Vectors
+ */
 
 #define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)	\
 
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 8bff44719d..c47508251e 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -177,6 +177,11 @@ struct Int64Vect3 {
   (_o).y = ((_i).y << (_l)); \
 }
 
+#define INT32_VECT2_SCALE_2(_a, _b, _num, _den) {   \
+  (_a).x = ((_b).x * (_num)) / (_den);        \
+  (_a).y = ((_b).y * (_num)) / (_den);        \
+}
+
 /*
  * Dimension 3 Vectors
  */
@@ -288,8 +293,8 @@ struct Int64Vect3 {
   }
 
 
-/* 
- * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html 
+/*
+ * http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html
  */
 #define INT32_RMAT_OF_QUAT(_rm, _q) {					    \
     const int32_t qx2  = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \