Ask Your Question
7

How to calculate covariance matrix?

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

pwong gravatar image

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

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

    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] = 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, 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 ... (more)

edit retag flag offensive close merge delete

Comments

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 -0600 )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 -0600 )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 -0600 )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 -0600 )edit

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?

pwong gravatar imagepwong ( 2014-01-21 17:12:01 -0600 )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 -0600 )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 -0600 )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 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
0

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

demmeln gravatar image

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/

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

Stats

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

Seen: 8,968 times

Last updated: Jan 22 '14