"euler_from_quaternion": when pitch close to ±90°, roll drift? Any solutions?

asked 2019-05-08 14:44:51 -0500

jzyyzr gravatar image

updated 2019-05-08 17:31:46 -0500

jayess gravatar image

I'm working with an IMU to estimate orientation. I provide the raw IMU data to imu_filter_madgwick topic imu/data_raw, it gives me back imu/data which includes a quaternion representing the orientation of the sensor.

Let's say the following code has the quaternion I get from the filter message msg:

quaternion = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)

If I compute euler angles by roll, pitch, yaw = tf.transformations.euler_from_quaternion(quaternion), and print them. I noticed that if the pitch angle is close to ±90°, the roll angle will drift about ±180°. I also noticed that this singularity behavior is mentioned here.

My question is how to solve this problem, and I'm asking because rviz is able to visualize this orientation perfectly without any drifting. So, if I use rviz to visualize the orientation using the same quaternion, rviz doesn't have this problem of roll angle drifts when pitch is close to ±90°. The frame moves smoothly at any roll/pitch/yaw angles.

Below is the code I used for rviz to visualized the same quaternion I used to get euler angles:

quaternion = (msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w)
br = tf.TransformBroadcaster()
br.sendTransform((0,0,0), quaternion, rospy.Time.now(), 'imu', 'world')

Then, I use rviz to display the imu frame w.r.t world frame. There's simply no drifting or whatever.

edit retag flag offensive close merge delete


For future questions/edits, to enable syntax highlighting please don't wrap code/terminal out/etc. in <code> tags, you can use the preformatted text (101010) button. To use it, highlight the text and then click on the preformatted text (101010) button. Also, no need to use <br> tags either. You can just hit Enter to get text to a new line.

jayess gravatar image jayess  ( 2019-05-08 17:36:24 -0500 )edit