图解卡尔曼滤波：核心原理与预测更新步骤通俗推导

卡尔曼滤波确实是一项令人惊叹的技术。但很多软件工程师和科学家对它仍然很陌生，实在可惜。它是一种能从不确定性中融合信息的强大工具，提取精确状态的能力看似不可思议。

1. 什么是卡尔曼滤波？

你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测。即便伴随着各种干扰，卡尔曼滤波总能指出真实发生的情况。

在连续变化的系统中使用卡尔曼滤波非常理想，它占用内存小（除了前一个状态量，无需保留其他历史数据），速度快，特别适合实时问题和嵌入式系统。

网上大部分关于卡尔曼滤波的数学推导都显得晦涩。其实，换一个视角，它的核心思想非常简单且容易理解。下面我们用清晰的图示和颜色演示一遍，你只需要懂一些概率和矩阵的基础知识即可。

2. 卡尔曼滤波能做什么？

举个玩具例子：你开发了一个可以在树林里到处跑的小机器人，它需要知道自己的确切位置才能导航。

我们可以用状态向量 来表示机器人的状态，它包含位置和速度：

实际上这个状态只是一组描述系统基本属性的数字，可以换成任何东西。本例中是位置和速度，它也可以是一个容器中的液位、汽车发动机的温度、触摸板上手指的坐标，或者任何你需要跟踪的信号。

机器人装有 GPS，精度大约 10 米，还算不错，但它需要将自身位置精确到 10 米以内。树林里沟壑悬崖密布，走错一步就可能坠落。光有 GPS 是不够的。

或许我们也知道一些关于机器人如何运动的信息：比如，机器人清楚马达指令，知道自己正朝向某个方向移动且没有外力干预，那么下一个状态它很可能朝同一方向运动。当然，机器人对自身运动的了解并不完美：可能受风吹、轮子偏斜或碰到不平地面而翻倒。所以，轮子转过的距离并不能精确代表实际行走距离，预测也不完美。

GPS 传感器给出了部分状态信息，我们的运动预测则暗示了机器人可能怎样移动，但两者都是间接的，并且掺杂着不确定性和误差。那么，综合所有可用信息，我们能否得到一个比单独依赖任何一种信息都更好的估计？答案当然是 YES，这就是卡尔曼滤波的用武之地。

3. 卡尔曼滤波的视角

下面继续用只有位置和速度两个状态的简单例子来阐释。

我们并不知道实际的位置和速度确切是多少，它们之间存在很多种可能的正确组合，但其中一些组合的可能性要大于其他组合：

卡尔曼滤波假设这两个变量（位置和速度）都是随机的，并且服从高斯分布。每个变量都有一个均值 μ，表示随机分布的中心（最可能的状态），以及方差 ，表示不确定性。

在上图中，位置和速度是不相关的，意味着由其中一个变量的状态无法推断出另一个变量可能的值。下面的例子更有趣：位置和速度相关，观测到特定位置的可能性取决于当前的速度：

这种情况很常见，例如我们基于旧位置来估计新位置。如果速度很高，可能已经移动很远了；如果缓慢移动，则距离不会很远。跟踪这种相关性极为重要，因为它能带来更多信息：一个测量值会透露其他变量可能的值。这正是卡尔曼滤波的目的——在充满不确定性的测量数据中尽可能提取更多信息！

这种相关性用协方差矩阵表示。矩阵中的每个元素 表示第 i 个和第 j 个状态变量之间的相关度。（协方差矩阵是对称矩阵，所以 i 和 j 可以互换。）协方差矩阵通常用 表示，其中的元素则表示为 。

4. 用矩阵描述

我们基于高斯分布来建立状态变量，所以在时刻 k 需要两个信息：最佳估计 （即均值，其他地方常用 μ 表示），以及协方差矩阵 。

当然，这里我们只用了位置和速度，实际上状态可以包含多个变量，代表任何你想描述的信息。接下来，我们需要根据当前状态（k-1 时刻）来预测下一状态（k 时刻）。记住，我们并不知道哪个预测才是“真实”的，但预测函数对此并不在意——它对所有可能性进行预测，并给出一个新的高斯分布。

我们可以用矩阵 来表示这个预测过程：

它将原始估计中的每个点都移动到一个新的预测位置。如果原始估计正确，这个新预测位置就是系统下一步会到达的位置。那具体怎样用矩阵来预测下一时刻的位置和速度呢？使用基本运动学公式：

现在，我们有了预测矩阵来表示下一时刻的状态，但还不知道怎么更新协方差矩阵。此时引入另一个公式：如果把分布中的每个点都乘以矩阵 A，那么它的协方差矩阵 会怎样变化？很简单：

结合式(4)和(3)得到：

5. 外部控制量

我们还没有捕捉到所有信息，可能存在外部因素对系统施加控制，带来与系统自身状态无关的改变。

例如，火车司机可能操纵油门让火车加速，导航软件可能发出指令让机器人轮子转向或停止。如果知道这些额外的信息，我们就可以用一个向量 来表示，将其加到预测方程中做修正。

假设我们知道期望的加速度 ，根据运动学方程：

用矩阵形式表示就是：

其中 称为控制矩阵， 称为控制向量（对于没有外部控制的简单系统，这部分可以忽略）。再想一下，如果我们的预测不是 100% 准确，那该怎么办？

6. 外部干扰

如果状态仅基于系统自身的属性或已知的外部控制作用来变化，则不会出现问题。但如果有未知的干扰呢？例如，跟踪四旋翼飞行器时可能会受到风的干扰，轮式机器人可能打滑或遇到小坡减速。如果把这些外部干扰忽略，预测就会产生偏差。

在每次预测之后，我们可以添加一些新的不确定性，来建立与“外界”（即我们没有跟踪的干扰）之间的不确定性模型：

原始估计中的每个状态变量更新到新状态后仍然服从高斯分布，但就好像 的每个点都移动到了一个协方差为 的新高斯区域。换言之，我们将这些未跟踪的干扰当作协方差为 的噪声来处理。

这产生了具有相同均值但不同协方差的新高斯分布。

我们通过简单地加上 来得到扩展的协方差，完整的预测步骤表达式为：

由此可见，新的最优估计是根据上一最优估计预测得到的，并加上已知外部控制量的修正；而新的不确定性则由上一不确定性预测得到，并加上外部环境的干扰。

好了，我们现在对系统可能的动向有了一个模糊的估计，用 和 表示。如果再结合传感器的数据会怎样？

7. 用测量值修正

我们可能有多个传感器来测量系统当前的状态，每个传感器具体测量哪个状态变量并不重要——也许一个测位置，一个测速度——每个都间接提供了一些状态信息。

注意，传感器读数的单位和尺度可能与我们要跟踪的状态不同。我们用一个矩阵 来表示传感器数据到状态向量的映射。

我们可以计算传感器读数的分布，用之前的方法表示如下：

卡尔曼滤波的一大优点就是能处理传感器噪声。换句话说，我们的传感器或多或少都有些不可靠，原始估计中的每个状态可以和一定范围内的传感器读数对应。

从测量到的传感器数据中，我们大致能猜出系统当前处于什么状态。但由于存在不确定性，某些状态可能比我们得到的读数更接近真实状态。

我们将这种不确定性（例如传感器噪声）用协方差 表示，该分布的均值就是我们读取到的传感器数据，记为 。

现在，我们有了两个高斯分布：一个在预测值附近，一个在传感器读数附近。

我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。

那么，最可能的状态是什么？对于任何可能的读数 ，有两种解释：(1) 它来自传感器测量；(2) 它是由前一状态预测得到的。如果我们想知道这两种情况同时发生的概率，将这两个高斯分布相乘就可以了。

剩下的重叠部分，它的均值就是两个估计最可能的值，即给定所有信息下的最优估计。

瞧！这个重叠的区域看起来像另一个高斯分布。

如你所见，把两个具有不同均值和方差的高斯分布相乘，你会得到一个具有新的均值和方差的独立高斯分布！下面用公式详细推导。

8. 融合高斯分布

先从一维高斯分布入手。具有方差 和均值 μ 的高斯曲线可以表示为：

如果把两个高斯函数相乘会得到什么？

将式(9)代入到式(10)中（注意重新归一化，使总概率为1）可得：

将式(11)中相同的部分用 k 表示：

下面把式(12)和(13)写成矩阵形式，如果 Σ 表示高斯分布的协方差， 表示各维度的均值，则：

矩阵 称为卡尔曼增益，马上就会用到。放松，我们快要完成了！

将所有公式整合起来

我们有两个高斯分布：预测部分 ，和测量部分 。将它们代入式(15)计算重叠部分：

由式(14)可得卡尔曼增益为：

将式(16)和式(17)的两边同时左乘矩阵的逆（注意 里包含了 ）将其约掉，再将式(16)的第二个等式两边同时右乘 的逆，得到：

上式给出了完整的更新步骤方程。 就是新的最优估计，我们可以把它和 送到下一个预测和更新方程中不断迭代。

9. 总结

在所有公式中，实际只需要记住式(7)、(18)和(19)即可（如果忘了，可根据式(4)和(15)重新推导）。

我们可以用这套公式对任何线性系统建立精确的模型。对于非线性系统，则使用扩展卡尔曼滤波（EKF），差别在于 EKF 多了一步将预测和测量部分线性化的过程。

来源：How a Kalman filter works, in pictures

（本文整理自外网，更多数学原理与技术基础欢迎访问 云栈社区 交流学习。）