From fb3dadd0fe4cff41cc694867070cb2458cdb2636 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 9 Jan 2022 21:22:25 +1100 Subject: [PATCH] ekf2: use legacy quat to rotation matrix for observation equations --- .../ekf2/EKF/python/ekf_derivation/main.py | 28 +++++++++++-------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/main.py b/src/modules/ekf2/EKF/python/ekf_derivation/main.py index 69ef5450d7..3d3391a15a 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/main.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/main.py @@ -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)