rbtposeekf的注意事项

1.发布的odom topic以及 imu topic必须加上协方差部分；
2.在发布odom的时候，去掉里面的odom->base_link的tf，因为这个tf会在robot_pose_ekf包里面发布；
3.要发布base_link到imu的静态tf，且imu的静态tf的名称必须与imu的topic的frame ID一致；
eg:<node name="base_imu_link" pkg = "tf" type = "static_transform_publisher" args=" 0 0 0 0 0 0 /base_link /IMU 100"/>
4.记得修改robot_pose_ekf里面的接受话题名称，以及融合后的发布话题robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)更改成类型为odom (nav_msgs/Odometry)；

------------恢复内容开始------------

以上是如何使用，下面分析里面的细节

先看一下原始的代码里面的launch文件的参数：

<launch>

<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
  <param name="output_frame" value="odom_combined"/>
  <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="imu_used" value="true"/>
  <param name="vo_used" value="true"/>

  <remap from="odom" to="pr2_base_odometry/odom" />
</node>

</launch>

View Code

odom(nav_msgs/Odometry):轮式里程计，平面上的位置和方向信息，在实际发送的2D位恣信息表示的是3D位恣，但是z，roll，pitch是直接被忽略。

imu_data(sensor_msgs/Imu):3D的方向信息，提供了小车坐标系相对于世界坐标系的Roll，Pitch，Yaw信息，对于Roll，Pitch由于重力参考系的原因，被解释为绝对角度。Yaw角度是一个相对角度。协方差矩阵说明了方向的不确定性。robotposeekf只有接收到本消息才会开始执行；另外也要接收要么是vo，或者odom消息。

vo(nav_msgs/Odometry):由视觉里程计提供的3D位恣：提供了完整的3D姿态信息，对于轮式里程计部分，只提供了3D姿态的一部分，其他没提供的，必须给出一个大的协方差值。

Robot Pose EKF 是怎样工作的？

1.位恣解释

所有发送到滤波器节点的传感器源都有各自的世界坐标系，这些世界坐标系会随着时间任意漂移。因此，不同传感器发送的绝对位恣不能相互比较。故滤波器节点使用每个传感器的相对位恣的不同来更新EKF。

2. 协方差解释

随着机器人的移动，其不确定性在世界坐标系中会越变越大。随着时间的推移，协方差会增大到没有边界。因此在发布位恣的同时发布协方差是没有用的，相反传感器源发布协方差是怎么随着时间改变的。注意使用对外界的观测信息，会降低对机器人位恣的不确定性；这是定位，不是里程计。

3.时序

假设robot pose filter 上一次更新在时刻t_0.节点不会更新滤波器，直到一个新的传感器测量在晚于t_0时刻到达。例如当里程计在时刻t_1,t_1>t_0到达，以及imu_data在t_2，t_2>t_1>t_0.滤波器现在会更新到最近的时间，即本例中t_1时刻。里程计的值由t_1时刻直接给出，而imu pose在t_1时刻的值由t_0和t_2时刻进行线性插值。滤波器更新用t_0到t_1时刻的相对的odom和imu位恣进行更新。

就单纯这个包而言比较简单，主要关注一下，它的时序关系；对于内部的EKF算法，是参考了BFL库实现的！后面再来分析，

下面去添加激光里程计到robot_pose_ekf中

1.rf2o_laser_odometry

http://wiki.ros.org/rf2o_laser_odometry

rf2o是一个快速且精准的方法，它从连续的激光帧宏估计激光的平面运动。对于每一个扫描的点，构建距离流约束方程，来对产生的几何约束方程进行最小化误差，进而获得运动估计。本方法不是寻找对应关系而是执行基于激光梯度的稠密激光对齐，类似于稠密的3D视觉里程计。最小化问题在一个coarse-to-fine的方式中解决，处理较大的位移，基于估计协方差的平滑滤波器用来处理在无约束场景（走廊）下的不确定性。

~freq (double, default: 10.0) Odometry publication rate (Hz)

只需要把激光里程计发布的topic（nav_msgs/Odometry）名字与robot pose ekf对应需要的topic名字对应上即可，在这里把robot_pose_ekf的vo和这里的激光里程计对应上就可以了。

rf2o_laser_odometry.launch

<launch>

  <node pkg="rf2o_laser_odometry" type="rf2o_laser_odometry_node" name="rf2o_laser_odometry" output="screen">
    <param name="laser_scan_topic" value="/laser_scan"/>        # topic where the lidar scans are being published
    <param name="odom_topic" value="/odom_rf2o" />              # topic where tu publish the odometry estimations可以改成/vo
    <param name="publish_tf" value="false" />                   # wheter or not to publish the tf::transform (base->odom)
    <param name="base_frame_id" value="/base_link"/>            # frame_id (tf) of the mobile robot base. A tf transform from the laser_frame to the base_frame is mandatory
    <param name="odom_frame_id" value="/odom" />                # frame_id (tf) to publish the odometry estimations    
    <param name="init_pose_from_topic" value="/base_pose_ground_truth" /> # (Odom topic) Leave empty to start at point (0,0)
    <param name="freq" value="6.0"/>                            # Execution frequency.
    <param name="verbose" value="true" />                       # verbose
  </node>
  
</launch>

------------恢复内容结束------------