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

ekf2: use legacy quat to rotation matrix for observation equations

Browse Source
This commit is contained in:
Paul Riseborough 2022-01-09 21:22:25 +11:00 committed by bresch
parent cb0b4c596d
commit fb3dadd0fe
1 changed files with 17 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/ekf2/EKF/python/ekf_derivation/main.py
View File

@ -6,22 +6,23 @@ from code_gen import *
# q: quaternion describing rotation from frame 1 to frame 2
# returns a rotation matrix derived form q which describes the same
# rotation
def quat2Rot(q):
def quat2Rot(q, use_legacy_method=0):
q0 = q[0]
q1 = q[1]
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]])
# 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]])
if use_legacy_method == 1:
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]])
else:
# 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
@ -615,6 +616,11 @@ def generate_code():
cov_code_generator.close()
# use legacy quaternion to rotation matrix conversion for observaton equation as it gives
# simpler equations
R_to_earth = quat2Rot(q,1)
R_to_body = R_to_earth.T
# derive autocode for observation methods
print('Generating heading observation code ...')
yaw_observation(P,state,R_to_earth)