fix quaternion integration equation in backend np (#383)

* fix quaternion integration equation in backend np
IMRCLab · Dec 13, 2023 · d446a09 · d446a09
1 parent 963df7b
commit d446a09
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/crazyflie_sim/crazyflie_sim/backend/np.py b/crazyflie_sim/crazyflie_sim/backend/np.py
@@ -110,9 +110,11 @@ def rpm_to_force(rpm):
         # to integrate the dynamics, see
         # https://www.ashwinnarayan.com/post/how-to-integrate-quaternions/, and
         # https://arxiv.org/pdf/1604.08139.pdf
+        # Sec 4.5, https://arxiv.org/pdf/1711.02508.pdf
+        omega_global = rowan.rotate(self.state.quat, self.state.omega)
         q_next = rowan.normalize(
             rowan.calculus.integrate(
-                self.state.quat, self.state.omega, dt))
+                self.state.quat, omega_global, dt))
 
         # mJ = Jw x w + tau_u
         omega_next = self.state.omega + (