diff --git a/conf/messages.xml b/conf/messages.xml
index 872611d9b1..f274eb8605 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -812,6 +812,9 @@
    <field name="refdot_p" type="int32"/>
    <field name="refdot_q" type="int32"/>
    <field name="refdot_r" type="int32"/>
+   <field name="sumerr_p" type="int32"/>
+   <field name="sumerr_q" type="int32"/>
+   <field name="sumerr_r" type="int32"/>
    <field name="ff_p" type="int32"/>
    <field name="ff_q" type="int32"/>
    <field name="ff_r" type="int32"/>
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 80ecdba9a2..294e8a6c6f 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -172,12 +172,15 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
                                   &booz_stabilization_rate_sp.p,        \
                                   &booz_stabilization_rate_sp.q,        \
                                   &booz_stabilization_rate_sp.r,        \
-                                  &booz_stabilization_rate_ref.p,      \
+                                  &booz_stabilization_rate_ref.p,       \
                                   &booz_stabilization_rate_ref.q,       \
                                   &booz_stabilization_rate_ref.r,       \
-                                  &booz_stabilization_rate_refdot.p,       \
-                                  &booz_stabilization_rate_refdot.q,       \
-                                  &booz_stabilization_rate_refdot.r,       \
+                                  &booz_stabilization_rate_refdot.p,    \
+                                  &booz_stabilization_rate_refdot.q,    \
+                                  &booz_stabilization_rate_refdot.r,    \
+                                  &booz_stabilization_rate_sum_err.p,    \
+                                  &booz_stabilization_rate_sum_err.q,    \
+                                  &booz_stabilization_rate_sum_err.r,    \
                                   &booz_stabilization_rate_ff_cmd.p,    \
                                   &booz_stabilization_rate_ff_cmd.q,    \
                                   &booz_stabilization_rate_ff_cmd.r,    \
diff --git a/sw/airborne/booz/guidance/booz2_guidance_h.c b/sw/airborne/booz/guidance/booz2_guidance_h.c
index 1efa72fa6f..37d155fdc3 100644
--- a/sw/airborne/booz/guidance/booz2_guidance_h.c
+++ b/sw/airborne/booz/guidance/booz2_guidance_h.c
@@ -171,7 +171,7 @@ void booz2_guidance_h_run(bool_t  in_flight) {
   switch ( booz2_guidance_h_mode ) {
 
   case BOOZ2_GUIDANCE_H_MODE_RATE:
-    booz_stabilization_rate_run();
+    booz_stabilization_rate_run(in_flight);
     break;
 
   case BOOZ2_GUIDANCE_H_MODE_ATTITUDE:
diff --git a/sw/airborne/booz/stabilization/booz_stabilization_rate.c b/sw/airborne/booz/stabilization/booz_stabilization_rate.c
index e12dfee12f..25a2b8430f 100644
--- a/sw/airborne/booz/stabilization/booz_stabilization_rate.c
+++ b/sw/airborne/booz/stabilization/booz_stabilization_rate.c
@@ -34,6 +34,8 @@
 #define REF_DOT_FRAC 11
 #define REF_FRAC  16
 
+#define MAX_SUM_ERR 4000000
+
 #ifndef BOOZ_STABILIZATION_RATE_DDGAIN_P
 #define BOOZ_STABILIZATION_RATE_DDGAIN_P 0
 #endif
@@ -43,15 +45,29 @@
 #ifndef BOOZ_STABILIZATION_RATE_DDGAIN_R
 #define BOOZ_STABILIZATION_RATE_DDGAIN_R 0
 #endif
-#ifndef BOOZ_STABILIZATION_RATE_TAU
-#define BOOZ_STABILIZATION_RATE_TAU 1
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_P
+#define BOOZ_STABILIZATION_RATE_IGAIN_P -64
 #endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_Q
+#define BOOZ_STABILIZATION_RATE_IGAIN_Q -64
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_IGAIN_R
+#define BOOZ_STABILIZATION_RATE_IGAIN_R -32
+#endif
+#ifndef BOOZ_STABILIZATION_RATE_TAU
+#define BOOZ_STABILIZATION_RATE_TAU 4
+#endif
+
+#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))
+#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
 
 struct Int32Rates booz_stabilization_rate_sp;
 struct Int32Rates booz_stabilization_rate_gain;
+struct Int32Rates booz_stabilization_rate_igain;
 struct Int32Rates booz_stabilization_rate_ddgain;
 struct Int32Rates booz_stabilization_rate_ref;
 struct Int32Rates booz_stabilization_rate_refdot;
+struct Int32Rates booz_stabilization_rate_sum_err;
 
 struct Int32Rates booz_stabilization_rate_fb_cmd;
 struct Int32Rates booz_stabilization_rate_ff_cmd;
@@ -64,7 +80,10 @@ void booz_stabilization_rate_init(void) {
                BOOZ_STABILIZATION_RATE_GAIN_P,
                BOOZ_STABILIZATION_RATE_GAIN_Q,
                BOOZ_STABILIZATION_RATE_GAIN_R);
-
+  RATES_ASSIGN(booz_stabilization_rate_igain,
+               BOOZ_STABILIZATION_RATE_IGAIN_P,
+               BOOZ_STABILIZATION_RATE_IGAIN_Q,
+               BOOZ_STABILIZATION_RATE_IGAIN_R);
   RATES_ASSIGN(booz_stabilization_rate_ddgain,
                BOOZ_STABILIZATION_RATE_DDGAIN_P,
                BOOZ_STABILIZATION_RATE_DDGAIN_Q,
@@ -72,6 +91,7 @@ void booz_stabilization_rate_init(void) {
 
   INT_RATES_ZERO(booz_stabilization_rate_ref);
   INT_RATES_ZERO(booz_stabilization_rate_refdot);
+  INT_RATES_ZERO(booz_stabilization_rate_sum_err);
 }
 
 
@@ -86,9 +106,10 @@ void booz_stabilization_rate_read_rc( void ) {
 
 void booz_stabilization_rate_enter(void) {
   RATES_COPY(booz_stabilization_rate_ref, booz_stabilization_rate_sp);
+  INT_RATES_ZERO(booz_stabilization_rate_sum_err);
 }
 
-void booz_stabilization_rate_run(void) {
+void booz_stabilization_rate_run(bool_t in_flight) {
 
   /* reference */
   struct Int32Rates _r;
@@ -107,7 +128,28 @@ void booz_stabilization_rate_run(void) {
   /* compute feed-back command */
   struct Int32Rates _error;
   RATES_DIFF(_error, booz_ahrs.body_rate, booz_stabilization_rate_sp);
-  RATES_EWMULT_RSHIFT(booz_stabilization_rate_fb_cmd, booz_stabilization_rate_gain, _error, 16);
+  if (in_flight) {
+    /* update integrator */
+    RATES_ADD(booz_stabilization_rate_sum_err, _error);
+    RATES_BOUND_CUBE(booz_stabilization_rate_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
+  }
+  else {
+    INT_RATES_ZERO(booz_stabilization_rate_sum_err);
+  }
+
+  /* PI */
+  booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_gain.p * _error.p +
+    OFFSET_AND_ROUND2((booz_stabilization_rate_igain.p  * booz_stabilization_rate_sum_err.p), 10);
+
+  booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_gain.q * _error.q +
+    OFFSET_AND_ROUND2((booz_stabilization_rate_igain.q  * booz_stabilization_rate_sum_err.q), 10);
+
+  booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_gain.r * _error.r +
+    OFFSET_AND_ROUND2((booz_stabilization_rate_igain.r  * booz_stabilization_rate_sum_err.r), 10);
+
+  booz_stabilization_rate_fb_cmd.p = booz_stabilization_rate_fb_cmd.p >> 16;
+  booz_stabilization_rate_fb_cmd.q = booz_stabilization_rate_fb_cmd.q >> 16;
+  booz_stabilization_rate_fb_cmd.r = booz_stabilization_rate_fb_cmd.r >> 16;
 
   /* sum to final command */
   booz_stabilization_cmd[COMMAND_ROLL]  = booz_stabilization_rate_ff_cmd.p + booz_stabilization_rate_fb_cmd.p;
diff --git a/sw/airborne/booz/stabilization/booz_stabilization_rate.h b/sw/airborne/booz/stabilization/booz_stabilization_rate.h
index 4d787cb257..f20fb43060 100644
--- a/sw/airborne/booz/stabilization/booz_stabilization_rate.h
+++ b/sw/airborne/booz/stabilization/booz_stabilization_rate.h
@@ -28,14 +28,16 @@
 
 extern void booz_stabilization_rate_init(void);
 extern void booz_stabilization_rate_read_rc(void);
-extern void booz_stabilization_rate_run(void);
+extern void booz_stabilization_rate_run(bool_t in_flight);
 extern void booz_stabilization_rate_enter(void);
 
 extern struct Int32Rates booz_stabilization_rate_sp;
 extern struct Int32Rates booz_stabilization_rate_gain;
+extern struct Int32Rates booz_stabilization_rate_igain;
 extern struct Int32Rates booz_stabilization_rate_ddgain;
 extern struct Int32Rates booz_stabilization_rate_ref;
 extern struct Int32Rates booz_stabilization_rate_refdot;
+extern struct Int32Rates booz_stabilization_rate_sum_err;
 
 extern struct Int32Rates booz_stabilization_rate_fb_cmd;
 extern struct Int32Rates booz_stabilization_rate_ff_cmd;