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 imagedemmeln ( 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 imagepwong ( 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 imagepwong ( 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 imagedemmeln ( 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 imagepwong ( 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 imagedemmeln ( 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 imagedemmeln ( 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 imagepwong ( 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

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


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

Seen: 8,365 times

Last updated: Jan 22 '14