diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index 3239dd5044..5552a61834 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -148,18 +148,17 @@ void booz_ins_propagate() {
 #endif /* USE_VFF */
 
 #ifdef USE_HFF
-  if (booz_ahrs.status == BOOZ_AHRS_RUNNING ) {
-	/* propagate horizontal filter */
-	b2_hff_propagate();
-	if ( booz_ins_ltp_initialised ) {
-	  /* update ins state from horizontal filter */
-	  booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
-	  booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
-	  booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
-	  booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
-	  booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
-	  booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
-	}
+  b2_hff_store_accel();
+  /* propagate horizontal filter */
+  b2_hff_propagate();
+  if ( booz_ins_ltp_initialised ) {
+    /* update ins state from horizontal filter */
+    booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+    booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+    booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+    booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+    booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
+    booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
   }
 #else
   booz_ins_ltp_accel.x = accel_ltp.x;
diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 1aa46d4ca3..45254336c1 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -123,7 +123,6 @@ STATIC_INLINE void booz2_main_init( void ) {
 
   booz_ahrs_aligner_init();
   booz_ahrs_init();
-  booz_ahrs_init_accel_rb();
 
   booz_ins_init();
 
@@ -249,8 +248,6 @@ static inline void on_gyro_accel_event( void ) {
   BoozImuScaleGyro();
   BoozImuScaleAccel();
 
-  booz_ahrs_store_accel();
-
   if (booz_ahrs.status == BOOZ_AHRS_UNINIT) {
     booz_ahrs_aligner_run();
     if (booz_ahrs_aligner.status == BOOZ_AHRS_ALIGNER_LOCKED)
diff --git a/sw/airborne/booz/booz_ahrs.c b/sw/airborne/booz/booz_ahrs.c
index 1dca75a885..7083200be3 100644
--- a/sw/airborne/booz/booz_ahrs.c
+++ b/sw/airborne/booz/booz_ahrs.c
@@ -26,52 +26,3 @@
 
 struct BoozAhrs booz_ahrs;
 struct BoozAhrsFloat booz_ahrs_float;
-
-#define RB_MAXN 64
-
-struct Int32Vect3 accel_buf[RB_MAXN];
-struct Int32Vect3 booz_ahrs_accel_mean;
-
-uint8_t rb_r; /* pos to read from, oldest measurement */
-uint8_t rb_w; /* pos to write to */
-uint8_t rb_n; /* number of elements in rb */
-
-void booz_ahrs_init_accel_rb(void) {
-  rb_r = 0;
-  rb_w = 0;
-  rb_n = 0;
-}
-
-void booz_ahrs_store_accel(void) {
-  VECT3_COPY(accel_buf[rb_w], booz_imu.accel);
-  rb_w = (rb_w + 1) < RB_MAXN ? (rb_w + 1) : 0;
-
-  /* once the buffer is full it always has the last RB_MAXN accel measurements */
-  if (rb_n < RB_MAXN) {
-	rb_n++;
-  } else {
-	rb_r = (rb_r + 1) < RB_MAXN ? (rb_r + 1) : 0;
-  }
-}
-
-/* compute the mean of the last n accel measurements */
-void booz_ahrs_compute_accel_mean(uint8_t n) {
-  struct Int32Vect3 sum;
-  int i, j;
-
-  INT_VECT3_ZERO(sum);
-
-  if (n > rb_n) {
-	n = rb_n;
-  }
-  for (i = 1; i <= n; i++) {
-	j = (rb_w - i) > 0 ? rb_w - i : rb_w - i + RB_MAXN;
-	VECT3_ADD(sum, accel_buf[j]);
-  }
-  if (n > 1) {
-	VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
-  } else {
-	VECT3_COPY(booz_ahrs_accel_mean, sum);
-  }
-}
-
diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h
index 3299d7b0ba..d309781aba 100644
--- a/sw/airborne/booz/booz_ahrs.h
+++ b/sw/airborne/booz/booz_ahrs.h
@@ -57,7 +57,6 @@ struct BoozAhrsFloat {
 
 extern struct BoozAhrs booz_ahrs;
 extern struct BoozAhrsFloat booz_ahrs_float;
-extern struct Int32Vect3 booz_ahrs_accel_mean;
 
 #define BOOZ_AHRS_FLOAT_OF_INT32() {					\
     QUAT_FLOAT_OF_BFP(booz_ahrs_float.ltp_to_body_quat, booz_ahrs.ltp_to_body_quat); \
@@ -74,8 +73,4 @@ extern void booz_ahrs_update(void);
 extern void booz_ahrs_update_accel(void);
 extern void booz_ahrs_update_mag(void);
 
-extern void booz_ahrs_init_accel_rb(void);
-extern void booz_ahrs_store_accel(void);
-extern void booz_ahrs_compute_accel_mean(uint8_t n);
-
 #endif /* BOOZ_AHRS_H */
diff --git a/sw/airborne/booz/ins/booz2_hf_float.c b/sw/airborne/booz/ins/booz2_hf_float.c
index be12c5606f..1f4a5829ce 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.c
+++ b/sw/airborne/booz/ins/booz2_hf_float.c
@@ -82,6 +82,53 @@ float b2_hff_y_meas;
 int b2_hff_ps_counter;
 
 
+/*
+ * accel(in body frame) buffer
+ */
+#define ACC_RB_MAXN 64
+struct AccBuf {
+  struct Int32Vect3 buf[ACC_RB_MAXN];
+  uint8_t r; /* pos to read from, oldest measurement */
+  uint8_t w; /* pos to write to */
+  uint8_t n; /* number of elements in rb */
+  uint8_t size;
+};
+struct AccBuf acc_body;
+struct Int32Vect3 acc_mean;
+
+void b2_hff_store_accel(void) {
+  VECT3_COPY(acc_body.buf[acc_body.w], booz_imu.accel);
+  acc_body.w = (acc_body.w + 1) < acc_body.size ? (acc_body.w + 1) : 0;
+
+  /* once the buffer is full it always has the last acc_body.size accel measurements */
+  if (acc_body.n < acc_body.size) {
+	acc_body.n++;
+  } else {
+	acc_body.r = (acc_body.r + 1) < acc_body.size ? (acc_body.r + 1) : 0;
+  }
+}
+
+/* compute the mean of the last n accel measurements */
+static inline void b2_hff_compute_accel_mean(uint8_t n) {
+  struct Int32Vect3 sum;
+  int i, j;
+
+  INT_VECT3_ZERO(sum);
+
+  if (n > acc_body.n) {
+	n = acc_body.n;
+  }
+  for (i = 1; i <= n; i++) {
+	j = (acc_body.w - i) > 0 ? acc_body.w - i : acc_body.w - i + acc_body.size;
+	VECT3_ADD(sum, acc_body.buf[j]);
+  }
+  if (n > 1) {
+	VECT3_SDIV(acc_mean, sum, n);
+  } else {
+	VECT3_COPY(acc_mean, sum);
+  }
+}
+
 /*
  * For GPS lag compensation
  *
@@ -107,7 +154,7 @@ int b2_hff_ps_counter;
  */
 #define MAX_PP_STEPS 6
 
-/* variables for accel buffer */
+/* variables for mean accel buffer */
 #define ACC_BUF_MAXN (GPS_LAG_N+10)
 #define INC_ACC_IDX(idx) {	idx = (idx + 1) < ACC_BUF_MAXN ? (idx + 1) : 0;	}
 
@@ -177,7 +224,13 @@ void b2_hff_init(float init_x, float init_xdot, float init_y, float init_ydot) {
   Rspeed = R_SPEED;
   b2_hff_init_x(init_x, init_xdot);
   b2_hff_init_y(init_y, init_ydot);
+  /* init buffer for mean accel calculation */
+  acc_body.r = 0;
+  acc_body.w = 0;
+  acc_body.n = 0;
+  acc_body.size = ACC_RB_MAXN;
 #ifdef GPS_LAG
+  /* init buffer for past mean accel values */
   acc_buf_r = 0;
   acc_buf_w = 0;
   acc_buf_n = 0;
@@ -229,7 +282,7 @@ static inline void b2_hff_init_y(float init_y, float init_ydot) {
 }
 
 #ifdef GPS_LAG
-void b2_hff_store_accel(float x, float y) {
+static inline void b2_hff_store_accel_ltp(float x, float y) {
   past_accel[acc_buf_w].x = x;
   past_accel[acc_buf_w].y = y;
   INC_ACC_IDX(acc_buf_w);
@@ -365,15 +418,15 @@ void b2_hff_propagate(void) {
 
     if (gps_lost_counter < GPS_LOST_LIMIT) {
       /* compute float ltp mean acceleration */
-      booz_ahrs_compute_accel_mean(HFF_PRESCALER);
+      b2_hff_compute_accel_mean(HFF_PRESCALER);
       struct Int32Vect3 mean_accel_body;
-      INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, booz_ahrs_accel_mean);
+      INT32_RMAT_TRANSP_VMULT(mean_accel_body, booz_imu.body_to_imu_rmat, acc_mean);
       struct Int32Vect3 mean_accel_ltp;
       INT32_RMAT_TRANSP_VMULT(mean_accel_ltp, booz_ahrs.ltp_to_body_rmat, mean_accel_body);
       b2_hff_xdd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.x);
       b2_hff_ydd_meas = ACCEL_FLOAT_OF_BFP(mean_accel_ltp.y);
 #ifdef GPS_LAG
-      b2_hff_store_accel(b2_hff_xdd_meas, b2_hff_ydd_meas);
+      b2_hff_store_accel_ltp(b2_hff_xdd_meas, b2_hff_ydd_meas);
 #endif
 
       /*
diff --git a/sw/airborne/booz/ins/booz2_hf_float.h b/sw/airborne/booz/ins/booz2_hf_float.h
index 0c366b779c..066000fc14 100644
--- a/sw/airborne/booz/ins/booz2_hf_float.h
+++ b/sw/airborne/booz/ins/booz2_hf_float.h
@@ -70,9 +70,8 @@ extern void b2_hff_update_pos(float xpos, float ypos);
 extern void b2_hff_update_v(float xspeed, float yspeed);
 extern void b2_hff_realign(struct FloatVect2 pos, struct FloatVect2 speed);
 
-#ifdef GPS_LAG
-extern void b2_hff_store_accel(float x, float y);
-#endif
+extern void b2_hff_store_accel(void);
+
 extern struct HfilterFloat *b2_hff_rb_last;
 extern int lag_counter_err;
 extern int save_counter;