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

无人驾驶汽车系统感知模块的重要技术——卡尔曼滤波，应用包括：
卡尔曼滤波与行人状态估计
扩展卡尔曼滤波（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的滤波效果就比较差。（即：其不适用于状态量的后验分布和高斯分布差很远的问题）

关键点示意