0
$\begingroup$

Rosanswers logo

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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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/


Originally posted by demmeln with karma: 4306 on 2014-01-23

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.