robot_localization中ekf滤波推导过程

robot_localization中ekf滤波推导过程

原文章链接：http://docs.ros.org/en/noetic/api/robot_localization/html/index.html

融合里程计和imu、GPS的robot_localization里面的ekf推导过程，我将其推导完成，并将其记录如下：
15个状态量

状态预测方程： ，非线性，用泰勒展开线性化。
融合原理：将多种传感器的数据融合到里程计坐标系下，坐标系为初始时刻车的方向和原点，即前左上，但是这里的速度x y z，仍然是和沿着车辆方向的。

坐标系确定

欧拉角能用来描述任意旋转，但最有意义的是使用笛卡尔坐标系（右手坐标系）并按照一定顺序所组成的旋转序列。最常用的约定，即所谓“Yaw-pitch-roll”约定，当我们说到旋转的顺序是“Yaw-pitch-roll”时，是指从导航坐标系到载体坐标系，如果从载体坐标系到导航坐标系则相反。若坐标系选择为右手坐标系时，旋转方向与坐标轴指向之间符合右手法则时欧拉角取正，反之取负。
这里从载体坐标系到导航坐标系 roll-pitch-yaw，旋转顺序为X-Y-Z，这和北东地下一样的，那么
由 系到 系的转换过程可描述为：

各次基本旋转（由b系到n系）对应的坐标变换矩阵分别为：

按照上述旋转顺序，三维空间内b系到n系的旋转矩阵（矩阵左乘）为：

根据姿态矩阵的定义， 即为载体的姿态矩阵，其计算结果为：

这个公式和文章《A Generalized Extended Kalman Filter Implementation for the Robot Operating System》一致。可查阅。

姿态角更新方法

姿态角更新有很多方法，这里用了欧拉角法：
载体坐标系下角速率可以表示为：

姿态更新过程如下

线性化求解雅克比矩阵

预测方程是非线性方程，利用泰勒展开将其进行线性化处理。泰勒展开在ekf中的应用原理为


针对robot_localization的方程式线性化过程为：


雅克比矩阵为：



整个过程要结合robot_localization代码一起看，才能更明白