diff --git a/sw/airborne/booz/booz2_main.c b/sw/airborne/booz/booz2_main.c
index 744be3ade5..fae7f4ef38 100644
--- a/sw/airborne/booz/booz2_main.c
+++ b/sw/airborne/booz/booz2_main.c
@@ -99,6 +99,7 @@ STATIC_INLINE void booz2_main_init( void ) {
 
   booz_ahrs_aligner_init();
   booz_ahrs_init();
+  booz_ahrs_init_accel_rb();
 
   booz_ins_init();
 
@@ -173,6 +174,8 @@ 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 d5599fcf39..7ca9f71a16 100644
--- a/sw/airborne/booz/booz_ahrs.c
+++ b/sw/airborne/booz/booz_ahrs.c
@@ -1,4 +1,74 @@
+/*
+ * $Id$
+ *
+ * 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 "booz_ahrs.h"
+#include "booz_imu.h"
 
 struct BoozAhrs booz_ahrs;
 struct BoozAhrsFloat booz_ahrs_float;
+
+#define RB_MAXN 64
+#define RB_TOP RB_MAXN - 1
+
+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_TOP ? 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++;
+    }
+}
+
+/* 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 = 0; i < n; i++) {
+        j = (rb_r + i) < RB_TOP ? rb_r + i : rb_r + i - RB_TOP;
+        VECT3_ADD(sum, accel_buf[j]);
+    }
+    VECT3_SDIV(booz_ahrs_accel_mean, sum, n);
+}
+
diff --git a/sw/airborne/booz/booz_ahrs.h b/sw/airborne/booz/booz_ahrs.h
index b1835a5f53..5a4cbb6d0f 100644
--- a/sw/airborne/booz/booz_ahrs.h
+++ b/sw/airborne/booz/booz_ahrs.h
@@ -1,6 +1,6 @@
 /*
  * $Id$
- *  
+ *
  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
  *
  * This file is part of paparazzi.
@@ -18,7 +18,7 @@
  * 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. 
+ * Boston, MA 02111-1307, USA.
  */
 
 #ifndef BOOZ_AHRS_H
@@ -33,15 +33,15 @@
 #define BOOZ_AHRS_RUNNING 1
 
 struct BoozAhrs {
-  struct Int32Eulers ltp_to_body_euler;
-  struct Int32Quat   ltp_to_body_quat;
-  struct Int32RMat   ltp_to_body_rmat;
-  struct Int32Eulers ltp_to_imu_euler;
-  struct Int32Quat   ltp_to_imu_quat;
-  struct Int32RMat   ltp_to_imu_rmat;
-  struct Int32Rates  imu_rate;
-  struct Int32Rates  body_rate;  
-  uint8_t status;
+    struct Int32Eulers ltp_to_body_euler;
+    struct Int32Quat ltp_to_body_quat;
+    struct Int32RMat ltp_to_body_rmat;
+    struct Int32Eulers ltp_to_imu_euler;
+    struct Int32Quat ltp_to_imu_quat;
+    struct Int32RMat ltp_to_imu_rmat;
+    struct Int32Rates imu_rate;
+    struct Int32Rates body_rate;
+    uint8_t status;
 };
 
 struct BoozAhrsFloat {
@@ -53,6 +53,7 @@ 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); \
@@ -69,5 +70,8 @@ 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 */