Ask Your Question

Interpretation of covariance of output of robot_pose_ekf

asked 2011-08-06 21:49:45 -0500

barry gravatar image

I'm confused about the interpretation of the covariance matrix of the output of the EKF filter in the robot_pose_ekf package.

From the documentation (and looking at the code) I understand that the covariances of the inputs are on velocity level. But is this also true for the covariance matrix of the output of the filter?

This is what I'm trying to accomplish. I'm creating a node that listens to the output messages of the EKF filter. When a messages is received I determine the change of pose with respect to the last messages and determine the covariance associated with the change of pose. Here's the code:

void MyNodelet::OdomPoseCallback(
    const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& ap_msg) {

static bool l_firstOdomMsg(true);
static tf::Transform l_lastOdomMeas;
static ros::Time l_lastStamp;

if (l_firstOdomMsg) {
    tf::poseMsgToTF(ap_msg->pose.pose, l_lastOdomMeas);
    l_lastStamp = ap_msg->header.stamp;
    l_firstOdomMsg = false;
} else {
    ros::Time l_stamp = ap_msg->header.stamp;
    tf::Transform l_odomMeas;
    tf::poseMsgToTF(ap_msg->pose.pose, l_odomMeas);
    Eigen::Matrix<double,6,6> l_odomCov;
    for (unsigned int i = 0; i < 6; i++)
        for (unsigned int j = 0; j < 6; j++)
            l_odomCov(i, j) = ap_msg->pose.covariance[6 * i + j];
    double l_dt = (l_stamp-l_lastStamp).toSec();
    ///@todo What's definition of the covariance matrix exactly?
    l_odomCov = l_odomCov * pow(l_dt,2);
    tf::Transform l_relMove = l_lastOdomMeas.inverse()*l_odomMeas;
    l_lastOdomMeas = l_odomMeas;
    l_lastStamp = l_stamp;

    mp_myAlg->UpdateDeadreckoning(l_stamp.toSec(), l_relMove, l_odomCov);

Right now I'm computing the covariance associated with the change in pose by multiplying the covariance from the EKF filter output with the square of the time difference between the current message and the last message. Is this correct?

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted

answered 2011-08-15 10:42:12 -0500

Wim gravatar image

The covariance on odometry is kind of tricky, because as your robot moves, the covariance on its position would keep growing without any bounds. To avoid this problem, the robot_pose_ekf resets the covariance of the filter before every sensor update. So the covariance that gets published, is the increase in covariance over the past time interval. Note that the size of this time interval is not always the same; it depends on how fast and when the sensor measurements arrive at the filter node.

You're correct that the covariances of the inputs are on velocity level.

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


Asked: 2011-08-06 21:49:45 -0500

Seen: 1,095 times

Last updated: Aug 15 '11