fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 00:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF python derivation: use unit quaternion constraint

Browse Source 
This produces a more numerically stable and simpler derivation as it
already contains the unit quaternion constraint
This commit is contained in:
bresch
2021-07-13 10:49:32 +02:00
committed by Mathieu Bresciani
parent 0bebe1a656
commit dccd662dfe
1 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/python/ekf_derivation/main.py
+10 -3
View File
@@ -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