From 1c8910ac59dc8ee1d1125ee97ffa8b122b353d1e Mon Sep 17 00:00:00 2001 From: Fabien Fellay <33905146+FabienFellay@users.noreply.github.com> Date: Thu, 6 Jan 2022 15:18:13 +0100 Subject: [PATCH] Fix quaternions to roll, pitch, yaw angles The series_quat2euler function computes wrong roll and pitch angles. This has been fixed by replacing the transforms3d.taitbryan.quat2euler function with the transforms3d.euler.quat2euler function called with axes='sxyz' option (1-2-3 extrinsic convention). Note that the transforms3d.taitbryan.quat2euler function is equivalent to the transforms3d.euler.quat2euler function called with axes='szyx' option (3-2-1 extrinsic convention), which is not the used convention in PX4 (3-2-1 intrinsic convention). Finally, an extrinsic convention is equivalent to an intrinsic convention but with rotations performed in reversed order (and vis versa), so that the proposed correction (1-2-3 extrinsic convention) is equivalent to the PX4 convention (3-2-1 intrinsic convention) without even the need to swap the order of outputs. To validate this correction: do some pure roll motion, then some pure pitch, then perform a 90 degrees pure yaw rotation and finally, from that new yaw orientation, do again some pure roll and pure pitch motions. You will see that the roll and pitch angles before this correction are wrong, while they are correct after. --- px4tools/ulog.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/px4tools/ulog.py b/px4tools/ulog.py index fc17696..e6a7d68 100644 --- a/px4tools/ulog.py +++ b/px4tools/ulog.py @@ -16,7 +16,7 @@ import pyulog import scipy.signal import transforms3d.quaternions as quat -import transforms3d.taitbryan as tf +import transforms3d.euler as tf # create index to state lookup for estimators EST_NAME = { @@ -486,14 +486,15 @@ def series_quatrot_inverse(x, y, z, q0, q1, q2, q3, rot_name): def series_quat2euler(q0, q1, q2, q3, msg_name): """ - Given pandas series q0-q4, compute series roll, pitch, yaw + Given pandas series q0-q3, compute series roll, pitch, yaw """ - yaw, pitch, roll = np.array([ - tf.quat2euler([q0i, q1i, q2i, q3i]) for - q0i, q1i, q2i, q3i in zip(q0, q1, q2, q3)]).T - yaw = pd.Series(name=msg_name + '__f_yaw', data=yaw, index=q0.index) - pitch = pd.Series(name=msg_name + '__f_pitch', data=pitch, index=q0.index) + roll, pitch, yaw = np.array([ + tf.quat2euler([q0i, q1i, q2i, q3i], axes='sxyz') for + q0i, q1i, q2i, q3i in zip(q0, q1, q2, q3)]).T roll = pd.Series(name=msg_name + '__f_roll', data=roll, index=q0.index) + pitch = pd.Series(name=msg_name + '__f_pitch', data=pitch, index=q0.index) + yaw = pd.Series(name=msg_name + '__f_yaw', data=yaw, index=q0.index) + return roll, pitch, yaw