经久不衰的卡尔曼滤波器：基础知识及公式推导

作者 | Zeal 编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/578920168

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【全栈】技术交流群

后台回复【领域综述】获取自动驾驶感知定位融合近80篇综述论文！

一、卡尔曼滤波器的应用场景

卡尔曼滤波器之前被广泛用来做动态系统的状态估计、预测。主要的目的就是用来从带噪声的观测量，比如各种传感器的观测（IMU、GPS、里程计等）估计出最优的系统状态（state）。不过要明确强调的是，由于测量都带有噪声，也就是随机性，所以真正准确的状态是无法获知的。

最小二乘法可以从一长串的测量值回归出一个最为匹配的模型。卡尔曼滤波相比于最小二乘法，采用了一种递归的计算方式，也就是每一时刻只需要保存上一时刻的状态。因此可以被用来处理实时任务。

虽然卡尔曼滤波器从美国阿波罗登月计划到如今已有几十年，且目前有更先进的因子图（Factor Graph）算法，但是在很多领域依旧可以看到卡尔曼滤波器的身影，特别是在自动驾驶的各个模块，比如：从IMU的数据（三轴加速度，三轴角速度）计算出运动物体的当前位置。

二、卡尔曼滤波器的两个步骤——宏观认识

卡尔曼滤波包含两个步骤

预测（prediction）—— Dynamic model
更新（correction/measurment update）—— Observation model

所谓预测，就是用一个数学模型，根据当前的传感器输入，直接计算此时系统的状态。可以理解为一个方程的计算就行。

所谓的更新，就是在某些时刻或者每一时刻，获取一些系统的状态输入（可以同样是传感器的值），甚至是预测阶段中同样的传感器的值，将其当作真值，我们将这个值叫做测量值。比较此刻预测的系统状态和测量的系统状态，对预测出的系统状态进行修正，因此也叫测量更新（measurment update）。

三、卡尔曼滤波的几大概念

1、状态向量State Vector

系统的状态向量包含了系统中我们关心的状态变量，比如速度，距离，加速度等。我们用表示系统向量

因为卡尔曼滤波有两个步骤，我们先以每个时刻都进行测量和修正这两个步骤作为讲解。那么，每一时刻的两个步骤的输出对应两个系统向量，一个是预测的系统向量，一个是修正后的系统向量。

ps:大写字母表示矩阵；小写加粗字母表示向量；小写字母表示变量

2、状态方程&&预测方程

状态方程

状态方程描述了将上一时刻的状态向量映射到当前的系统状态

其中，矩阵称为转换矩阵（Transition Matrix），是当前时刻的系统输入，矩阵称为控制矩阵（Control Matrix）反映了系统输入到系统状态的映射关系，是过程噪声，我们假定其符合均值为0，协方差矩阵为的高斯噪声。这里要注意的有几点：

矩阵、都随着时间演进进行更新
从公式1的形式可以看出，卡尔曼滤波的数学建模形式是线性方程，这也是卡尔曼滤波的限制之一。扩展卡尔曼滤波器（Extended Kalman Filter）支持非线性模型。
这里的状态方程是一个“准确”的表达，因为不准确的部分已经放在噪声项中，要跟下面的预测方程区分开来。这个“准确”的表达由于有噪声（随机向量），所以我们没办法使用它作为输出，只能用它来分析、推导。

预测方程

预测方程跟状态方程基本一样，要强调的一点是，噪声是均值为0的高斯噪声，因此最大概率对应的值为0，因此我们在预测状态向量的时候其实不用管最后一项

这里对系统状态向量上面加上一个帽子表示这是一个估计量，也就是我们实际输出的量。其中表示上一时刻执行完步骤二更新后的状态向量，表示当前时候只执行了步骤1预测的状态向量。

理解预测方程

预测方程（公式2）从形式上看只是把状态方程（公式1）去掉了噪声项，但其中包含了很多含义。其中，要时刻记得的是我们把系统状态当作一个随机向量处理，严格的表述需要引入随机过程的相关知识，但其实没太大必要。由于公式（1）中状态向量是一个随机向量，且噪声项是均值为0的高斯噪声，那么状态向量也满足高斯分布，其最大概率的地方就对应这噪声为0的地方，因此我们有理由将这个概率最大处对应的值当作我们的预测。

3、测量方程&&测量估计方程

测量方程

测量方程反映了系统的测量值和系统状态向量之间的关系

其中，是当前时刻的测量值，称为测量矩阵（Measurement Matrix），描述了从系统状态到测量值的转换关系（举一个最简单的关系，系统状态是物体的直线距离，测量值是使用激光笔测出来的光从原点到物体的时间，那么就是光速的倒数）, 是测量噪声，我们假定其符合均值为0，协方差为的高斯噪声。注意这里无论是状态向量还是测量向量都没有加帽子（），表示这是个"准确"的表达式。

测量估计方程

同样，我们如果忽略噪声项，就变成了对此时测量量的估计（注意状态向量的上标和角标）：

4、测量更新方程

上面给出了对测量量的估计，但是现在的情况是，我们有了一个测量值（通过某些测量方式或者传感器数据），这是一个不需要计算得到的量，我们如何用这个测量得出的量来更新我们对状态向量的估计？这就涉及到测量更新方程，这也是卡尔曼滤波里最难的部分，这里先给出更新方程的形式。

其中，叫做卡尔曼增益；是系统测量值的误差协方差矩阵。

四、误差和协方差矩阵

根据公式（1）减去公式（2），我们可以得出误差的协方差矩阵的表达形式

把公式（1）和（2）带入误差表达式，可以推导出系统状态的协方差矩阵的递归表达形式

其中，从第二个等号到第三个等号利用了状态向量和随机噪声的无关性（协方差为0）。因此我们可以得到下面的公式（7）

五、更新方程的推导

现在我们进行到了这样的情况，我们使用预测方程公式（2）得到了时刻的系统状态预测量；同时，我们在这个时刻得到了一个测量值，且根据测量方程，我们认为它与此时的状态向量满足公式（3）：。那么其实我们可以从公式（3）也得到此时系统状态的另一个估计，由公式（3）将移动到等式左侧，可以得到：

这里之所以没有把噪声表示出来，是因为这里将当作一个随机向量处理，其均值为，也就是当前时刻测量得出的量；其方差为（在公式3中给出）

我们的状态方程也给出了状态向量的一个等式

这里用上标1和2表示状态向量的两个来源。要强调的是，这两个等式里面都包含了一个随机噪声，因此、都是随机向量。同时，他们的分布都符合高斯分布。它们的均值和方差分别是：

现在有了两个关于状态向量的概率分布，那接下来的事情就简单了。因为这两个状态向量的来源我们可以认为是独立的，因此他们的联合概率分布是各自概率分布的乘积。重点是，高斯分布的乘积依旧是高斯分布！！！！新的高斯分布的均值和方差有如下表达形式：

用上面的式子代入，可以得到联合概率分布的均值和方差：均值

方差

这个就是更新后的系统状态误差的协方差矩阵，也就是得到下面的式子

六、总结

预测方程

更新方程

往期回顾

Nuscenes SOTA！BEVFormer v2: 通过透视监督使流行的图像Backbones适应BEV识别！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称

自动驾驶之心【知识星球】

想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球（三天内无条件退款），日常分享论文+代码，这里汇聚行业和学术界大佬，前沿技术方向尽在掌握中，期待交流！