fix quaternion integration equation in backend np

IMRCLab · Dec 12, 2023 · 8e54aa5 · 8e54aa5
1 parent 5bd08d8
commit 8e54aa5
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
+        # 
+        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 + (