odom协方差初始化

在使用robot_pose_ekf时，常遇到接收到的odometry数据格式错误的问题。一个可能的原因为底盘或其他设备发布odometry数据的协方差矩阵默认为0矩阵。解决的方法由两种：一种为在底盘将信息封装发布前对协方差矩阵进行初始化；另一种方法为在robot_pose_ekf中添加判断，如果接收到的odometry信息的协方差矩阵没有进行初始化，则进行初始化。
做工程时采用了第二种方法。初始化位于odom_estimation_node.cpp中：
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
{
…
odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
for (unsigned int i=0; i<6; i++)
for (unsigned int j=0; j<6; j++)
odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
if (odom_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
measNoiseOdom_Cov(1,1) = pow(0.01221,2); // = 0.01221 meters / sec
measNoiseOdom_Cov(2,2) = pow(0.01221,2); // = 0.01221 meters / sec
measNoiseOdom_Cov(3,3) = pow(0.01221,2); // = 0.01221 meters / sec
measNoiseOdom_Cov(4,4) = pow(0.007175,2); // = 0.41 degrees / sec
measNoiseOdom_Cov(5,5) = pow(0.007175,2); // = 0.41 degrees / sec
measNoiseOdom_Cov(6,6) = pow(0.007175,2); // = 0.41 degrees / sec
odom_covariance_ = measNoiseOdom_Cov;
}
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, “wheelodom”), odom_covariance_);
…
}

由于对于我手头情况而言odometry确定协方差矩阵为0，故判断条件设得较为简单：odom_covariance_(1,1) == 0.0。判断条件应根据实际情况设定。协方差矩阵具体值可以考虑设置为精度的二次方。

关于卡尔曼滤波中协方差矩阵的设定没能深入学习理解，感觉较为复杂。但在做工程中确保odom的协方差矩阵对角线元素不均0则robot_pose_ekf即可工作。