使用EKF融合odometry及imu数据

      整理资料发现早前学习robot_pose_ekf的笔记，大抵是一些原理基础的东西加一些自己的理解，可能有不太正确的地方。当时做工程遇到的情况为机器人在一些如光滑的地面上打滑的情形，期望使用EKF利用imu对odom数据进行校正。就结果来看，机器人旋转性能得到改善，前进方向性能没有改善，符合程序原理。若需要提高前进方向的性能，可以考虑加入VO或gps模块。实际工程中应注意odom数据进行协方差矩阵初始化的问题，否则程序会报错。

      一、扩展卡尔曼滤波（EKF）原理

      当前状态的概率的概率分布是关于上一状态和将要执行的控制量的二元函数，再叠加一个高斯噪声，测量值同样是关于当前状态的函数叠加高斯噪声。系统可为非线性系统。

                                                                     

      对g(x_(t-1)，u_t )和h(x_t )泰勒展开，将其线性化，只取一次项为一阶EKF滤波。

                                                  

      g(xt-1,ut )在上一状态估计的最优值μt-1处取一阶导数，h(xt)在当前时刻预测值μ ̅t处取一阶导数，得到Gt和Ht。
      结合通用的Kalman滤波公式如下：
      预测过程：

                                                                          

      更新过程：

                                                                    

      二、卡尔曼滤波的多传感器融合

      多传感器系统模型模型如下：

                                                                  

      和传统的卡尔曼滤波系统模型相比，多传感器系统的观测方程有多个，每个传感器的测量量都可以不同。
      单观测模型系统：

                                                    

      卡尔曼滤波的核心为卡尔曼系数K的选取。对于多传感器系统，y(k)-Cx ̅(k)无法直接计算。
      多传感器信息融合：
      状态预测方程依然为：

                                                                    

      第一个传感器的观测方程更新后得到系统的状态量x(k)及系统协方差矩阵Σ(k)。将二者作为下一个传感器更新过程的系统预测状态量x ̅(k)和系统预测协方差矩阵Σ ̅(k)进行状态更新。将最后一个传感器更新后得到的系统的状态量x(k)及系统协方差矩阵Σ(k)作为融合后输出，并将二者用于预测过程进行下一时刻的迭代。
      扩展卡尔曼滤波工具包robot_pose_ekf

      基于里程计系统建模：

      系统状态量：

      系统输入：

      系统方程：

                       

                                       

      过程激励协方差矩阵根据实际情况设定。同时在初始时刻，系统状态及系统协方差矩阵需要进行初始化。
      观测系统模型：
      通过topic信息分别得到轮式里程计（ODOMETRY）、IMU、视觉里程计(VO)、GPS各观测值和观测协方差矩阵，运用多传感器信息融合的原理对系统状态进行更新。
      其中，ODOMETRY的观测量为[x,y,z,pitch,roll,raw]^T，观测矩阵为：
[1 0 0 0 0 0;0 1 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0;0 0 0 0 0 0 1]
      IMU的观测量为：[pitch,roll,raw]^T，观测矩阵为：
[0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 1]
      VO的观测量为[x,y,z,pitch,roll,raw]^T，观测矩阵为：
[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;0 0 0 1 0 0;0 0 0 0 1 0;0 0 0 0 0 0 1]
      GPS的观测量为[x,y,z]^T，观测矩阵为：

[1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0]

      三、程序流程（默认系统模型及各测量模型已建立完毕）：

      ①开始监听ODOMETRY、IMU、VO及GPS信息；

      ②构建扩展卡尔曼滤波器，具体可参考：https://blog.csdn.net/zhxue_11/article/details/83822625，扩展卡尔曼滤波器的非线性模型为上文提到的里程计模型；

      ③开始更新，起始只引入系统噪声；

      ④获取ODOMETRY信息作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；

      ⑤获取IMU信息作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；

      ⑥获取VO信息作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；

      ⑦获取GPS信息作为观测量及观测协方差矩阵，进行状态更新，得到更新后的系统状态及系统协方差矩阵；

      ⑧发布更新后的状态量及协方差矩阵作为融合后的信息odom_combined。

      四、robot_pose_ekf包使用方法

      1、参数调整（odom_estimation_node.cpp中）：

           ①设定系统过程激励协方差矩阵；

           ②初始化设定系统协方差矩阵；

       2、robot_pose_ekf.launch文件的设置：

           ①将”base_footprint”替换为自身机器人坐标系；

           ②EKF发布的频率为30Hz；

           ③传感器超时设置为1s；

  ④启动轮式例程计信息作为观测量，”imu_used”，”vo_used”，”gps_used”同理；

           ⑤将”pr2_base_odometry/odom”替换为自身发布的odom的topic名称，该映射实现对自身机器人信息的监听，”imu”，”vo”，”gps”同理，注意其对应声明分别为“imu_data”、“vo”、“gps”。

      五、robot_pose_ekf官方文档

      http://wiki.ros.org/robot_pose_ekf?distro=kinetic

      http://wiki.ros.org/robot_pose_ekf/Tutorials/AddingGpsSensor