手把手教你写扩展卡尔曼滤波器（C++）

手把手教你写扩展卡尔曼滤波器（C++）

正文

附赠自动驾驶最全的学习资料和量产经验：链接

源码在结尾

毫米波雷达的数据

毫米波雷达观察世界的方式与激光雷达有所不同。激光雷达测量的原理是光的直线传播，因此在测量时能直接获得障碍物在笛卡尔坐标系下x方向、y方向和z方向上的距离；而毫米波雷达的原理是多普勒效应，它所测量的数据都是在极坐标系下的。

如下图所示，毫米波雷达能够测量障碍物在极坐标下离雷达的距离ρ、方向角ϕ以及距离的变化率（径向速度）ρ’，如下图所示。

图片出处：优达学城（Udacity）无人驾驶工程师学位

扩展卡尔曼滤波器理论

我们再次祭出卡尔曼老先生给我们留下的宝贵财富，即状态估计时预测和测量值更新时所用到的7个公式，如下图所示。扩展卡尔曼滤波的理论和编程依旧需要使用到这些公式，相比于原生的卡尔曼滤波，只在个别地方有所不同。

图片出处：优达学城（Udacity）无人驾驶工程师学位

完成扩展卡尔曼滤波器C++代码的过程，实际上就是结合上面的公式，一步步完成初始化、预测、观测的过程。由于公式中涉及大量的矩阵转置和求逆运算，我们使用开源的矩阵运算库Eigen辅助我们代码的编写。

代码：初始化（Initialization）

扩展卡尔曼滤波的初始化，需要将各个变量进行设置，对于不同的运动模型，状态向量是不一样的。为了保证代码对不同状态向量的兼容性，我们使用Eigen库中非定长的数据结构。

如下所示，我们新建了一个ExtendedKalmanFilter类，定义了一个叫做x_的状态向量（state vector）。代码中的VerctorXd表示X维的列向量，元素的数据类型为double。

初始化扩展卡尔曼滤波器时需要输入一个初始的状态量x_in，用以表示障碍物最初的位置和速度信息，一般直接使用第一次的测量结果。

代码：预测（Prediction）

完成初始化后，我们开始写Prediction部分的代码。首先是公式

这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x’。这里的F被称作状态转移矩阵（