卡尔曼滤波，达官显贵的“护城河”

显示全部楼层 · 2022-6-20 23:09:47

卡尔曼滤波算法是自动驾驶公司中一条著名的护城河，将公司分成了河内的达官显贵（算法、开发）和河外的劳苦大众（产品、项目、运营）。达官显贵嘲笑劳苦大众年轻气盛，有勇无谋，劳苦大众羡慕达官显贵对酒当歌、人生几何。

达官显贵为了巩固自己的地位，在卡尔曼滤波算法护城河之后，又不断修筑起粒子滤波、图优化、深度学习等里三层外三层的皇宫大院，从气势上已经吓退了期望通过科举考试跻身达官显贵阶层的劳苦大众。但是护城河越深、护城墙越高，就越激发劳动大众对城内生活的向往和追求。

笔者作为劳苦大众中的一员，也无时无刻不在思索着如何乔装打扮混入城内，一睹河畔灯红酒绿的生活。但在一切投机取巧的办法失败后，作者决定发挥愚公移山和肖生克越狱的精神，勤勤恳恳挖一条越过护城河的隧道。自动驾驶圈黑话第二十一期，作者就将挖隧道的心得分享给劳苦大众，首先需要跨越的便是古老但又赫赫有名的卡尔曼滤波算法护城河。

诞生背景

听名字就知道又是一种以名字而命名的算法，而这个人的全名就是鲁道夫·卡尔曼（Rudolf E.Kalman）。1930年，卡尔曼出生在一个犹太人家庭。1957年，从哥伦比亚大学博士毕业后，先在IBM干了一年技术员，随即加入到一位著名数学家创办的工作室做数学研究，这一干就是6年。在这6年间，卡尔曼悬梁刺股、挑灯夜战，在前人工作基础上、团队成员协作下，发明了在专业工程师圈子熟知的卡尔曼滤波算法。

但是刚出道的卡尔曼滤波算法并没有迎来学术界的聚光灯，反而遭遇学术界九代长老们的非议和质疑，这像极了历史上其它著名理论的出道遭遇。所幸的是，卡尔曼在三十而立之年的时候，遇到了伯乐，时任NASA动力分析处主任的斯坦尼·施密特（Stanley F. Schmidt），一位主导人类历史上第一个登月计划中导航项目的男人。

施密特当时遇到的难题是：飞船从陀螺仪、加速度计和雷达等传感器上获取的测量数据充满了不确定性误差和随机噪声。当飞船飞向月球表面时，如果不能很好地处理这些误差和噪声，产生的危害将是致命的。施密特尝试了时下各种滤波算法后，效果均不理想，直到一次偶然的机会听闻卡尔曼有个很厉害的新滤波算法。

施密特随即邀请卡尔曼访问NASA，双方一见钟情，眉来眼去之后便确认通过卡尔曼滤波算法可以对飞船所处的位置和速度做出精确估计，同时还可以确定这些估计在统计意义上的好坏程度。随后，两人爱情的结晶成功助力阿波罗飞船登月。卡尔曼滤波算法自此一战成名，直到今天，还一直引领控制理论和系统科学的主流。卡尔曼自此也开始成为各种荣誉和奖项的收割机。

《麻省理工学院技术评论》在一篇纪念卡尔曼先生的文章中提到道：“卡尔曼最重要的发明是卡尔曼滤波算法，该算法成就了过去50年间的许多基本技术，如把阿波罗号宇航员送上月球的航天计算机系统、把人类送去探索深海和星球的机器人载体，以及几乎所有需要从噪声数据去估算实际状态的项目。有人甚至把包括环绕地球的卫星系统、卫星地面站及各类计算机系统在内的整个GPS系统统称为一个巨大无比的卡尔曼滤波器。”

卡尔曼滤波，达官显贵的“护城河”-1.jpg

原理的直观理解

目前可以看到的介绍卡尔曼滤波算法文章，上来不是正态分布，就是协方差矩阵，突然感觉回到了大学本科概率统计的课堂。知道老师介绍的字面意思是什么，但无法领会这东西可以解决工程中的什么问题。这似乎是大学所有基础类课程的通病，只学会了公式的推导过程，求解方法，但没有领悟这公式可以带来的“人民币”价值。

直到偶然机会看到一则通俗介绍卡尔滤波算法的回答，备受启发，能让一个算法之外的自动驾驶从业人员快速了解其原理。本文于是借鉴其表达思路，也通过一个朴素而不普通的实例来直观的介绍卡尔曼滤波算法在融合定位领域的一个作用。

我们假设一辆在公路上行驶的汽车，起点位置A，下一秒汽车驶入林荫道中的B点，再下一秒汽车驶出林荫道来到C点。这辆汽车上安装有IMU和GNSS两种定位传感器。起点A的位置和速度假设是已知的，距离惯性坐标系X轴原点的距离为2m，速度为Vt-1。当车辆驶入到B点时，一方面我们可以根据IMU测量得到汽车本体的三轴加速度和三轴角速度，结合初始速度Vt-1，便可以计算得到B点相对于A点在X轴方向行驶距离的估计值，这里我们假设估计值为10m。

卡尔曼滤波，达官显贵的“护城河”-2.jpg

另一方面，通过GNSS我们可以直接测量出来B点的经纬度，通过坐标转换之后可以直接获得B点在X轴方向行驶的观测值，这里我们假设观测值为13m。问题来了，距离惯性坐标系点的距离现在有估计值（12m=10m+2m）和观测值（13m）两个值，且两个值不一致，我们该如何确定B点的准确估计值呢？

众所周知，IMU测量汽车本体的加速度和角速度过程中，存在误差和噪声，GNSS通过卫星信号定位车辆经纬度的过程中也存在误差和噪声。两种传感器给出的值都是一种概率最大，意思是在这个位置的概率最大，其它位置不是不可能只是概率较小。而在卡尔曼滤波算法的理论中，无论是汽车上一秒的位置，IMU的测量数据，还是GNSS的直接观测值均被认为服从正态分布。

正态分布是一种概率分布，一般表示为N(均值、方差)。服从正态分布的随机变量取与均值邻近的值的概率大，取与均值越远的值的概率越小。同时方差越小，分布越集中在均值附近，方差越大，分布越分散。

这里我们假设A点位置Xt-1服从N(2,0.22)，如下图所示。从图中可以看出2m处纵坐标最大，概率最大，其它取值概率均小于此处。方差0.22则代表A点位置的误差水平。

卡尔曼滤波，达官显贵的“护城河”-3.jpg

而对于IMU和GNSS这两种传感器来说，他们的噪声方差是可以测量的，在使用时是已知的。这里我们省略移动模型建模过程，直接假设基于IMU获得B点相对于A点的距离估计值XI服从N(10,0.12) ，方差0.12则代表IMU噪声的误差水平，此误差水平受IMU的精度，测量的累积时长有关。

GNSS通过卫星信号获得B点相对于原点的测量值XG服从N(13,0.42) ，方差0.42代表GNSS噪声的误差水平。此误差水平一方面受GNSS里卫星板卡的精度水平，另一方面主要受所处的环境有关，是否有遮挡，是否多金属环境等。由于B点处于林荫道下，卫星信号时有时无，因此此时0.42的误差水平还是比较高的。

现在我们有了两组数据，一组是B点相对于原点0的估计值XBOI = XI + XA = N(10,0.12) + N(2,0.22) = N(12,0.12 +0.22)，一组是B点相对于原点0的观测值XG = N(13,0.42)。全文的高潮点来了，我该如何从两个概率分布中找到最准确的那个估计值呢，可以想象的是这个准确的估计值也遵从正态分布，这个正态分布的均值就是我们苦苦追寻的值。卡尔曼给出的答案是，直接将两个概率分布相乘，即N(12,0.12 +0.22) * N(13,0.42)。

卡尔曼滤波，达官显贵的“护城河”-4.jpg

正态分布的乘法公式如上，将N(12,0.12 +0.22) * N(13,0.42)中的值带入下面公式，则可以得到如下正态分布。B点准确估计值就是此正态分布的均值12.23m,此准确估计值的误差水平为0.192。0.222/(0.222+0.42)这个参数在卡尔曼滤波算法理论中被称为卡尔曼增益。

卡尔曼滤波，达官显贵的“护城河”-5.jpg

B点的（12.23,0.192）又将作为计算C点位置的初始值，重复上述过程。

算法的理论分析

卡尔曼滤波算法是那种理解起来很难，但用起来非常简单的算法。整个算法只有五个公式，公式的详细推导对于绝大多数人来说没有必要也没有兴趣了解。因此，本段直接给出推导后的预测方程和更新方程，并简单解释变量和方程的含义。

一、预测方程

xk-1为系统上一时刻状态向量，这个状态向量可以包含任何需要跟踪的信号，且状态向量中的每个变量都服从正态分布。在上文直观理解车辆运动例子中，这个状态变量包含速度和位置。

我们采用预测矩阵F,来刻画上一时刻状态到当前时刻状态的关系。同时考虑到外部因素会带来一些与系统自身状态没有相关性的改变，我们引入外部控制量ut-1及系统参数B。至此，一个考虑了自身和外部因素的状态预测方程便诞生了，如下式所示。

卡尔曼滤波，达官显贵的“护城河”-6.png

在车辆运动例子中，上一时刻的状态变量中速度和位置变量是存在一定关系的，速度大，运行到当前时刻时，车辆就会行驶得更远。但在其它的系统中，我们可能无法仅凭肉眼就直观的看到变量间的依赖关系，而这也是卡尔曼滤波算法的核心目的，从不确定的系统中，尽可能的挖掘确定的信息。

卡尔曼滤波算法在这里使用的是协方差矩阵Pk-1来衡量上一时刻状态变量中每个变量的相关程度。同时将没有跟踪到的噪音干扰当作协方差为Q的噪音来影响，由此我们可以获得当前时刻考虑外部因素的协方差矩阵，如下式所示。

卡尔曼滤波，达官显贵的“护城河”-7.jpg

二、更新方程

通过上文的预测过程，我们对系统当前时刻的状态有了一个模糊的估计。与此同时，我们也将通过传感器测量到当前时刻系统状态的观测值zk及其测量的不确定性R。对于测量量，我们还需要一个变换矩阵H，来将系统真实状态空间映射成观测空间。随后再经过艰苦卓绝的推导便可以得到如下的状态更新方程组。

8 j+ q9 l! ~8 X- Q- C
- B c3 a: K' @7 k
自动驾驶中的应用

卡尔曼滤波算法在自动驾驶的经典应用之一就是融合定位。L4/L5自动驾驶系统基本标配GNSS和IMU。GNSS定位数据更新频率低（典型10Hz），有遮挡或多径影响下，输出的定位数据不可靠。IMU定位数据更新频率高（典型150Hz）,但是内部积分运算会随着时间的推移，产生较大的累计误差。

而为了获得全局准确的定位数据，各家利用GNSS可靠的定位数据来对IMU进行校准，消除IMU的累计误差。同时在GNSS定位不可靠时，利用IMU自身推算结果优化GNSS定位不可靠的数据。而从GNSS/IMU的不确定信息中获得更多确定性信息的手段就是利用的卡尔曼滤波算法。

而在障碍物追踪和预测方面，卡尔曼滤波算法广泛被用于通过融合激光雷达和毫米波雷达的数据，估计障碍物的位置和速度。而在车道线追踪和预测方面，卡尔曼滤波算法可以通过对当前图像的预测来估计下一时刻车道线的位置，并且能识别判断出车道线的走向(左转或右转)，提取有效的道路信息进行车道线的跟踪。

小结

在深度学习口号喊得震天响的今天，笔者不得不戳穿的一个真相便是，卡尔曼滤波算法可能仍是多数自动驾驶公司的当家花旦。这个朴素而不普通的算法，仅凭五个公式便可解决极其复杂的问题，没有理由不受到工程技术人员的拥戴。有时为了追求技术先进性而一味的盲目从众，消耗的还是自己本身。