Each source gives a pose estimate and a covariance. The sources operate at different rates and with different latencies. A source can appear and disappear over time, and the node will automatically detect and use the available sensors

给滤波器节点提供信息的所有传感器源都有自己的参考坐标系，并且随着时间推移都可能出现漂移现象。因此，每个传感器发出来的绝对位姿不能直接对比。

robot_pose_ekf使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器，用松耦合方式融合不同传感器信息实现位姿估计。

在位姿本身上发布协方差是没有用的，而是传感器源发布协方差如何随时间变化，即速度的协方差。使用对世界的观测（例如，测量到已知墙壁的距离）将减少机器人位姿的不确定性；但是这是定位而不是里程计。

准备工作

编译前先安装orocos-bfl

校正好IMU,因为融合后的效果就是要看imu的校准度

注释掉StatusPublisher.cpp中的sendTransform(tf::StampedTransform(transform, current_time.fromSec(base_time_), "odom", "base_footprint")); 因为ekf包会为我们处理好这部分tf，所以不需要我们发布odom变换了。但是保留odom话题的发布

修改 robot_pose_ekf.launch

<launch>
    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
      <param name="output_frame" value="odom_ekf"/>
      <param name="base_footprint_frame" value="base_footprint"/>
      <param name="freq" value="30.0"/>
      <param name="sensor_timeout" value="1.0"/>
      <param name="odom_used" value="true"/>
      <param name="odom_data" value="odom"/>
      <param name="imu_used" value="true"/>
      <param name="imu_data" value="imu"/>
      <param name="vo_used" value="false"/>

      <remap from="/robot_pose_ekf/odom_combined" to="/odom_ekf" />
    </node>
 </launch>

• freq：滤波器更新和发布频率。注意频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高位姿估计的精度。

• sensor_timeout：当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。

• odom_used, imu_used, vo_used：确认是否输入。

启动之后,发布一下tf树,看看各坐标系名称是否正确,而且注意odom_ekf是不是robot_pose_ekf发布的，它提供的tf变换是： odom_ekf --> base_footprint。 我们最终要实现的目标TF关系是： odom_ekf --> base_footprint --> base_link

启动launch: roslaunch robot_pose_ekf robot_pose_ekf.launch

为了查看ekf包是否正常工作，可以用下面代码：

rosservice call /robot_pose_efk/get_status

协方差问题

robot_pose_ekf不输出协方差信息，但要求输入协方差，协方差是on velocity level。接收的odometry数据格式错误的问题，一般是由于底盘发布的odometry数据的协方差矩阵默认为0矩阵。解决的方法由两种：一种是底盘对协方差矩阵进行初始化；另一种方法在robot_pose_ekf中添加判断，如果接收到的odometry信息的协方差矩阵没有进行初始化，则进行初始化。

turtlebot_node对odom的协方差矩阵设置为

ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                        0, 1e-3, 0, 0, 0, 0,
                        0, 0, 1e6, 0, 0, 0,
                        0, 0, 0, 1e6, 0, 0,
                        0, 0, 0, 0, 1e6, 0,
                        0, 0, 0, 0, 0, 1e3]

ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                         0, 1e-3, 0, 0, 0, 0,
                         0, 0, 1e6, 0, 0, 0,
                         0, 0, 0, 1e6, 0, 0,
                         0, 0, 0, 0, 1e6, 0,
                         0, 0, 0, 0, 0, 1e3]

这篇文章也提出了几个例子

注意imu信息的协方差矩阵中代表机器人航向角的分量方差为1e-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3，两个值相差很大。在进行EKF融合时，会更“相信”imu提供的姿态信息，因为其方差更小。比如机器人在转动过程中轮子发生了打滑，用编码器推算出的姿态一直在旋转，而实际姿态（主要由IMU测量得到）却没发生太大变化，这种情况就需要使用信息融合方法来减小误差。 协方差矩阵中的参数设置非常重要，要根据传感器手册或者实际使用测量来确定。

这里的协方差矩阵就是<<概率机器人>>154页的Vt*Mt*VtT, 根据之前速度积分的模型,我们已知的是ωt,Δt,θ

只有a1~a4还未知,它们的说明在《概率机器人》103页， 似乎不对： 这里就涉及到粒子滤波和AMCL了,其实它们就是AMCL的四个参数,即里程计的四个噪声分量. 我们在使用AMCL定位前就要调节这四个参数, 有了这四个参数,就能算出里程计的协方差矩阵了.

随着机器人的移动，其位姿的不确定性越来越大。随着时间的流逝，协方差将无限增长。robot_pose_ekf会在每个传感器更新数据前重置协方差， 所以发布的协方差是the increase in covariance over the past time interval. 这个时间间隔不是不变的，它取决于传感器的测量速度和什么时候完成．

try adjusting the process noise vs. the measurement covariance for the state variables you’re fusing. Lower covariance in the measurement and higher process noise will mean that the filter trusts your sensors more. Otherwise, the filter will prefer to stick with its predictions

里程计消息的协方差平时可以没有,但是如果要用到卡尔曼滤波做融合,就必须有,否则到OdomEstimation::addMeasurement会报错，那里是判断协方差矩阵的对角线元素

[ERROR] [1519539033.600801081]: Covariance specified for measurement on topic wheelodom is zero

但是小强中的里程计协方差矩阵赋值大部分是0

CarOdom.pose.covariance =  boost::assign::list_of(var_len)(0)(0)(0)(0)(0)(0)(var_len)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(999)(0)(0)(0)(0)(0)(0)(var_angle);

因此修改robot_pose_ekf/src/odom_estimation_node.cpp中的函数OdomEstimationNode::odomCallback如下:

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];
 
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0)  // 或者不用 if
{
  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
}
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);

这是仿照源码中的函数imuCallback做的修改

如果协方差太大，那么说明机器人不太依靠里程计．协方差矩阵具体值可以考虑设置为精度的二次方。工程中确保odom的协方差矩阵对角线元素不均为0，则robot_pose_ekf即可工作。

源码的坐标系错误

使用rviz观看滤波后的行走轨迹，结果报错：

它居然要求的是odom坐标系，但是查看tf树，坐标关系是正确的odom_combined --> base_footprint，没有odom了。

使用echo查看/robot_pose_ekf/odom_combined话题，发现header里的frame_id: “odom”，看来是哪个地方发布错了。 从robot_pose_ekf源码里查找，发现OdomEstimation::getEstimate(geometry_msgs::PoseWithCovarianceStamped& estimate)中，有这样一句estimate.header.frame_id = "odom";，按说新的坐标系是odom_combined，不知为什么这里还是odom，只能认为是源码写错了。

参考:
turtlebot所用的里程计和IMU协方差是手动设置的
ROS answer的回答
XiaoXiaoTao博客