自动驾驶基础之——如何写卡尔曼滤波器？

2018-09-26 23:09:20· 来源：自动驾驶干货铺

由于传感器本身的特性，任何测量结果都是有误差的。以障碍物检测为例，如果直接使用传感器的测量结果，在车辆颠簸时，可能会造成障碍物测量结果的突变，这对无人车的感知来说是不可接受的。因此需要在传感器测量结果的基础上，进行跟踪，以此来保证障碍物的位置、速度等信息不会发生突变。

最经典的跟踪算法莫过于卡尔曼老爷子在1960年提出的卡尔曼滤波器。在无人车领域，卡尔曼滤波器除了应用于障碍物跟踪外，也在车道线跟踪、障碍物预测以及定位等领域大展身手。

在介绍卡尔曼滤波器数学原理之前，先从感性上看一下它的工作原理。简单来讲，卡尔曼滤波器就是根据上一时刻的状态，预测当前时刻的状态，将预测的状态与当前时刻的测量值进行加权，加权后的结果才认为是当前的实际状态，而不是仅仅听信当前的测量值。

初始化（Initialization）

假设有个小车在道路上向右侧匀速运动，我们在左侧安装了一个测量小车距离和速度传感器，传感器每1秒测一次小车的位置s和速度v，如下图所示。

我们用向量xt来表示当前小车的状态，该向量也是最终的输出结果，被称作状态向量（state vector）：

由于测量误差的存在，传感器无法直接获取小车位置的真值，只能获取在真值附近的一个近似值，可以假设测量值在真值附近服从高斯分布。如下图所示，测量值分布在红色区域的左侧或右侧，真值则在红色区域的波峰处。

由于是第一次测量，没有小车的历史信息，我们认为小车在1秒时的状态x与测量值z相等，表示如下：

预测（Prediction）

预测是卡尔曼滤波器中很重要的一步，这一步相当于使用历史信息对未来的位置进行推测。根据第1秒小车的位置和速度，我们可以推测第2秒时，小车所在的位置应该如下图所示。会发现，图中红色区域的范围变大了，这是因为预测时加入了速度估计的噪声，是一个放大不确定性的过程。

根据小车第一秒的状态进行预测，得到预测的状态xpre：

其中，pre是prediction的简称；时间间隔为1秒，所以预测位置为距离+速度*1；由于小车做的是匀速运动，因此速度保持不变。

观测（Measurement）

在第2秒时，传感器对小车的位置做了一次观测，我们认为小车在第2秒时观测值为z2，用向量表示第2秒时的观测结果为：

很显然，第二次观测的结果也是存在误差的，我们将预测的小车位置与实际观测到的小车位置放到一个图上，即可看到：

图中红色区域为预测的小车位置，蓝色区域为第2秒的观测结果。

很显然，这两个结果都在真值附近。为了得到尽可能接近真值的结果，我们将这两个区域的结果进行加权，取加权后的值作为第二秒的状态向量。

为了方便理解，可以将第2秒的状态向量写成：

其中，w1为预测结果的权值，w2为观测结果的权值。两个权值的计算是根据预测结果和观测结果的不确定性来的，这个不确定性就是高斯分布中的方差的大小，方差越大，波形分布越广，不确定性越高，这样一来给的权值就会越低。

加权后的状态向量的分布，可以用下图中绿色区域表示：

你会发现绿色区域的方差比红色区域和蓝色区域的小。这是因为进行加权运算时，需要将两个高斯分布进行乘法运算，得到的新的高斯分布的方差比两个做乘法的高斯分布都小。

两个不那么确定的分布，最终得到了一个相对确定的分布，这是卡尔曼滤波的一直被推崇的原因。

再预测，再观测（Prediction & Measurement）

第1秒的初始化以及第2秒的预测、观测，实现卡尔曼滤波的一个周期。

同样的，我们根据第2秒的状态向量做第3秒的预测，再与第3秒的观测结果进行加权，就得到了第3秒的状态向量；再根据第3秒的状态向量做第4秒的预测，再与第4秒的观测结果进行加权，就得到了第4秒的状态向量。以此往复，就实现了一个真正意义上的卡尔曼滤波器。

以上就是卡尔曼滤波器的感性分析过程，下面我们回归理性，谈谈如何将以上过程写成代码。

分享到：

点赞 0 反对 0 举报 0 收藏 0 评论 0

0 条相关评论

• 亦庄：自动驾驶之城	• 智能网联汽车仿真测试标准体系研究
• 汽车自动驾驶仿真：场景呈现的技术深度探讨	• 汽车自动驾驶感知模块的开发与验证：技术挑战与未来展望
• 汽车自动驾驶感知模块开发与验证中的场景仿真技术探析	• 智能网联不确定路口场景下汽车驾驶性能的实验与评价研究
• 智能网联汽车产品模拟仿真测试可信度评估方法	• 智能网联汽车产品模拟仿真测试可信度评估框架
• 智能网联汽车产品测试策略：确保安全可靠的驾驶体验	• 智能网联汽车视觉感知系统测评验证：性能评估与未来展望

编辑推荐