ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to calculate covariance matrix?

asked 2014-01-19 15:40:18 -0500

pwong gravatar image

updated 2014-01-21 12:54:32 -0500

I'm trying to use robot_pose_ekf and I have errors:

Covariance speficied for measurement on topic wheelodom is zero


filter time older than odom message buffer

I've wrote the odometry node and it works fine. It is true that I don't have a covariance matrix for it. I've looked at other ROS answers threads and read the wiki page, but I don't quite understand it or how to calculate the values for it.

For example, I've attempted to populate an IMU message:

    self.imu_msg.header.stamp =
    self.imu_msg.header.frame_id =  '/base'
    self.imu_msg.orientation_covariance = [-1,   0,   0,
                                            0,  -1,   0,
                                            0,   0,  -1 ] #sensor doesn't have orientation
    self.imu_msg.angular_velocity.x = data[x_rps] #sensor outputs in rad/s
    self.imu_msg.angular_velocity.y = data[y_rps]
    self.imu_msg.angular_velocity.z = data[z_rps]
    self.imu_msg.angular_velocity_covariance = [ 0, -1, 0,
                                                 0, 0, 0,
                                                 0, 0, 0 ]

    self.imu_msg.linear_acceleration.x = 9.8 * data[x_g] #my sensor output in g's
    self.imu_msg.linear_acceleration.y = 9.8 * data[y_g] 
    self.imu_msg.linear_acceleration.z = 9.8 
    self.imu_msg.linear_acceleration_covariance = [ 0, -1, 0,
                                                    0, 0, 0,
                                                    0, 0, 0 ]


The documentation said fill in a -1 for the first element if I don't know how to populate it. I've tried visualizing it in rviz using the beginner tutorial plug-in, but it just flashes large slabs of purple for me. Using the also had a problem compiling(something about the property_manager.h file not existing).

Is that that imu message correct? How would I go calculate it for odom? I'll also have a covariance matrix for the navsat message.

Edit 1: I'm working through adding a covariance matrix for odom, imu, and gps. I'm currently using a segway rmp unit, so I was able to pull this from the segway_rmp's covariance values from that source code.

this->odom_msg.pose.covariance[0] = 0.00001;
this->odom_msg.pose.covariance[7] = 0.00001;
this->odom_msg.pose.covariance[14] = 1000000000000.0;
this->odom_msg.pose.covariance[21] = 1000000000000.0;
this->odom_msg.pose.covariance[28] = 1000000000000.0;
this->odom_msg.pose.covariance[35] = 0.001;

This got rid of the errors for the ekf node. Although, I have no idea why the twist covariance matrix didn't need to be filled. I would still want to know how to find it for my own set up. According to this explanation of covariances,

It looks like all we care about in the matrix is the error of a single variable, so x-axis to x-axis, roll-to-roll etc... So in that segway_rmp code, we only care about x, y, and yaw, which makes sense.

I did that for my imu node but I still get the errors

MSG to TF:  Quaternion Not Properly Normalized
filter time older than imu message buffer

I'll update this as I go!

Edit ... (more)

edit retag flag offensive close merge delete


Refering to the error: "MSG to TF: Quaternion Not Properly Normalized": Make sure that the quaternion inside the pose of the the odometry message is valid (i.e. not 0 0 0 0). How do you set this orientation?

demmeln gravatar image demmeln  ( 2014-01-21 11:58:20 -0500 )edit

I've tried a few values for it. I saw on another post that the sum of the squared values is equal to 1. So I put in 0.0625 for eachc x,y,z,w but it didn't work out either, I got the same 'MSG to TF' warning. I don't know why I would need fill out if I indicated that my sensor doesn't do orientation.

pwong gravatar image pwong  ( 2014-01-21 12:59:01 -0500 )edit

One other thing, even though I set the 0th element of my node to -1, I wasn't able to leave the rest of the covariance matrix empty. It would still throw errors at me.

pwong gravatar image pwong  ( 2014-01-21 13:00:27 -0500 )edit

Your odometry gives no orientation? AFAIK robot_pose_ekf isnt as general as it might seem. It was written for a specific scenario (pr2) and does not necessarily work for all configurations of input.

demmeln gravatar image demmeln  ( 2014-01-21 14:17:05 -0500 )edit

I use the wheel encoders to track the position and calculate the orientation using I input the calculated orientation to the odom message. Should I add that same value into the imu_data message as well?

pwong gravatar image pwong  ( 2014-01-21 17:12:01 -0500 )edit

Ah ok, I thought that invalid quaternion came from odometry. I guess you should not put it into the IMU. For your reference a simple valid quaternion to remember is 0 0 0 1 for x y z w. This is the identity rotation (i.e. 0 degrees along all axes).

demmeln gravatar image demmeln  ( 2014-01-22 14:54:37 -0500 )edit

Maybe you should look into the code of robot_pose_ekf to find out exactly which bits of odom and IMU message are used under which circumstances (i.e. if the 'deactivation' with -1 for covariance is respected).

demmeln gravatar image demmeln  ( 2014-01-22 14:55:36 -0500 )edit

I will definitely go do that! I realized that adding the GPS isn't going to work, after reading a few other threads. It makes sense because of the global frame/error from the GPS. As you mentioned before, what is the specific use case for robot_pose_ekf? I wasn't able to find that anywhere.

pwong gravatar image pwong  ( 2014-01-22 17:56:17 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2014-01-22 23:59:31 -0500

demmeln gravatar image

Regarding your questions about limitations of robot_pose_ekf, this might interest you and others for future reference:

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2014-01-19 15:40:18 -0500

Seen: 11,870 times

Last updated: Jan 22 '14