广告
自动驾驶基础之——如何写卡尔曼滤波器？
卡尔曼滤波器的理论
前文用了一个简答的例子对卡尔曼滤波器的整个流程进行了介绍，下面我们根据卡尔曼滤波器的原理，编写代码，跟踪连续的激光雷达点。
在这里就要祭出卡尔曼老先生给我们留下的宝贵财富了，下面7个公式就是卡尔曼滤波器的理性描述，使用下面7个公式，就能够实现一个完整的卡尔曼滤波器。现在看不懂这7个公式没关系，继续往下看，我会一个一个做解释。
写代码（C++）的过程，实际上就是结合上面的公式，一步步完成初始化、预测、观测的过程。由于公式中涉及大量的矩阵转置和求逆运算，我们使用开源的矩阵运算库——Eigen库。
代码：初始化（Initialization）
在Initialization这一步，需要将各个变量初始化，对于不同的运动模型，其状态向量肯定是不一样的，比如前文小车的例子，只需要一个距离s和一个速度v就可以表示小车的状态；再比如在一个2维空间中的点，需要x方向上的距离和速度以及y方向上的距离和速度才能表示，这样的状态方程就有4个变量。
因此我们使用Eigen库中非定长的数据结构，下图中的VerctorXd表示X维的列矩阵，其中的元素数据类型为double。
在这里，我们新建了一个KalmanFilter类，其中定义了一个叫做x_的变量，表示这个卡尔曼滤波器的状态向量。
代码：预测（Prediction）
完成初始化后，我们开始写Prediction部分的代码。首先是公式
这里的x为状态向量，通过左乘一个矩阵F，再加上外部的影响u，得到预测的状态向量x'。这里的F成为状态转移矩阵（state transistion matrix）。以2维的匀速运动为例，这里的x为
对于匀速运动模型，根据中学物理课本中的公式s1 = s0 + vt，经过时间△t后的预测状态向量应该是
由于假设当前运动为匀速运动，加速度为0，加速度不会对预测造成影响，即
如果换成加速或减速运动模型，就可以引入加速度a_x和a_y，根据s1 = s0 + vt + at^2/2这里的u会变成：
作为入门课程，这里不讨论太复杂的模型，因此公式
最终将写成
由于每次做预测时，△t的大小不固定，因此我们专门写一个函数SetF()。
再看预测模块的第二个公式
该公式中P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器中，不确定程度会变小，P的专业术语叫状态协方差矩阵（state covariance matrix）；这里的Q表示过程噪声（process covariance matrix），即无法用x'=Fx+u表示的噪声，比如车辆运动时突然到了上坡，这个影响是无法用之前的状态转移估计的。
以激光雷达为例。激光雷达只能测量点的位置，无法测量点的速度，因此对于激光雷达的协方差矩阵来说，对于位置信息，其测量位置较准，不确定度较低；对于速度信息，不确定度较高。因此可以认为这里的P为：
由于Q对整个系统存在影响，但又不能太确定对系统的影响有多大。工程上，我们一般将Q设置为单位矩阵参与运算，即
根据以上内容和公式
我们就可以写出预测模块的代码了
际编程时x'及P'不需要申请新的内存去存储，使用原有的x和P代替即可。
代码：观测（Measurement）
观测的第一个公式是
这个公式计算的是实际观测到的测量值z与预测值x'之间差值y。不同传感器的测量值一般不同，比如激光雷达测量的位置信号为x方向和y方向上的距离，毫米波雷达测量的是位置和角度信息。因此需要将状态向量左乘一个矩阵H，才能与测量值进行相应的运算，这个H被称为测量矩阵（Measurement Matrix）。
激光雷达的测量值为
其中xm和ym分别表示x方向上的测量（measurement）值。
由于x'是一个4*1的列向量，如果要与一个2*1的列向量z进行减运算，需要左乘一个2*4的矩阵才行，因此整个公式最终要写成：
即，对于激光雷达来说，这里的测量矩阵H为：
求得y值后，对y值乘以一个加权量，再加到原来的预测量上去，就可以得到一个既考虑了测量值，又考虑了预测模型的位置的状态向量了。
那么y的这个权值该如何取呢？
再看接下里的两个公式
这两个公式求的是卡尔曼滤波器中一个很重要的量——卡尔曼增益K（Kalman Gain），用人话讲就是求y值的权值。
公式中的R是测量噪声矩阵（measurement covariance matrix），这个表示的是测量值与真值之间的差值。一般情况下，传感器的厂家会提供该值。S只是为了简化公式，写的一个临时变量，不要太在意。
看最后两个公式
这两个公式，实际上完成了卡尔曼滤波器的闭环，第一个公式是完成了当前状态向量x的更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声，第二个公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的运算，该公式中的I为与状态向量同维度的单位矩阵。
- 下一篇：自动驾驶平台盘点
- 上一篇：解析丰田对自动驾驶汽车的愿景:不完全是无人驾驶的模式
广告
最新资讯
-
国外Rivian和Lucid被认为是特斯拉劲敌，而
2024-05-03 09:26
-
干货丨燃料电池低温特性测试——冷冻解冻循
2024-05-03 09:25
-
特斯拉放弃“一体化压铸”
2024-05-03 08:58
-
纯电动汽车电驱动系统功能安全技术全方位解
2024-05-03 08:54
-
动力电池行业离职率飙升
2024-05-02 17:19
广告
广告
