robot_pose_ekf初始化odometry协方差矩阵

      在使用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即可工作。