Ask Your Question

# How do I compute the covariance matrix for an orientation sensor? [closed]

If I'm writing a driver for an orientation sensor that outputs quaternions, how do I calculate the orientation covariance matrix? I don't even know the units for covariance in the context of the quat's cov matrix (rad^2 maybe?). obviously, I'm lost.

The purpose of the driver (at least in my use-case) is for using it with robot_pose_ekf so I believe I need a valid cov matrix for robot_pose_ekf to do its magic...

edit retag reopen merge delete

## 2 Answers

Sort by » oldest newest most voted Shorter answer: I had to dig through the robot_pose_ekf code, and the default covariance for the 3x3 IMU is just (0.01)^2 degrees converted into radians along the diagonal [so ~0.00017)^2]. You should probably set this to roll pitch yaw covariance in rads^2.

Long answer: the units in here don't look so great. What ROS will fill in as a default is listed as 0.01 degrees/sec. This should be because the robot_pose_ekf only filters on the differences between the last message and the current message. So this might mean that you'll actually need to send in the covariance of your angular velocities to get the best results, as this value does get multiplied by dt^2 later. However, the code does clearly read orientation_covariance and NOT angular_velocity_covariance.

Original question answer: This thesis seems to have laid the math out okay for getting the necessary covariance into RPW from quaternions. It's on page physical page 69 for reference, but I doubt you have your covariance in quaternion space. To get the quaternion covariances, you should be able to just inverse the math there.

more

## Comments

2
Yeah, generally the covariance won't change on an IMU sensor very much. You can pull it from the datasheet of the individual sensors for the most part. So, in ROS, that 0.01 is a pretty solid guess. If your IMU doesn't have that axis, set it to something obscenely high (MAX_DBL works well).
Chad, strangely enough my CH Robotics UM6 IMU sensor does in fact output a cov matrix in quaternion space. I'll attempt to implement the transform that thesis points to. Thanks!
Chad, for your "Long answer" I'm a bit confused... are you saying the robot_pose_ekf is using the orientation cov matrix as if it was the angular velocity cov matrix? That sounds like a bug!
1
It sure does! They really take the change in orientation (new-old) over a certain dt, resulting in an angular velocity. My guess is that you should try the node by just feeding in the approximate covariances of your angular velocities and see if it works well.
1
After much discussion with my roommate, we were a bit dubious as to how "Kalmany" ROS's EKF node really is. It actually seems like it is closer to a sensor blending using a weighted average based on the covariance values you feed in. Covariance on integrated sensors becomes tricky (and incorrect).
Correct. It uses weighted zero-mean Gaussians to merge sensors. However, the covariance does not grow unbounded. As a design choice, the filter they implemented actually calculates dx, dy, dz, dr, dp, dy and then propagates that as "truth". It calculates the differences between messages [next]..
and then uses that as a relative increment from the last message. The only instance where this is not the case is IMU roll and pitch. So the robot_pose_ekf is very exclusive to a PR2-like robot needing better odometry for SLAM/AMCL, but not useful for any robot needing to have absolute odometry.
If one were to really compute the covariances for this method, it would need to be something like [thisMsg - lastMsg]/dt and then apply the appropriate variance propagation techniques. However, as Kalman filtering typically goes, you end up guessing the covariance values until you like the results

I looked into this and found an implementation of a quaternion covariance to RPY covariance in the MIRA project in YawPitchRoll.h (documentation). See quaternionCovToYawPitchRollCov on line 318.

more

## Comments

Funny... I ended up here looking for resources to help me debug exactly that code ;)

## Stats

Asked: 2011-03-16 06:52:10 -0500

Seen: 13,111 times

Last updated: Aug 09 '17