03-数据融合-ROS轮式机器人数据融合-odom&IM
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)传感器消息输入。
