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 2:
I got rid of the error messages for the imu. Heres how it looks.
self.imu_msg.header.stamp = rospy.Time.now()
self.imu_msg.header.frame_id = '/base_footprint'
orientation_quat = tf.transformations.quaternion_from_euler(0,0,0)
self.imu_msg.orientation.x = orientation_quat[0] #No orientation available
self.imu_msg.orientation.y = orientation_quat[1]
self.imu_msg.orientation.z = orientation_quat[2]
self.imu_msg.orientation.w = orientation_quat[3]
self.imu_msg.orientation_covariance[0] = -1.0
self.imu_msg.orientation_covariance[4] = 10000000000000.0
self.imu_msg.orientation_covariance[8] = 10000000000000.0
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] = 0.0001
self.imu_msg.angular_velocity_covariance[4] = 0.0001
self.imu_msg.angular_velocity_covariance[8] = 0.0001
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] = 0.0001
self.imu_msg.linear_acceleration_covariance[4] = 0.0001
self.imu_msg.linear_acceleration_covariance[8] = 0.0001
self.imu_pub.publish(self.imu_msg)
If the IMU gave me position for the yaw, I would put it in:
orientation_quat = tf.transformations.quaternion_from_euler(0,0,yaw)
I wasn't able to find the variances/errors from my manual, so the values I input for the covariance matrix are just numbers I picked out...
I ran the ekf node with both the odom and imu message, but the results didn't seem much better than when I was just using the odometry data without the ekf. I'll work on the GPS node next.
Originally posted by pwong on ROS Answers with karma: 447 on 2014-01-19
Post score: 7
Original comments
Comment by demmeln on 2014-01-21:
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?
Comment by pwong on 2014-01-21:
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.
Comment by pwong on 2014-01-21:
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.
Comment by demmeln on 2014-01-21:
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.
Comment by pwong on 2014-01-21:
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?
Comment by demmeln on 2014-01-22:
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).
Comment by demmeln on 2014-01-22:
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).
Comment by pwong on 2014-01-22:
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.
Comment by pwong on 2014-01-22:
I see that on the robot_pose_ekf page, the pr2 was able to get much getter results with just the wheel odometry and imu. Maybe I will just stick with these two sensors and optimize it.
Comment by demmeln on 2014-01-23:
I posted an answer for greater visibility.