Robot_pose_ekf源码笔记

        Robot_pose_ekf是ROS中实现多传感器融合输出机器人位姿的程序包。其基本原理是利用扩展卡尔曼滤波，利用多种传感器实现机器人位姿的最优估计。目前支持轮式里程计、IMU、GPS和视觉里程计4种传感器。官方说明文档：robot_pose_ekf - ROS Wiki

        以下是源码的一些笔记，其中有些不懂的地方也找学长交流过，有的问题解决了，有的还是不太清楚，也都在笔记中标记出来了，欢迎大家一起交流、批评指正！

0、对于整体算法流程的理解

      假设系统在时刻t-1的融合滤波输出为，在时刻t接收到了不同传感器的数据设这些传感器的观测矩阵依次为协方差矩阵依次为那么，按照卡尔曼滤波的步骤，首先执行状态预测：

之后利用第一种传感器进行测量更新：

在得到yt(1)和Σt(1)的基础上，以相同的步骤利用第二种传感器再进行一次测量更新：

之后再融合剩下的第三种、第四种传感器，步骤与之前相同。

    Robot_pose_ekf可以处理4种传感器：轮式里程计、惯性元件（IMU）、视觉里程计和GPS。其中里程计的融合滤波过程会比其他传感器稍微复杂一些，因为程序是取里程计每一个时间段内的相对变化值，而并不是直接将里程计输出的绝对观测量进行测量更新。这一部分在下文里程计的部分会说明。

    不过值得注意的是Robot_pose_ekf虽然确实用到了扩展卡尔曼滤波，但是它对系统的预测模型作出了很大的简化，所以完全可以仅用卡尔曼滤波的思路去理解整个程序。这一部分将在下文的建立系统模型部分会做详细说明。

1、建立模型：

a.建立系统模型

设机器人的空间坐标为，机器人的姿态用欧拉角来描述，机器人线速度为，角速度为。则系统状态变量选择为