ROSNOTE ：ROS中robot_pose_ekf包的使用

原文：https://blog.youkuaiyun.com/wilylcyu/article/details/51891299?locationNum=10
扩展卡尔曼滤波为了估计机器人位姿

订阅的话题
odom (nav_msgs/Odometry)
2D pose (轮式里成绩)：2D位姿包括平面机器人的位置和航向，和相应位姿的协方差。发送这个2D位姿的消息实际表示3D位姿，但是z，roll，pitch被忽略。

imu_data(senser_msgs/Imu)
3D orientation （imu使用）：3D方向提供相对与世界坐标系下的机器人坐标系下的roll，pitch，yaw角。roll和pitch角被描述成绝对角度，并且yaw角被描述成一个相对角。一个协方差矩阵指出方向测量的不确定性。当仅仅接收在这个主题上的消息是，robot pose ekf不会启动；它也期望/odom主题或者/vo主题上的消息。

vo(nav_msgs/Odometry)
3D pose (视觉里程计)：3D位姿表示机器人的全位置和方向，和位姿的协方差。当传感器仅仅检测3D位姿的部分时，简单的指定一个大的协方差在未检测部分。

robot_pose_ekf节点不需要所有的三个传感器源都同时可用。每个源给定一个位姿估计和一个方差。源以不同的频率和不同的延时运行。源可以随着时间消失或者出现，并且节点可以自动检测和使用可用的源。为了添加自己的传感器输入，请看 the Adding a GPS sensor tutorial。
2.1.2 节点所发布的主题

robot_pose_ekf/odom_combined(geometry_msgs/PoseWithCovarianceStamped)
滤波器的输出（估算好的3D机器人位姿）

2.1.3 提供的tf转换

odom_combined → base_footprint

3. Robot Pose EKF 如何工作

3.1 位姿说明

所有发送信息到滤波器节点的传感器源都有它自己的world参考坐标系，并且这些坐标系可以随着时间任意的漂移。因此，由不同传感器发送的绝对坐标系不能彼此比较。节点使用每个传感器的相对位姿差异更新扩展卡尔曼滤波器。

3.2 协方差说明

因为机器人绕圈移动，其在世界坐标系下的位姿的不确定性不断的变得越来越大。随着时间推移，协方差会无界增长。因此在自身位姿发布协方差没有用，代替方法是传感器源发布协方差随着时间的变化情况，也就是，速度上的协方差。注意使用世界的观测将会减少机器人位姿的不确定性；然而这是定位而不是里程计。

3.3 时间的选择

假设机器人位姿滤波器在时刻t_0被最近更新。直到每个传感器的至少一个测量在t_0之后的时间点到达，节点才会更新机器人位姿滤波器。