diff --git a/conf/airframes/TU_Delft/MicrojetCDW.xml b/conf/airframes/TU_Delft/MicrojetCDW.xml
index 742bf02ce6..3e7c5c5230 100644
--- a/conf/airframes/TU_Delft/MicrojetCDW.xml
+++ b/conf/airframes/TU_Delft/MicrojetCDW.xml
@@ -184,9 +184,11 @@
 
   <firmware name="fixedwing">
     <target name="ap" 			board="tiny_1.1">
-      <param name="PERIODIC_FREQUENCY" value="960" />
-      <param name="AHRS_ALIGNER_LED" value="1" />
-      <param name="CPU_LED" value="2" />
+      <configure name="PERIODIC_FREQUENCY" value="960" /> <!-- IMU FREQ -->
+      <configure name="AHRS_PROPAGATE_FREQUENCY" value="120" />
+      <configure name="AHRS_CORRECT_FREQUENCY" value="60" />
+      <configure name="AHRS_ALIGNER_LED" value="1" />
+      <configure name="CPU_LED" value="2" />
     </target>
     <target name="sim" 			board="pc" />
 
diff --git a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
index 606a43bec4..a4ca390a27 100644
--- a/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
+++ b/conf/autopilot/subsystems/fixedwing/attitude_dcm.makefile
@@ -7,14 +7,27 @@ ifeq ($(ARCH), lpc21)
 
 ap.CFLAGS += -DUSE_AHRS
 
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
-ap.srcs += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_aligner.c
+ap.srcs   += $(SRC_SUBSYSTEMS)/ahrs/ahrs_float_dcm.c
 ap.CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
 
-  ifdef CPU_LED
-    ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
-  endif
+ifdef CPU_LED
+  ap.CFLAGS += -DAHRS_CPU_LED=$(CPU_LED)
+endif
+
+ifdef AHRS_PROPAGATE_FREQUENCY
+else
+  AHRS_PROPAGATE_FREQUENCY = 60
+endif
+
+ifdef AHRS_CORRECT_FREQUENCY
+else
+  AHRS_CORRECT_FREQUENCY = 60
+endif
+
+ap.CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=$(AHRS_PROPAGATE_FREQUENCY)
+ap.CFLAGS += -DAHRS_CORRECT_FREQUENCY=$(AHRS_CORRECT_FREQUENCY)
 
 endif
 
diff --git a/conf/settings/tuning_ins.xml b/conf/settings/tuning_ins.xml
index bc080d1c9b..454dd326a1 100644
--- a/conf/settings/tuning_ins.xml
+++ b/conf/settings/tuning_ins.xml
@@ -32,6 +32,7 @@
     <dl_settings NAME="control">
 
       <dl_settings NAME="ins">
+        <dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="imu_overrun" module="subsystems/imu" />
         <dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
         <dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
       </dl_settings>
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
index 456c7bff65..9d056e3adf 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.c
@@ -23,6 +23,7 @@
 
 #include "subsystems/imu.h"
 
+int imu_overrun_error = 0;
 volatile uint8_t imu_ssp_status;
 static void SSP_ISR(void) __attribute__((naked));
 #if 0
diff --git a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
index d54b22452a..76e7804d31 100644
--- a/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
+++ b/sw/airborne/arch/lpc21/subsystems/imu/imu_b2_arch.h
@@ -45,6 +45,8 @@
 #define IMU_SSP_STA_BUSY_MAX1168   1
 #define IMU_SSP_STA_BUSY_MS2100    2
 extern volatile uint8_t imu_ssp_status;
+extern int imu_overrun_error;
+
 
 
 
diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/sw/airborne/firmwares/fixedwing/main_ap.c
index 3eaa229848..d80ccec89a 100644
--- a/sw/airborne/firmwares/fixedwing/main_ap.c
+++ b/sw/airborne/firmwares/fixedwing/main_ap.c
@@ -381,15 +381,7 @@ void periodic_task_ap( void ) {
 
 #ifdef USE_IMU
   // Run at PERIODIC_FREQUENCY (60Hz if not defined)
-
-#ifdef AHRS_CPU_LED
-    LED_ON(AHRS_CPU_LED);
-#endif
-
   imu_periodic();
-#ifdef AHRS_CPU_LED
-    LED_OFF(AHRS_CPU_LED);
-#endif
 
 #endif // USE_IMU
 
@@ -669,42 +661,67 @@ void event_task_ap( void ) {
 
 #ifdef USE_AHRS
 static inline void on_gyro_accel_event( void ) {
-  static uint8_t _reduced_rate = 0;
+  static uint8_t _reduced_propagation_rate = 0;
+  static uint8_t _reduced_correction_rate = 0;
+  static struct Int32Vect3 acc_avg, gyr_avg;
 
 #ifdef AHRS_CPU_LED
     LED_ON(AHRS_CPU_LED);
 #endif
 
-  ImuScaleGyro(imu);
-  ImuScaleAccel(imu);
 
-  
-  if (ahrs.status == AHRS_UNINIT) {
-    ahrs_aligner_run();
-    if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
-      ahrs_align();
+
+  gyr_avg.x += imu.gyro_unscaled.p;
+  gyr_avg.y += imu.gyro_unscaled.q;
+  gyr_avg.z += imu.gyro_unscaled.r;
+  INT32_VECT3_ADD(acc_avg, imu.accel_unscaled);
+
+  _reduced_propagation_rate++;
+  if (_reduced_propagation_rate < (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY))
+  {
   }
-  else {
+  else
+  {
+    _reduced_propagation_rate = 0;
+
+    INT32_VECT3_SDIV(gyr_avg, gyr_avg, (PERIODIC_FREQUENCY / AHRS_PROPAGATE_FREQUENCY) );
+    imu.gyro_unscaled.p = gyr_avg.x;
+    imu.gyro_unscaled.q = gyr_avg.y;
+    imu.gyro_unscaled.r = gyr_avg.z; 
+    INT_VECT3_ZERO(gyr_avg);
+
+    ImuScaleGyro(imu);
+
+    
+    if (ahrs.status == AHRS_UNINIT) {
+      ImuScaleAccel(imu);
+      ahrs_aligner_run();
+      if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+        ahrs_align();
+    }
+    else {
 
-    //if (_reduced_rate % 4)
-    {
       ahrs_propagate();
+
+      _reduced_correction_rate++;
+      if (_reduced_correction_rate >= (AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY))
+      {
+        _reduced_correction_rate = 0;
+        INT32_VECT3_SDIV(acc_avg, acc_avg, (PERIODIC_FREQUENCY / AHRS_CORRECT_FREQUENCY) );
+        INT32_VECT3_COPY(imu.accel_unscaled, acc_avg);
+        INT_VECT3_ZERO(acc_avg);
+        ImuScaleAccel(imu);
+        ahrs_update_accel();
+        ahrs_update_fw_estimator();
+      }
+
     }
-
-
-    _reduced_rate++;
-    if (_reduced_rate >= (PERIODIC_FREQUENCY / 60))
-    {
-      _reduced_rate = 0;
-      ahrs_update_accel();
-      ahrs_update_fw_estimator();
-    }
-
   }
   
 #ifdef AHRS_CPU_LED
     LED_OFF(AHRS_CPU_LED);
 #endif
+
 }
 
 static inline void on_mag_event(void) {
diff --git a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
index 819019e74f..b6a7198e55 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
@@ -50,10 +50,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
 // Positive yaw : clockwise
 
 // DCM Working variables
-#ifndef PERIODIC_FREQUENCY
-#define PERIODIC_FREQUENCY 60
-#endif
-float G_Dt=1. / ((float) PERIODIC_FREQUENCY );
+float G_Dt=1. / ((float) AHRS_PROPAGATE_FREQUENCY );
 
 struct FloatVect3 accel_float = {0,0,0};