1. 如何使用Robot Pose EKF

1.1 配置

EKF节点默认launch文件可以在robot_pose_ekf包中找到，launch文件包含一系列参数：

• freq: 滤波器频率，更高的频率单位时间传递更多传感数据，但不会提高估计的精度。
• sensor_timeout: 当传感器停止向滤波器发送数据，滤波器等待接受数据的时间，超时则在没有此数据的情况下继续滤波器工作。
• odom_used, imu_used, vo_used: 开启或关闭输入。

配置示例如下所示：

 <launch> 
   <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf"> 
    <param name="output_frame" value="odom"/> 
    <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"/> 
    <param name="debug" value="false"/> 
    <param name="self_diagnose" value="false"/> 
   </node> 
</launch>

1.2 运行

Build编译

$ rosdep install robot_pose_ekf
$ roscd robot_pose_ekf
$ rosmake

Run 运行

$ roslaunch robot_pose_ekf.launch

2. 节点

2.1 robot_pose_ekf

robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态

2.1.1 订阅主题

odom（编码器）
(nav_msgs/Odometry)2D pose (轮式里程计): 二维姿态包含机器人平面中的坐标和朝向以及方位协方差。平面机器人中，其中z, roll and pitch 忽略。

imu_data（IMU）
(sensor_msgs/Imu)3D orientation (used by the IMU): 提供包含相对世界坐标系的Roll, Pitch 和 Yaw 角度。 Roll 和 Pitch 角是绝对角度，Yaw是相对角度。协方差矩阵决定了方向的不确定性， robot pose ekf 在仅仅接收此msg时不会工作，它需要 'vo' 或'odom' 主题。

vo（视觉里程计）
(nav_msgs/Odometry)3D pose (used by Visual Odometry): 此主题包含机器人全方向及相应协方差信息，当传感器只测量部分3维信息时，需要制定一个较大的协方差用来忽略此项数据。

The robot_pose_ekf节点不需要三个主题同时有效，每个主题数据都会产生一个位置估计及协方差。主题的频率也不一定一致，也具有不同的延迟。数据间歇收发，这时节点将自动检测使用有效的传感数据。添加自主的传感数据参照GPS的示例即可。 the Adding a GPS sensor tutorial

2.1.2 发布的主题

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)
《滤波器的输出 (the estimated 3D robot pose)》

2.1.3 TF变换

odom_combined → base_footprint

3. Robot Pose EKF如何工作

3.1 位置解析

所有传感器发送的数据依据自己的世界坐标系，每个世界坐标系将随着时间漂移，因此，各个传感器发送的绝对位置无法直接比较，节点使用每个传感器的相对坐标系更新滤波器。

3.2 协方差解析

随着机器人行动，机器人全局位置的不确定性越来越大，协方差也将逐渐增大而无界限。因此在位置本身发布协方差是毫无意义的，取而代之的是传感器数据发布随时间变化的协方差， *注意通过外部传感器观测将减少估计的不确定性，不过这是定位技术而不是轨迹推演。

3.3 Timing时间

假设滤波器上一刻在t_0时间更新，直到某一传感器数据的时间戳在t_0之后获取，滤波器才会工作。. The node will not update the robot pose filter until at least one measurement of each sensor arrived with a timestamp later than t_0. When e.g. a message was 当一个odom传感器接受时间 t_1 > t_0，IMU的数据时间戳在t_2 > t_1 > t_0，在上一刻所有传感器数据都就绪的时间戳滤波器将会更新。本例在 t_1具有odom数据而IMU数据是t_0 到 t_2间线性插入的，滤波器将会更新t_0 到 t_1间的相对odom和IMU数据。

上图所示，PR2在给定的起点（绿色）开始，里程计图展示了整个闭环，蓝色线表示编码器的输入，蓝点表示终点。红线表示输出的滤波数据，它融合的滤波器和IMU，红点是估计的终点。

4. 软件包状态

4.1 Stability稳定性

代码经过长期测试ok，随着ROS的API更改，将来也需要做调整。

4.2 路标

滤波器主要用于PR2中的三种传感器输入(wheel odometry, imu and vo) ，我们计划开发更为通用的版本，将来的版本支持多个 (nav_msgs/Odometry)传感器消息输入。

5. 教程

robot_pose_ekf/Tutorials/AddingGpsSensor