From e44277d1f90e00a6c69916e744845e213ae5e98b Mon Sep 17 00:00:00 2001
From: Ewoud Smeur <ewoud_smeur@msn.com>
Date: Wed, 12 May 2021 22:41:32 +0200
Subject: [PATCH] Filter rates indi (#2718)

* Changed to use the filter interface

* Cleaning defines from airframe files

* update module doc

* [conf] Fix Nederdrone back wing numbering

* Filter frequency in Hz

* Fix nameing with prefix

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
---
 conf/airframes/BR/bebop_indi.xml              |  1 -
 conf/airframes/BR/bebop_indi_frog.xml         |  1 -
 conf/airframes/BR/ladybird_kit_indi_bart.xml  |  1 -
 .../BR/quadshot_asp21_FutabaPPMonUart1.xml    |  2 +-
 conf/airframes/ENAC/hoops_gen_ap.xml          | 12 +---
 conf/airframes/ENAC/quadrotor/bebop2_fish.xml |  9 +--
 .../ENAC/quadrotor/bebop2_fish_outdoor.xml    |  9 +--
 .../ENAC/quadrotor/crazyflie_2.1.xml          |  6 +-
 conf/airframes/KS/ks_bebop2_stereo.xml        |  8 ---
 conf/airframes/KS/ks_mavtec1.xml              |  2 +-
 conf/airframes/MM/bebop2_lum1_xbee.xml        | 14 ----
 .../OPENUAS/openuas_3dr_iris_plus.xml         |  6 --
 .../openuas_bitcraze_crazyflie_2_1.xml        |  7 +-
 .../OPENUAS/openuas_eachine_trashcan.xml      |  6 --
 .../OPENUAS/openuas_emax_tinyhawk_ii.xml      |  6 --
 .../OPENUAS/openuas_own_jetson_tx220.xml      |  6 --
 .../OPENUAS/openuas_parrot_ardrone_2.xml      |  6 --
 .../OPENUAS/openuas_parrot_bebop.xml          | 14 +---
 .../OPENUAS/openuas_parrot_bebop_2.xml        | 13 +---
 .../openuas_transitionrobotics_quadshot.xml   |  7 +-
 conf/airframes/examples/bebop2_indi.xml       |  6 --
 conf/airframes/examples/bebop2_opticflow.xml  | 18 +----
 .../bebop2_ukf_magnetometer_calibration.xml   |  7 +-
 conf/airframes/examples/quadrotor_navstik.xml |  1 -
 .../quadshot_asp21_FutabaPPMonUart1.xml       |  2 +-
 .../examples/quadshot_asp21_spektrum.xml      |  2 +-
 conf/airframes/examples/trashcan.xml          |  6 --
 conf/airframes/tudelft/ardrone2_OF_hover.xml  |  1 -
 conf/airframes/tudelft/ardrone2_indi.xml      |  1 -
 conf/airframes/tudelft/ardrone2_optitrack.xml |  1 -
 conf/airframes/tudelft/bebop2_indi.xml        |  5 --
 conf/airframes/tudelft/bebop2_no_damping.xml  |  9 +--
 conf/airframes/tudelft/bebop2_opticflow.xml   | 18 +----
 conf/airframes/tudelft/bebop2_optitrack.xml   |  7 +-
 .../tudelft/bebop2_optitrack_visionfront.xml  |  9 +--
 .../tudelft/bebop2_undistort_front.xml        |  8 ---
 conf/airframes/tudelft/bebop_OF_hover.xml     |  1 -
 conf/airframes/tudelft/bebop_flip.xml         |  1 -
 conf/airframes/tudelft/bebop_mavlink.xml      |  1 -
 conf/airframes/tudelft/bebop_opticflow.xml    |  1 -
 .../tudelft/guido_ardrone2_optitrack.xml      |  1 -
 conf/airframes/tudelft/iris_indi.xml          |  6 +-
 conf/airframes/tudelft/mavtec1.xml            |  2 +-
 conf/airframes/tudelft/mavtec5.xml            |  2 +-
 conf/airframes/tudelft/neddrone4.xml          | 29 ++++----
 conf/airframes/tudelft/neddrone5.xml          | 22 +++---
 conf/airframes/tudelft/robird.xml             |  6 +-
 conf/airframes/tudelft/splash_px4.xml         |  5 --
 conf/modules/stabilization_indi_simple.xml    |  7 +-
 conf/userconf/tudelft/conf.xml                |  2 +-
 .../stabilization/stabilization_indi_simple.c | 71 ++++++++++++++-----
 .../stabilization/stabilization_indi_simple.h |  2 +
 52 files changed, 111 insertions(+), 285 deletions(-)

diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml
index f80a52df46..58922bb6c0 100644
--- a/conf/airframes/BR/bebop_indi.xml
+++ b/conf/airframes/BR/bebop_indi.xml
@@ -169,7 +169,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/BR/bebop_indi_frog.xml b/conf/airframes/BR/bebop_indi_frog.xml
index e0b26e2928..3a6a9d0764 100644
--- a/conf/airframes/BR/bebop_indi_frog.xml
+++ b/conf/airframes/BR/bebop_indi_frog.xml
@@ -172,7 +172,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/BR/ladybird_kit_indi_bart.xml b/conf/airframes/BR/ladybird_kit_indi_bart.xml
index 0a6e495c5e..3cf9aca4ca 100644
--- a/conf/airframes/BR/ladybird_kit_indi_bart.xml
+++ b/conf/airframes/BR/ladybird_kit_indi_bart.xml
@@ -140,7 +140,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
diff --git a/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml b/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
index 9376a449e4..abaa7f2bd5 100644
--- a/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
+++ b/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
@@ -233,7 +233,7 @@
     <define name="REF_RATE_R" value="12.0"/>
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
     <define name="ACT_DYN_Q" value="0.03"/>
diff --git a/conf/airframes/ENAC/hoops_gen_ap.xml b/conf/airframes/ENAC/hoops_gen_ap.xml
index c4a33e5b24..8561a83ec6 100644
--- a/conf/airframes/ENAC/hoops_gen_ap.xml
+++ b/conf/airframes/ENAC/hoops_gen_ap.xml
@@ -206,9 +206,9 @@
     <define name="G1_R" value="0.0022"/>
     <define name="G2_R" value="0.20"/>
 
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
-    <define name="FILTER_PITCH_RATE" value="TRUE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
+    <!--unit: Hz-->
+    <define name="FILT_CUTOFF_P" value="20.0"/>
+    <define name="FILT_CUTOFF_Q" value="20.0"/>
 
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="170.0"/>
@@ -218,12 +218,6 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
-    <!-- second order filter parameters -->
-    <define name="FILT_OMEGA" value="20.0"/>
-    <define name="FILT_ZETA" value="0.55"/>
-    <define name="FILT_OMEGA_R" value="20.0"/>
-    <define name="FILT_ZETA_R" value="0.55"/>
-
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
     <define name="ACT_DYN_Q" value="0.06"/>
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
index 4f587e18af..60622130f1 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
@@ -155,13 +155,6 @@
     <define name="G1_R" value="0.0025"/>
     <define name="G2_R" value="0.36"/-->
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -172,7 +165,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
index f42f763007..823e7adb51 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
@@ -157,13 +157,6 @@
     <define name="G1_R" value="0.0025"/>
     <define name="G2_R" value="0.36"/-->
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -174,7 +167,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
index c1c8e11122..78b7e9c7cd 100644
--- a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
+++ b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
@@ -203,10 +203,6 @@
     <define name="G1_R" value="-0.0015942f"/>
     <define name="G2_R" value="-0.11281f"/>
 
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="3.57f"/>
     <define name="REF_ERR_Q" value="3.57f"/>
@@ -217,7 +213,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0f"/>
-    <define name="FILT_CUTOFF_R" value="8.0f"/>
+    <define name="FILT_CUTOFF_RDOT" value="8.0f"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03149f"/>
diff --git a/conf/airframes/KS/ks_bebop2_stereo.xml b/conf/airframes/KS/ks_bebop2_stereo.xml
index f4e528e2ba..0b41749624 100644
--- a/conf/airframes/KS/ks_bebop2_stereo.xml
+++ b/conf/airframes/KS/ks_bebop2_stereo.xml
@@ -148,13 +148,6 @@
     <define name="G1_R" value="0.0010"/>
     <define name="G2_R" value="0.236"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -165,7 +158,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/KS/ks_mavtec1.xml b/conf/airframes/KS/ks_mavtec1.xml
index 52e145eaf3..325b89580d 100644
--- a/conf/airframes/KS/ks_mavtec1.xml
+++ b/conf/airframes/KS/ks_mavtec1.xml
@@ -180,7 +180,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
diff --git a/conf/airframes/MM/bebop2_lum1_xbee.xml b/conf/airframes/MM/bebop2_lum1_xbee.xml
index 53fc9366ed..e3c84e235c 100644
--- a/conf/airframes/MM/bebop2_lum1_xbee.xml
+++ b/conf/airframes/MM/bebop2_lum1_xbee.xml
@@ -176,13 +176,6 @@
     <define name="G1_R" value="0.0022"/>
     <define name="G2_R" value="0.20"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -191,15 +184,8 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
-    <!-- second order filter parameters -->
-    <define name="FILT_OMEGA" value="20.0"/>
-    <define name="FILT_ZETA" value="0.7"/>
-    <define name="FILT_OMEGA_R" value="20.0"/>
-    <define name="FILT_ZETA_R" value="0.7"/>
-
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
index 26b7ac5197..d372f9894c 100644
--- a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
+++ b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
@@ -495,11 +495,6 @@ The most crucial part for the magnetometer calibration:
     <define name="G1_R" value="0.001049" />
     <define name="G2_R" value="0.076486" />
 
-    <!-- It could be well possible we need to filter the roll rate -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="100.0"/>
     <define name="REF_ERR_Q" value="200.0"/>
@@ -509,7 +504,6 @@ The most crucial part for the magnetometer calibration:
     <define name="REF_RATE_R" value="20.0"/>
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.04"/>
diff --git a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
index b0ec9c48bd..540e827967 100644
--- a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
+++ b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
@@ -261,10 +261,6 @@
     <define name="G1_R" value="-0.0015942f"/>
     <define name="G2_R" value="-0.11281f"/>
 
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="3.57f"/>
     <define name="REF_ERR_Q" value="3.57f"/>
@@ -275,7 +271,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0f"/>
-    <define name="FILT_CUTOFF_R" value="8.0f"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03149f"/>
@@ -350,4 +345,4 @@
     <define name="ALT_SHIFT_MINUS" value="-1"/>
     <define name="AC_ICON" value="quadrotor_xi"/>
   </section>
-</airframe>
\ No newline at end of file
+</airframe>
diff --git a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
index bad1cabf95..4cc07586a4 100644
--- a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
+++ b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
@@ -380,11 +380,6 @@
         <define name="G1_R" value="0.0022"/>
         <define name="G2_R" value="0.20"/>
 
-        <!-- For some airframes, e.g. Bebop2 we need to filter the roll rate due to the dampers -->
-        <define name="FILTER_ROLL_RATE" value="FALSE"/>
-        <define name="FILTER_PITCH_RATE" value="FALSE"/>
-        <define name="FILTER_YAW_RATE" value="FALSE"/>
-
         <!-- reference acceleration for attitude control -->
         <define name="REF_ERR_P" value="170.0"/>
         <define name="REF_ERR_Q" value="600.0"/>
@@ -396,7 +391,6 @@
         <!-- second order filter parameters -->
         <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
         <define name="FILT_CUTOFF" value="5.0"/>
-        <define name="FILT_CUTOFF_P" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
         <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml b/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
index 0d277b4166..93797a3f9c 100644
--- a/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
+++ b/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
@@ -329,11 +329,6 @@
         <define name="G1_R" value="0.0022"/>
         <define name="G2_R" value="0.20"/>
 
-        <!-- For some airframes, e.g. Bebop2 we need to filter the roll rate due to the dampers -->
-        <define name="FILTER_ROLL_RATE" value="FALSE"/>
-        <define name="FILTER_PITCH_RATE" value="FALSE"/>
-        <define name="FILTER_YAW_RATE" value="FALSE"/>
-
         <!-- reference acceleration for attitude control -->
         <define name="REF_ERR_P" value="170.0"/>
         <define name="REF_ERR_Q" value="600.0"/>
@@ -345,7 +340,6 @@
         <!-- second order filter parameters -->
         <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
         <define name="FILT_CUTOFF" value="5.0"/>
-        <define name="FILT_CUTOFF_P" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
         <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
index 6fb20a8d0d..70a078e6e4 100644
--- a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
+++ b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
@@ -376,11 +376,6 @@
         <define name="G1_R" value="0.0022"/>
         <define name="G2_R" value="0.20"/>
 
-        <!-- For some airframes, e.g. Bebop2 we need to filter the roll rate due to the dampers -->
-        <define name="FILTER_ROLL_RATE" value="FALSE"/>
-        <define name="FILTER_PITCH_RATE" value="FALSE"/>
-        <define name="FILTER_YAW_RATE" value="FALSE"/>
-
         <!-- reference acceleration for attitude control -->
         <define name="REF_ERR_P" value="170.0"/>
         <define name="REF_ERR_Q" value="600.0"/>
@@ -392,7 +387,6 @@
         <!-- second order filter parameters -->
         <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
         <define name="FILT_CUTOFF" value="5.0"/>
-        <define name="FILT_CUTOFF_P" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
         <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
index 10a34248d4..cba8e9a79c 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
@@ -425,11 +425,6 @@ The most crucial part for the magnetometer calibration:
     <define name="G1_R" value="0.0032"/>
     <define name="G2_R" value="0.16"/>
 
-    <!-- For the ArDrone MAYBE we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="380.0"/>
     <define name="REF_ERR_Q" value="380.0"/>
@@ -440,7 +435,6 @@ The most crucial part for the magnetometer calibration:
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
index 4673f83480..1b6bacbb95 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
@@ -464,16 +464,8 @@ The most crucial part for the magnetometer calibration:
     <define name="G1_R" value="0.0022"/>
     <define name="G2_R" value="0.20"/>-->
 
-    <!-- ? Maybe with a High capacity battery(heavier, and weight lies higher) for the bebop1 we also need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
-    <!-- Here it is assumed that your removed the damping from your bebop!
-         The dampers damp, but also cause oscillation. By making them fix, the bebop could fly better with INDI -->
-    <!--<define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>-->
+    <!--unit: Hz-->
+    <define name="FILT_CUTOFF_P" value="20.0"/>
 
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
@@ -490,7 +482,7 @@ The most crucial part for the magnetometer calibration:
 
 second order filter parameters -->
     <!-- NOT USED <define name="FILT_CUTOFF" value="4.0"/>
-    <define name="FILT_CUTOFF_R" value="4.0"/> -->
+    <define name="FILT_CUTOFF_RDOT" value="4.0"/> -->
 
     <!-- first order actuator dynamics -->
  <!--   <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
index b5925b171c..aa4b21cefc 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
@@ -470,16 +470,8 @@ The most crucial part for the magnetometer calibration:
 
     <define name="G2_R" value="0.20"/>
 
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers, with fixed damper set to false -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers damp, but also cause oscillation. By making them fix, the bebop2 will fly much better on INDI -->
-    <!--<define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>-->
+    <!--unit: Hz-->
+    <define name="FILT_CUTOFF_P" value="20.0"/>
 
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="170.0"/>
@@ -499,7 +491,6 @@ The most crucial part for the magnetometer calibration:
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
index 2e4afaacf9..bf1fb27184 100644
--- a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
+++ b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
@@ -567,11 +567,6 @@ The most crucial part for the magnetometer calibration:
     <define name="G1_R" value="0.0095"/>
     <define name="G2_R" value="0.0"/>
 
-    <!-- It could be well possible we need to filter the roll rate -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- setpoints -->
     <define name="SP_MAX_PHI" value="60." unit="deg"/>
     <define name="SP_MAX_THETA" value="60." unit="deg"/>
@@ -590,7 +585,7 @@ The most crucial part for the magnetometer calibration:
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml
index a193782655..922d9f1222 100644
--- a/conf/airframes/examples/bebop2_indi.xml
+++ b/conf/airframes/examples/bebop2_indi.xml
@@ -129,11 +129,6 @@
     <define name="G1_Q" value="0.0473"/>
     <define name="G1_R" value="0.00122"/>
 
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/><!-- Set to TRUE when flying without locked dampers -->
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="170.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -144,7 +139,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/examples/bebop2_opticflow.xml b/conf/airframes/examples/bebop2_opticflow.xml
index 1a580871e9..bc14eb22d8 100644
--- a/conf/airframes/examples/bebop2_opticflow.xml
+++ b/conf/airframes/examples/bebop2_opticflow.xml
@@ -144,13 +144,6 @@
     <define name="G1_R" value="0.0010"/>
     <define name="G2_R" value="0.236"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -159,16 +152,7 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
-    <!-- second order filter parameters -->
-    <!-- old?
-	<define name="FILT_OMEGA" value="20.0"/>
-    <define name="FILT_ZETA" value="0.7"/>
-    <define name="FILT_OMEGA_R" value="20.0"/>
-    <define name="FILT_ZETA_R" value="0.7"/>
-	-->
-
-	<define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+	  <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
index 4694248600..89d436999f 100644
--- a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
+++ b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
@@ -148,10 +148,8 @@
     <define name="G1_Q" value="0.025"/>
     <define name="G1_R" value="0.0022"/>
 
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
+    <!--unit: Hz-->
+    <define name="FILT_CUTOFF_P" value="20.0"/>
 
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="170.0"/>
@@ -163,7 +161,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/examples/quadrotor_navstik.xml b/conf/airframes/examples/quadrotor_navstik.xml
index 2e7a49dadd..7bf8ccb18f 100644
--- a/conf/airframes/examples/quadrotor_navstik.xml
+++ b/conf/airframes/examples/quadrotor_navstik.xml
@@ -125,7 +125,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.2"/>
diff --git a/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml b/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
index a93d2a2ea8..97b8b137ee 100644
--- a/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
+++ b/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
@@ -233,7 +233,7 @@
     <define name="REF_RATE_R" value="12.0"/>
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
     <define name="ACT_DYN_Q" value="0.03"/>
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index 55fe6b2f8d..a6fb51beb1 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -231,7 +231,7 @@
     <define name="REF_RATE_R" value="12.0"/>
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
     <define name="ACT_DYN_Q" value="0.03"/>
diff --git a/conf/airframes/examples/trashcan.xml b/conf/airframes/examples/trashcan.xml
index 6fb20a8d0d..70a078e6e4 100644
--- a/conf/airframes/examples/trashcan.xml
+++ b/conf/airframes/examples/trashcan.xml
@@ -376,11 +376,6 @@
         <define name="G1_R" value="0.0022"/>
         <define name="G2_R" value="0.20"/>
 
-        <!-- For some airframes, e.g. Bebop2 we need to filter the roll rate due to the dampers -->
-        <define name="FILTER_ROLL_RATE" value="FALSE"/>
-        <define name="FILTER_PITCH_RATE" value="FALSE"/>
-        <define name="FILTER_YAW_RATE" value="FALSE"/>
-
         <!-- reference acceleration for attitude control -->
         <define name="REF_ERR_P" value="170.0"/>
         <define name="REF_ERR_Q" value="600.0"/>
@@ -392,7 +387,6 @@
         <!-- second order filter parameters -->
         <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
         <define name="FILT_CUTOFF" value="5.0"/>
-        <define name="FILT_CUTOFF_P" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
         <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/ardrone2_OF_hover.xml b/conf/airframes/tudelft/ardrone2_OF_hover.xml
index 7fddba5f7c..f82bcf887b 100644
--- a/conf/airframes/tudelft/ardrone2_OF_hover.xml
+++ b/conf/airframes/tudelft/ardrone2_OF_hover.xml
@@ -195,7 +195,6 @@
 
 	<!-- second order filter parameters -->
 	<define name="FILT_CUTOFF" value="3.2" />
-	<define name="FILT_CUTOFF_R" value="3.2" />
 
 	<!-- first order actuator dynamics -->
 	<define name="ACT_DYN_P" value="0.06" />
diff --git a/conf/airframes/tudelft/ardrone2_indi.xml b/conf/airframes/tudelft/ardrone2_indi.xml
index ac18dc913a..627b9879c6 100644
--- a/conf/airframes/tudelft/ardrone2_indi.xml
+++ b/conf/airframes/tudelft/ardrone2_indi.xml
@@ -151,7 +151,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/ardrone2_optitrack.xml b/conf/airframes/tudelft/ardrone2_optitrack.xml
index 4fe769e5b1..4d0aa23e65 100644
--- a/conf/airframes/tudelft/ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/ardrone2_optitrack.xml
@@ -124,7 +124,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop2_indi.xml b/conf/airframes/tudelft/bebop2_indi.xml
index 7c445ec59f..9df614cd27 100644
--- a/conf/airframes/tudelft/bebop2_indi.xml
+++ b/conf/airframes/tudelft/bebop2_indi.xml
@@ -103,11 +103,6 @@
     <!--Counter torque effect of spinning up a rotor-->
     <define name="G2" value="{ 110.f,   -110.f,   110.f,   -110.f}"/>
 
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="380.f"/>
     <define name="REF_ERR_Q" value="380.f"/>
diff --git a/conf/airframes/tudelft/bebop2_no_damping.xml b/conf/airframes/tudelft/bebop2_no_damping.xml
index 350fd9466b..b155397acb 100644
--- a/conf/airframes/tudelft/bebop2_no_damping.xml
+++ b/conf/airframes/tudelft/bebop2_no_damping.xml
@@ -130,13 +130,6 @@
     <define name="G1_R" value="0.0022"/>
     <define name="G2_R" value="0.20"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -147,7 +140,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop2_opticflow.xml b/conf/airframes/tudelft/bebop2_opticflow.xml
index 661891f180..6977594c5d 100644
--- a/conf/airframes/tudelft/bebop2_opticflow.xml
+++ b/conf/airframes/tudelft/bebop2_opticflow.xml
@@ -162,13 +162,6 @@
     <define name="G1_R" value="0.0010"/>
     <define name="G2_R" value="0.236"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -177,16 +170,7 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
-    <!-- second order filter parameters -->
-    <!-- old?
-	<define name="FILT_OMEGA" value="20.0"/>
-    <define name="FILT_ZETA" value="0.7"/>
-    <define name="FILT_OMEGA_R" value="20.0"/>
-    <define name="FILT_ZETA_R" value="0.7"/>
-	-->
-
-	<define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+	  <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack.xml b/conf/airframes/tudelft/bebop2_optitrack.xml
index 142addf0ed..180bcfce17 100644
--- a/conf/airframes/tudelft/bebop2_optitrack.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack.xml
@@ -138,11 +138,6 @@
     <define name="G1_R" value="0.0022"/>
     <define name="G2_R" value="0.20"/>
 
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="170.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -153,7 +148,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
index dd82e8ee17..d59359f66a 100644
--- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -174,13 +174,6 @@
     <define name="G1_R" value="0.0025"/>
     <define name="G2_R" value="0.36"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -191,7 +184,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop2_undistort_front.xml b/conf/airframes/tudelft/bebop2_undistort_front.xml
index 3471e45914..4c7985faa8 100644
--- a/conf/airframes/tudelft/bebop2_undistort_front.xml
+++ b/conf/airframes/tudelft/bebop2_undistort_front.xml
@@ -163,13 +163,6 @@
     <define name="G1_R" value="0.0025"/>
     <define name="G2_R" value="0.36"/>
 
-    <!-- Here it is assumed that your removed the damping from your bebop2!
-         The dampers do not really damp, but cause oscillation. By removing/
-         fixing them, the bebop2 will fly much better-->
-    <define name="FILTER_ROLL_RATE" value="FALSE"/>
-    <define name="FILTER_PITCH_RATE" value="FALSE"/>
-    <define name="FILTER_YAW_RATE" value="FALSE"/>
-
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="600.0"/>
     <define name="REF_ERR_Q" value="600.0"/>
@@ -180,7 +173,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
index 274eb3d6cf..3e2adbdb3d 100644
--- a/conf/airframes/tudelft/bebop_OF_hover.xml
+++ b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -173,7 +173,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/tudelft/bebop_flip.xml b/conf/airframes/tudelft/bebop_flip.xml
index d986ff1843..05914c8038 100644
--- a/conf/airframes/tudelft/bebop_flip.xml
+++ b/conf/airframes/tudelft/bebop_flip.xml
@@ -171,7 +171,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/tudelft/bebop_mavlink.xml b/conf/airframes/tudelft/bebop_mavlink.xml
index 11a4d1bf67..e19998f940 100644
--- a/conf/airframes/tudelft/bebop_mavlink.xml
+++ b/conf/airframes/tudelft/bebop_mavlink.xml
@@ -180,7 +180,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/tudelft/bebop_opticflow.xml b/conf/airframes/tudelft/bebop_opticflow.xml
index 84336b72e8..1343cdbe90 100644
--- a/conf/airframes/tudelft/bebop_opticflow.xml
+++ b/conf/airframes/tudelft/bebop_opticflow.xml
@@ -154,7 +154,6 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="8.0"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.1"/>
diff --git a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
index 4984348c10..ecce76fba9 100644
--- a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
@@ -130,7 +130,6 @@ ARDrone2 with optical_flow landing.
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.06"/>
diff --git a/conf/airframes/tudelft/iris_indi.xml b/conf/airframes/tudelft/iris_indi.xml
index c0dcaa5262..1630335245 100644
--- a/conf/airframes/tudelft/iris_indi.xml
+++ b/conf/airframes/tudelft/iris_indi.xml
@@ -195,10 +195,6 @@
     <define name="G1_Q" value="0.018315" />
     <define name="G1_R" value="0.001049" />
     <define name="G2_R" value="0.076486" />
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE" />
-    <define name="FILTER_PITCH_RATE" value="FALSE" />
-    <define name="FILTER_YAW_RATE" value="FALSE" />
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="100.0" />
     <define name="REF_ERR_Q" value="100.0" />
@@ -208,7 +204,7 @@
     <define name="REF_RATE_R" value="14.0" />
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.04" />
     <define name="ACT_DYN_Q" value="0.04" />
diff --git a/conf/airframes/tudelft/mavtec1.xml b/conf/airframes/tudelft/mavtec1.xml
index f4c2630431..5f66ab7dbd 100644
--- a/conf/airframes/tudelft/mavtec1.xml
+++ b/conf/airframes/tudelft/mavtec1.xml
@@ -184,7 +184,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.03"/>
diff --git a/conf/airframes/tudelft/mavtec5.xml b/conf/airframes/tudelft/mavtec5.xml
index efbec20c41..381a586bc1 100644
--- a/conf/airframes/tudelft/mavtec5.xml
+++ b/conf/airframes/tudelft/mavtec5.xml
@@ -174,7 +174,7 @@
 
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="8.0"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.15"/>
diff --git a/conf/airframes/tudelft/neddrone4.xml b/conf/airframes/tudelft/neddrone4.xml
index ab17b1ce8c..6b471d4cb0 100644
--- a/conf/airframes/tudelft/neddrone4.xml
+++ b/conf/airframes/tudelft/neddrone4.xml
@@ -21,12 +21,9 @@
         <configure name="SBUS_PORT" value="UART3"/>
       </module>
 
-      <module name="airspeed" type="ets">
+      <module name="airspeed" type="ms45xx_i2c">
         <define name="USE_I2C4"/>
-        <configure name="AIRSPEED_ETS_I2C_DEV" value="I2C4"/>
-        <define name="AIRSPEED_ETS_SYNC_SEND"/>
-        <define name="USE_AIRSPEED_ETS" value="TRUE"/>
-        <define name="AIRSPEED_ETS_SCALE" value="1.24"/>
+        <define name="MS45XX_I2C_DEV" value="i2c4"/>
       </module>
 
       <!-- TODO: add this module -->
@@ -148,10 +145,10 @@
     <servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="FLAP_3" no="6" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_3" no="7" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_4" no="8" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_4" no="9" min="6000" neutral="0" max="-6000"/>
+    <servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
+    <servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
+    <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
+    <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
   </servos>
 
   <commands>
@@ -169,8 +166,8 @@
   </rc_commands>
 
   <section name="MIXING" prefix="MOTOR_MIXING_">
-    <!-- frontleft left (CW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CCW) -->
-    <!-- backleft left (CCW), backleft mid (CW), backleft right (CCW), backright left (CW), backright mid (CCW), backright right (CW) -->
+    <!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
+    <!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
     <define name="NB_MOTOR"    value="12"/>
     <define name="SCALE"       value="256"/>
     <define name="ROLL_COEF"   value="{256,  157,   56,  -56, -157, -256,  256,  157,   56,  -56, -157, -256}"/>
@@ -200,12 +197,12 @@
     <!-- Removed ApplyDiff for differential control -->
     <set servo="AIL_1"     value="-2*@PITCH + (  2*@YAW)"/>
     <set servo="AIL_2"     value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_3"     value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_4"     value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="AIL_3"     value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="AIL_4"     value=" 2*@PITCH + (- 2*@YAW)"/>
     <set servo="FLAP_1"    value="-2*@PITCH + (  2*@YAW)"/>
     <set servo="FLAP_2"    value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_3"    value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_4"    value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="FLAP_3"    value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="FLAP_4"    value=" 2*@PITCH + (- 2*@YAW)"/>
   </command_laws>
 
   <section name="MISC">
@@ -373,8 +370,6 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
-    <define name="FILTER_YAW_RATE" value="TRUE"/>
-
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.0354"/>
     <define name="ACT_DYN_Q" value="0.0354"/>
diff --git a/conf/airframes/tudelft/neddrone5.xml b/conf/airframes/tudelft/neddrone5.xml
index 365e80849f..95c556985a 100644
--- a/conf/airframes/tudelft/neddrone5.xml
+++ b/conf/airframes/tudelft/neddrone5.xml
@@ -146,10 +146,10 @@
     <servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="FLAP_3" no="6" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_3" no="7" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_4" no="8" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_4" no="9" min="6000" neutral="0" max="-6000"/>
+    <servo name="AIL_3" no="6" min="6000" neutral="0" max="6000"/>
+    <servo name="FLAP_3" no="7" min="6000" neutral="0" max="6000"/>
+    <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
+    <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
   </servos>
 
   <commands>
@@ -167,8 +167,8 @@
   </rc_commands>
 
   <section name="MIXING" prefix="MOTOR_MIXING_">
-    <!-- frontleft left (CW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CCW) -->
-    <!-- backleft left (CCW), backleft mid (CW), backleft right (CCW), backright left (CW), backright mid (CCW), backright right (CW) -->
+    <!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
+    <!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
     <define name="NB_MOTOR"    value="12"/>
     <define name="SCALE"       value="256"/>
     <define name="ROLL_COEF"   value="{256,  157,   56,  -56, -157, -256,  256,  157,   56,  -56, -157, -256}"/>
@@ -198,12 +198,12 @@
     <!-- Removed ApplyDiff for differential control -->
     <set servo="AIL_1"     value="-2*@PITCH + (  2*@YAW)"/>
     <set servo="AIL_2"     value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_3"     value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_4"     value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="AIL_3"     value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="AIL_4"     value=" 2*@PITCH + (- 2*@YAW)"/>
     <set servo="FLAP_1"    value="-2*@PITCH + (  2*@YAW)"/>
     <set servo="FLAP_2"    value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_3"    value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_4"    value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="FLAP_3"    value=" 2*@PITCH + (  2*@YAW)"/>
+    <set servo="FLAP_4"    value=" 2*@PITCH + (- 2*@YAW)"/>
   </command_laws>
 
   <section name="MISC">
@@ -375,8 +375,6 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
-    <define name="FILTER_YAW_RATE" value="TRUE"/>
-
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.0354"/>
     <define name="ACT_DYN_Q" value="0.0354"/>
diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml
index c503a53fd6..12f20b88ee 100644
--- a/conf/airframes/tudelft/robird.xml
+++ b/conf/airframes/tudelft/robird.xml
@@ -181,10 +181,6 @@ Flapping wing frame equiped with
     <define name="G1_Q" value="0.018315" />
     <define name="G1_R" value="0.001049" />
     <define name="G2_R" value="0.076486" />
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE" />
-    <define name="FILTER_PITCH_RATE" value="FALSE" />
-    <define name="FILTER_YAW_RATE" value="FALSE" />
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="100.0" />
     <define name="REF_ERR_Q" value="100.0" />
@@ -194,7 +190,7 @@ Flapping wing frame equiped with
     <define name="REF_RATE_R" value="14.0" />
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
+    <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.04" />
     <define name="ACT_DYN_Q" value="0.04" />
diff --git a/conf/airframes/tudelft/splash_px4.xml b/conf/airframes/tudelft/splash_px4.xml
index cf739a992f..7efe962454 100644
--- a/conf/airframes/tudelft/splash_px4.xml
+++ b/conf/airframes/tudelft/splash_px4.xml
@@ -166,10 +166,6 @@
     <define name="G1_Q" value="0.025867" />
     <define name="G1_R" value="0.002055" />
     <define name="G2_R" value="0.15865" />
-    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="FALSE" />
-    <define name="FILTER_PITCH_RATE" value="FALSE" />
-    <define name="FILTER_YAW_RATE" value="FALSE" />
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="100.0" />
     <define name="REF_ERR_Q" value="100.0" />
@@ -179,7 +175,6 @@
     <define name="REF_RATE_R" value="14.0" />
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
-    <define name="FILT_CUTOFF_R" value="3.2"/>
     <!-- first order actuator dynamics -->
     <define name="ACT_DYN_P" value="0.04" />
     <define name="ACT_DYN_Q" value="0.04" />
diff --git a/conf/modules/stabilization_indi_simple.xml b/conf/modules/stabilization_indi_simple.xml
index c16a1a4bf6..370c780828 100644
--- a/conf/modules/stabilization_indi_simple.xml
+++ b/conf/modules/stabilization_indi_simple.xml
@@ -37,7 +37,8 @@
       <define name="REF_RATE_Q" value="28.0" description="linear gains for INDI reference"/>
       <define name="REF_RATE_R" value="28.0" description="linear gains for INDI reference"/>
       <define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
-      <define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
+      <define name="FILT_CUTOFF" value="8.0" description="second order filter cutoff frequency Hz"/>
+      <define name="FILT_CUTOFF_RDOT" value="8.0" description="second order filter cutoff frequency rdot Hz"/>
       <define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
       <define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/>
       <define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/>
@@ -45,6 +46,9 @@
       <define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
       <define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
       <define name="FULL_AUTHORITY" value="FALSE" description="Enable full control authority"/>
+      <define name="FILT_CUTOFF_P" value="30.0" description="First order filter P cutoff value"/>
+      <define name="FILT_CUTOFF_Q" value="30.0" description="First order filter Q cutoff value"/>
+      <define name="FILT_CUTOFF_R" value="30.0" description="First order filter R cutoff value"/>
     </section>
   </doc>
   <settings>
@@ -63,6 +67,7 @@
         <dl_setting var="indi.adaptive" min="0" step="1" max="1" 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.01" max="400.0" 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="400.0" shortname="max_yaw_rate_attitude" param="STABILIZATION_INDI_MAX_R" unit="rad/s" alt_unit="deg/s"/>
+        <dl_setting var="indi.cutoff_r" min="0.2" step="0.1" max="30.0" shortname="r_cutoff_hz" handler="reset_r_filter_cutoff" type="float" module="stabilization/stabilization_indi_simple"/>
       </dl_settings>
     </dl_settings>
   </settings>
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index ae62168fea..98ad920dc3 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -293,7 +293,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi_simple.xml"
    gui_color="blue"
   />
   <aircraft
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
index aa44fccfc7..ef9111daf4 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
@@ -39,6 +39,7 @@
 #include "generated/airframe.h"
 #include "paparazzi.h"
 #include "subsystems/radio_control.h"
+#include "filters/low_pass_filter.h"
 
 #if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
 #error You have to define the first order time constant of the actuator dynamics!
@@ -51,8 +52,8 @@
 #endif
 
 // the yaw sometimes requires more filtering
-#ifndef STABILIZATION_INDI_FILT_CUTOFF_R
-#define STABILIZATION_INDI_FILT_CUTOFF_R STABILIZATION_INDI_FILT_CUTOFF
+#ifndef STABILIZATION_INDI_FILT_CUTOFF_RDOT
+#define STABILIZATION_INDI_FILT_CUTOFF_RDOT STABILIZATION_INDI_FILT_CUTOFF
 #endif
 
 #ifndef STABILIZATION_INDI_MAX_RATE
@@ -71,10 +72,28 @@
 #define STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF 4.0
 #endif
 
+#ifdef STABILIZATION_INDI_FILT_CUTOFF_P
+#define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
+#else
+#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
+#endif
+
+#ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
+#define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
+#else
+#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
+#endif
+
+#ifdef STABILIZATION_INDI_FILT_CUTOFF_R
+#define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
+#else
+#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
+#endif
+
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
 
-struct FloatRates rates_filt_fo;
+static struct FirstOrderLowPass rates_filt_fo[3];
 
 static inline void lms_estimation(void);
 static void indi_init_filters(void);
@@ -83,6 +102,7 @@ static void indi_init_filters(void);
 #define INDI_EST_SCALE 0.001
 
 struct IndiVariables indi = {
+  .cutoff_r = STABILIZATION_INDI_FILT_CUTOFF_R,
   .max_rate = STABILIZATION_INDI_MAX_RATE,
   .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
 
@@ -172,8 +192,8 @@ void indi_init_filters(void)
 {
   // tau = 1/(2*pi*Fc)
   float tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF);
-  float tau_r = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
-  float tau_axis[3] = {tau, tau, tau_r};
+  float tau_rdot = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_RDOT);
+  float tau_axis[3] = {tau, tau, tau_rdot};
   float tau_est = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_ESTIMATION_FILT_CUTOFF);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
   // Filtering of gyroscope and actuators
@@ -183,8 +203,21 @@ void indi_init_filters(void)
     init_butterworth_2_low_pass(&indi.est.u[i], tau_est, sample_time, 0.0);
     init_butterworth_2_low_pass(&indi.est.rate[i], tau_est, sample_time, 0.0);
   }
+
   // Init rate filter for feedback
-  RATES_COPY(rates_filt_fo, (*stateGetBodyRates_f()));
+  float time_constants[3] = {1.0/(2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P), 1.0/(2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q), 1.0/(2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R)};
+
+  init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
+  init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
+  init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
+}
+
+// Callback function for setting cutoff frequency for r
+void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff) {
+  float sample_time = 1.0 / PERIODIC_FREQUENCY;
+  indi.cutoff_r = new_cutoff;
+  float time_constant = 1.0/(2.0 * M_PI * indi.cutoff_r);
+  init_first_order_low_pass(&rates_filt_fo[2], time_constant, sample_time, stateGetBodyRates_f()->r);
 }
 
 void stabilization_indi_enter(void)
@@ -309,33 +342,33 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
   // Calculate the derivative of the rates
   finite_difference_from_filter(indi.rate_d, indi.rate);
 
-  //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
-
+  //The rates used for feedback are by default the measured 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.
+  //Note that due to the delay, the PD controller may need relaxed gains.
+  struct FloatRates rates_filt;
 #if STABILIZATION_INDI_FILTER_ROLL_RATE
-  rates_filt_fo.p = (rates_filt_fo.p*3+body_rates->p)/4;
+  rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
 #else
-  rates_filt_fo.p = body_rates->p;
+  rates_filt.p = body_rates->p;
 #endif
 #if STABILIZATION_INDI_FILTER_PITCH_RATE
-  rates_filt_fo.q = (rates_filt_fo.q*3+body_rates->q)/4;
+  rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
 #else
-  rates_filt_fo.q = body_rates->q;
+  rates_filt.q = body_rates->q;
 #endif
 #if STABILIZATION_INDI_FILTER_YAW_RATE
-  rates_filt_fo.r = (rates_filt_fo.r*3+body_rates->r)/4;
+  rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
 #else
-  rates_filt_fo.r = body_rates->r;
+  rates_filt.r = body_rates->r;
 #endif
 
-  //This separates the P and D controller and lets you impose a maximum yaw rate.
+  //This lets you impose a maximum yaw rate.
   BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
 
   // Compute reference angular acceleration:
-  indi.angular_accel_ref.p = (rate_sp.p - rates_filt_fo.p) * indi.gains.rate.p;
-  indi.angular_accel_ref.q = (rate_sp.q - rates_filt_fo.q) * indi.gains.rate.q;
-  indi.angular_accel_ref.r = (rate_sp.r - rates_filt_fo.r) * indi.gains.rate.r;
+  indi.angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi.gains.rate.p;
+  indi.angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi.gains.rate.q;
+  indi.angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi.gains.rate.r;
 
   //Increment in angular acceleration requires increment in control input
   //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
index 02188a4c16..3d5af3292d 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h
@@ -58,6 +58,7 @@ struct IndiEstimation {
 };
 
 struct IndiVariables {
+  float cutoff_r;
   struct FloatRates angular_accel_ref;
   struct FloatRates du;
   struct FloatRates u_in;
@@ -87,6 +88,7 @@ extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t h
 extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
 extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
 extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
+extern void stabilization_indi_simple_reset_r_filter_cutoff(float new_cutoff);
 
 #endif /* STABILIZATION_INDI_SIMPLE_H */