diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
index 336b882210..ed55d50e6f 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.c
@@ -25,7 +25,6 @@
  */
 
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_saturate.h"
 #include "generated/airframe.h"
 
 
diff --git a/sw/misc/attitude_reference/control.py b/sw/misc/attitude_reference/control.py
index 639305b2fd..67309a079a 100644
--- a/sw/misc/attitude_reference/control.py
+++ b/sw/misc/attitude_reference/control.py
@@ -122,8 +122,7 @@ class att_ref_sat_naive(att_ref_default):
         for p in ['sat_vel', 'sat_accel']:
             self.ensure_vect(p)
 
-    def update_quat(self, setpoint, dt):
-        super(att_ref_sat_naive, self).update_quat(setpoint, dt)
+    def saturate_naive(self):
         self.accel = np.clip(self.accel, -self.sat_accel, self.sat_accel)
         for i in range(0, 3):
             if self.vel[i] >= self.sat_vel[i]:
@@ -134,6 +133,10 @@ class att_ref_sat_naive(att_ref_default):
                 self.vel[i] = -self.sat_vel[i]
                 if self.accel[i] < 0:
                     self.accel[i] = 0
+
+    def update_quat(self, setpoint, dt):
+        super(att_ref_sat_naive, self).update_quat(setpoint, dt)
+        self.saturate_naive()
         return self.quat, self.vel, self.accel
 
 
@@ -164,9 +167,11 @@ class att_ref_sat_nested(att_ref_sat_naive):
         # self.K = pc.butterworth(2, self.omega[0])
         omega, xi = self.omega[0], self.xi[0]
         omega_d = omega * math.sqrt(1 - xi ** 2)
+        LOG.debug('omega_d: {}'.format(omega_d))
+        LOG.debug('-omega*xi: {}'.format(-omega * xi))
         coefs = -np.poly([complex(-omega * xi, omega_d), complex(-omega * xi, -omega_d)])[::-1]
         self.K = np.real(coefs[:-1])
-        LOG.debug('sat_nested.__init__  omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K)))
+        LOG.debug('sat_nested.__init__  omega: {} coefs:{} K:{} poles:{} '.format(self.omega[0], coefs, self.K, pc.poles(self.K)))
         self.sats = np.array([self.sat_vel, self.sat_accel])
         LOG.debug(' sats:\n{}'.format(self.sats))
         self.M = np.array(self.sats)