diff --git a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
index bea1065a23..74bd47bd05 100644
--- a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
+++ b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml
@@ -100,7 +100,7 @@
     <!-- setpoint limits for attitude stabilization rc flight -->
     <define name="SP_MAX_PHI" value="45" unit="deg"/>
     <define name="SP_MAX_THETA" value="45" unit="deg"/>
-    <define name="SP_MAX_R" value="300" unit="deg/s"/>
+    <define name="SP_MAX_R" value="120" unit="deg/s"/>
     <define name="DEADBAND_A" value="0"/>
     <define name="DEADBAND_E" value="0"/>
     <define name="DEADBAND_R" value="50"/>
@@ -139,6 +139,9 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
+    <!--Maxium yaw rate, to avoid instability-->
+    <define name="MAX_R" value="120.0" unit="deg/s"/>
+
     <!-- second order filter parameters -->
     <define name="FILT_OMEGA" value="50.0"/>
     <define name="FILT_ZETA" value="0.55"/>
diff --git a/conf/settings/control/stabilization_indi.xml b/conf/settings/control/stabilization_indi.xml
index bd73eb2301..4a7e550136 100644
--- a/conf/settings/control/stabilization_indi.xml
+++ b/conf/settings/control/stabilization_indi.xml
@@ -15,7 +15,8 @@
       <dl_setting var="indi.g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
       <dl_setting var="indi.g2" min="0" step="0.01" max="10" module="stabilization/stabilization_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
       <dl_setting var="indi.adaptive" min="0" step="1" max="1" module="stabilization/stabilization_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
-      <dl_setting var="indi.max_rate" min="0" step="0.0001" max="80.0" module="stabilization/stabilization_indi" shortname="max_rate"/>
+      <dl_setting var="indi.max_rate" min="0" step="0.01" max="80.0" module="stabilization/stabilization_indi" shortname="max_rate" param="STABILIZATION_INDI_MAX_RATE" unit="rad/s" alt_unit="deg/s"/>
+      <dl_setting var="indi.attitude_max_yaw_rate" min="0" step="0.01" max="80.0" module="stabilization/stabilization_indi" shortname="max_yaw_rate_attitude" param="STABILIZATION_INDI_MAX_R" unit="rad/s" alt_unit="deg/s"/>
     </dl_settings>
 
   </dl_settings>
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index dc283914c4..6eb4076e90 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -71,6 +71,10 @@
 #warning "Use caution with adaptive indi. See the wiki for more info"
 #endif
 
+#ifndef STABILIZATION_INDI_MAX_R
+#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
+#endif
+
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
 
@@ -83,6 +87,7 @@ static inline void lms_estimation(void);
 #define INDI_EST_SCALE 0.001 //The G values are scaled to avoid numerical problems during the estimation
 struct IndiVariables indi = {
   .max_rate = STABILIZATION_INDI_MAX_RATE,
+  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
 
   .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
   .g2 = STABILIZATION_INDI_G2_R,
@@ -206,28 +211,33 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct I
   struct FloatRates *body_rates = stateGetBodyRates_f();
   stabilization_indi_second_order_filter(&indi.rate, body_rates);
 
+  //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
+  struct FloatRates rates_for_feedback;
+  RATES_COPY(rates_for_feedback, (*body_rates));
+
+  //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
+  //Note that due to the delay, the PD controller can not be as aggressive.
 #if STABILIZATION_INDI_FILTER_ROLL_RATE
-  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
-                             - indi.reference_acceleration.rate_p * indi.rate.x.p;
-#else
-  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
-                             - indi.reference_acceleration.rate_p * body_rates->p;
+  rates_for_feedback.p = indi.rate.x.p;
 #endif
 #if STABILIZATION_INDI_FILTER_PITCH_RATE
-  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
-                             - indi.reference_acceleration.rate_q * indi.rate.x.q;
-#else
-  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
-                             - indi.reference_acceleration.rate_q * body_rates->q;
+  rates_for_feedback.q = indi.rate.x.q;
 #endif
 #if STABILIZATION_INDI_FILTER_YAW_RATE
-  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
-                             - indi.reference_acceleration.rate_r * indi.rate.x.r;
-#else
-  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
-                             - indi.reference_acceleration.rate_r * body_rates->r;
+  rates_for_feedback.r = indi.rate.x.r;
 #endif
 
+  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
+                             - indi.reference_acceleration.rate_p * rates_for_feedback.p;
+
+  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
+                             - indi.reference_acceleration.rate_q * rates_for_feedback.q;
+
+  //This separates the P and D controller and lets you impose a maximum yaw rate.
+  float rate_ref_r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/indi.reference_acceleration.rate_r;
+  BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);
+  indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);
+
   /* Check if we are running the rate controller and overwrite */
   if(rate_control) {
     indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * indi.max_rate - body_rates->p);
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
index c703f50de8..1b211888f5 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
@@ -82,7 +82,8 @@ struct IndiVariables {
   struct ReferenceSystem reference_acceleration;
 
   bool adaptive;             ///< Enable adataptive estimation
-  float max_rate;            ///< Maximum rate in rate control
+  float max_rate;            ///< Maximum rate in rate control in rad/s
+  float attitude_max_yaw_rate; ///< Maximum yaw rate in atttiude control in rad/s
   struct IndiEstimation est; ///< Estimation parameters for adaptive INDI
 };