Interpretation of covariance of output of robot_pose_ekf
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?
