robot_pose_ekf
robot_pose_ekf实现了用于确定机器人姿势的扩展卡尔曼滤波。
配置
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>订阅主题
odom (nav_msgs/Odometry)
- 2D姿势（由车轮测距法使用）：2D姿势包含机器人在地平面中的位置和方向以及此姿势的协方差。发送此2D姿势的消息实际上代表3D姿势，但是简单地忽略z，roll和pitch。
imu_data（sensor_msgs / Imu）
- 3D方向（由IMU使用）：3D方向提供关于机器人基础框架相对于世界参考框架的滚动，俯仰和偏转角度的信息。Roll和Pitch角度被解释为绝对角度（因为IMU传感器具有重力参考），并且偏航角度被解释为相对角度。协方差矩阵指定方向测量的不确定性。当机器人姿势ekf仅收到关于该主题的消息时，它将不会启动; 它还期望有关'vo'或'odom'主题的消息。
- 3D姿态（由视觉测距使用）：3D姿势表示机器人的完整位置和方向以及此姿势的协方差。当传感器仅测量3D姿势的一部分时（例如，车轮测距法仅测量2D姿势），只需在3D姿势的未实际测量的部分上指定大的协方差。
发布主题
robot_pose_ekf / odom_combined（geometry_msgs / PoseWithCovarianceStamped）
EKF工作原理
1.位姿说明
所有发送信息到滤波器节点的传感器源都有自己的世界参考坐标系，并且这些坐标系可以随着时间任意的漂移。因此，由不同传感器发送的绝对坐标系不能相互比较。节点使用每个传感器的相对位姿差异更新扩展卡尔曼滤波器。
2.协方差说明
因为机器人绕圈移动，其在世界坐标系下的位姿的不确定性不断的变得越来越大。随着时间推移，协方差会无界增长。因此在自身位姿发布协方差没有用，代替方法是传感器源发布协方差随着时间的变化情况，也就是，速度上的协方差。注意使用世界的观测将会减少机器人位姿的不确定性；然而这是定位而不是里程计。
3.定时
想象一下，机器人姿势过滤器最后一次更新时间为t_0。节点将不会更新机器人姿势过滤器，直到每个传感器的至少一次测量到达时间戳晚于t_0。当收到在例如消息奥多姆与时间戳T -1> T_0话题，并在imu_data时间戳话题T_2> T_1> T_0，过滤器现在将更新到其所有传感器的信息是可用的，在这种情况下，最晚时间到时间t_1。直接给出t_1处的odom姿势，并且通过t_0和t_2之间的imu姿势的线性插值获得t_1处的imu姿势。机器人姿势过滤器使用odom和imu的相对姿势在t_0和t_1之间更新。
上图显示了当PR2机器人从给定的初始位置（绿点）开始，被驱动并返回到初始位置时的实验结果。一个完美的odometry xy图应该显示一个精确的循环闭合。蓝线显示车轮里程表的输入，蓝点表示估计的终点位置。红线显示robot_pose_ekf的输出，其结合了车轮里程计和imu的信息，红点表示估计的结束位置。