diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
index 7b95101f01..56651719f4 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
@@ -95,7 +95,41 @@ void ahrs_align(void) {
 
 }
 
+//#define USE_NOISE_CUT 1
+//#define USE_NOISE_FILTER 1
+//#define NOISE_FILTER_GAIN 50
 
+#ifdef USE_NOISE_CUT
+#include "led.h"
+static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
+  struct Int32Rates diff;
+  RATES_DIFF(diff, i1, i2);
+  if (diff.p < -threshold || diff.p > threshold ||
+      diff.q < -threshold || diff.q > threshold ||
+      diff.r < -threshold || diff.r > threshold) {
+    return TRUE;
+  } else {
+    return FALSE;
+  }
+}
+#define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
+
+static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
+  struct Int32Vect3 diff;
+  VECT3_DIFF(diff, i1, i2);
+  if (diff.x < -threshold || diff.x > threshold ||
+      diff.y < -threshold || diff.y > threshold ||
+      diff.z < -threshold || diff.z > threshold) {
+    LED_ON(4);
+    return TRUE;
+  } else {
+    LED_OFF(4);
+    return FALSE;
+  }
+}
+#define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
+
+#endif
 
 /*
  *
@@ -116,9 +150,17 @@ void ahrs_propagate(void) {
   /* unbias gyro             */
   struct Int32Rates uf_rate;
   RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
-  /* low pass rate */
-  RATES_ADD(ahrs.imu_rate, uf_rate);
-  RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
+#ifdef USE_NOISE_CUT
+  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
+  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
+#endif
+    /* low pass rate */
+    RATES_ADD(ahrs.imu_rate, uf_rate);
+    RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
+#ifdef USE_NOISE_CUT
+  }
+  RATES_COPY(last_uf_rate, uf_rate);
+#endif
 
   /* integrate eulers */
   struct Int32Eulers euler_dot;
@@ -152,7 +194,19 @@ void ahrs_propagate(void) {
 
 void ahrs_update_accel(void) {
 
-  get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
+#ifdef USE_NOISE_CUT
+  static struct Int32Vect3 last_accel = { 0, 0, 0 };
+  if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
+#endif
+#ifdef USE_NOISE_FILTER
+    VECT3_SUM_SCALED(imu.accel, imu.accel, last_accel, NOISE_FILTER_GAIN);
+    VECT3_SDIV(imu.accel, imu.accel, NOISE_FILTER_GAIN+1);
+#endif
+    get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi, &ahrs_impl.measurement.theta, imu.accel);
+#ifdef USE_NOISE_CUT
+  }
+  VECT3_COPY(last_accel, imu.accel);
+#endif
 
 }