# How to calculate covariance matrix? I'm trying to use robot_pose_ekf and I have errors:

Covariance speficied for measurement on topic wheelodom is zero


and

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 =  rospy.Time.now()
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 ]

self.imu_pub.publish(self.imu_msg)


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 http://wiki.ros.org/rviz_imu_plugin 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. http://wiki.ros.org/segway_rmp

this->odom_msg.pose.covariance = 0.00001;
this->odom_msg.pose.covariance = 0.00001;
this->odom_msg.pose.covariance = 1000000000000.0;
this->odom_msg.pose.covariance = 1000000000000.0;
this->odom_msg.pose.covariance = 1000000000000.0;
this->odom_msg.pose.covariance = 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, http://manialabs.wordpress.com/2012/08/06/covariance-matrices-with-a-practical-example/

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 ...

edit retag 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?

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.

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.

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.

I use the wheel encoders to track the position and calculate the orientation using http://en.wikipedia.org/wiki/Dead_reckoning I input the calculated orientation to the odom message. Should I add that same value into the imu_data message as well?

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).

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).

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.

Sort by » oldest newest most voted

Regarding your questions about limitations of robot_pose_ekf, this might interest you and others for future reference: http://answers.ros.org/question/39790/robot_pose_ekf-and-gps/

more