卡尔曼滤波及其无人驾驶应用卡尔曼滤波及其无人驾驶应用

无人驾驶汽车系统感知模块的重要技术——卡尔曼滤波，应用包括：
卡尔曼滤波与行人状态估计
扩展卡尔曼滤波（EKF）与传感器融合
处理模型，无损卡尔曼滤波（UKF）与车辆状态轨迹

KF卡尔曼滤波卡尔曼滤波器是基于贝叶斯法则的，且相关不确定性的概率分布是符合高斯分布，

文章图片
基于贝叶斯滤波的卡尔曼滤波器
在应用卡尔曼滤波器时，要记住其应用的核心前提：线性模型

文章图片
三个重要假设
参考及简化卡尔曼滤波方程如下

文章图片
卡尔曼滤波方程

变量定义说明：
- 状态向量
- 状态转移矩阵
- 误差协方差矩阵
- 过程噪声协方差矩阵
- 测量噪音协方差矩阵
- 用于计算卡尔曼增益的中间矩阵
- 卡尔曼增益
? - 预测状态和测量状态之差
- 测量向量（激光雷达数据、雷达数据等）
- 单位矩阵

EKF扩展卡尔曼滤波简介
EKF扩展卡尔曼滤波要解决的是卡尔曼滤波不适用于非线性模型的问题。其和卡尔曼滤波算法结构相同，只是将非线性模型线性化，然后再应用卡尔曼滤波完成状态估计。
将非线性模型线性化的数学方法为泰勒展开（前提条件是非线性函数可微分），将非线性函数展开成泰勒级数，并略去二阶及以上项，得到近似的线性模型。

文章图片
泰勒一阶展开

文章图片
泰勒一阶开展说明
非线性的转换方程和线性化泰勒一阶展开，对比EKF和KF方程如下

文章图片
EKF和KF方程对比所谓有得有失，扩展卡尔曼滤波解决了非线性的应用问题，同样带了计算量问题，同时扩展卡尔曼滤波仍存在一定局限性，如下

文章图片
EKF的缺点 EKF应用实例——Radar
【卡尔曼滤波及其无人驾驶应用】因RADAR测量到的数据对应的为极坐标系，其和二维坐标系的转换是一个非线性的过程，所以H相关的测量数据和状态转换方程是非线性的。

The predicted measurement vector x′ is a vector containing values in the form [px?,py?,vx?,vy??]（状态矩阵为二维的坐标和速度）. The radar sensor will output values in polar coordinates（极坐标的表示）:

文章图片
极坐标
h可以表示为：

文章图片
极坐标转换二维直角坐标
Jacobian 雅可比矩阵的求解（注意编程时分母不能为0）：

文章图片
雅可比矩阵求解最后将和带入EKF方程

文章图片
EKF和KF简化方程对比说明
The main differences are:
the F matrix will be replaced by Fj? when calculating P′.
the H matrix in the Kalman filter will be replaced by the Jacobian matrix Hj? when calculating S, K, and P.
to calculate x′, the prediction update function, f, is used instead of the F matrix.
to calculate y, the h function is used instead of the H matrix.
For this project, however, we do not need to use the f function or Fj?. If we had been using a non-linear model in the prediction step, we would need to replace the F matrix with its Jacobian, Fj?. However, we are using a linear model for the prediction step. So, for the prediction step, we can still use the regular Kalman filter equations and the F matrix rather than the extended Kalman filter equations.
One important point to reiterate is that the equation y=z?Hx′ for the Kalman filter does not become y=z?Hj?x for the extended Kalman filter.
UKF无迹卡尔曼滤波核心思想：近似非线性函数的概率分布比近似任意一个非线性函数本身要容易；UKF只是将非线性函数映射通过关键点的映射来实现。
涉及知识：UT变换
（个人理解：选取概率分布中具有代表意义的sigma点，对sigma点进行非线性转换，然后将转换后的点用高斯分布近似表达出来）
如果状态量的后验分布近似高斯分布，那么UKF很高效，相比EKF，UKF的优点：精度高；不需要计算雅克比矩阵
缺点：如果状态量的后验分布和高斯分布差很远，UKF的滤波效果就比较差。（即：其不适用于状态量的后验分布和高斯分布差很远的问题）