From dccd662dfe809a4f57f6662c782e01c921d3818b Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 9 Jul 2021 11:32:40 +0200 Subject: [PATCH] EKF python derivation: use unit quaternion constraint This produces a more numerically stable and simpler derivation as it already contains the unit quaternion constraint --- EKF/python/ekf_derivation/main.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/EKF/python/ekf_derivation/main.py b/EKF/python/ekf_derivation/main.py index 44155332bb..2bb0d3b719 100755 --- a/EKF/python/ekf_derivation/main.py +++ b/EKF/python/ekf_derivation/main.py @@ -12,9 +12,16 @@ def quat2Rot(q): q2 = q[2] q3 = q[3] - Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], - [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], - [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) + # Rot = Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + # [2*(q1*q2 + q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 - q0*q1)], + # [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]) + + # Use the simplified formula for unit quaternion to rotation matrix + # as it produces a simpler and more stable EKF derivation given + # the additional constraint: q0^2 + q1^2 + q2^2 + q3^2 = 1 + Rot = Matrix([[1 - 2*q2**2 - 2*q3**2, 2*(q1*q2 - q0*q3), 2*(q1*q3 + q0*q2)], + [2*(q1*q2 + q0*q3), 1 - 2*q1**2 - 2*q3**2, 2*(q2*q3 - q0*q1)], + [2*(q1*q3-q0*q2), 2*(q2*q3 + q0*q1), 1 - 2*q1**2 - 2*q2**2]]) return Rot