卡尔曼滤波（Kalman Filter）原理浅析-图例分析

前言

最近项目需求涉及到目标跟踪部分，准备从 DeepSORT 多目标跟踪算法入手。DeepSORT 中涉及的内容有点多，以前也就对其进行了简单的了解，但是真正去做发现总是存在这样或者那样的困惑，头疼，果然欠下的总该还的

一个个来吧，这个系列文章主要分享博主在学习 DeepSORT 中的 Kalman Filter 的相关知识，主要从两方面分享，一方面是数学理论推导，另一方面是比较通俗易懂的图例分析。

这篇文章主要分享从图例分析的方式去理解卡尔曼滤波器。

博主为初学者，欢迎交流讨论，若有问题欢迎各位看官批评指正！！！

图例分析

翻译自：https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures

参考自：详解卡尔曼滤波（Kalman Filter）原理

注：博主参杂了自己的理解，所以翻译并不是原汁原味，建议阅读原文

1 背景

卡尔曼滤波（Kalman Filter，KF）做到的事情简直令人惊叹！

但很可惜的是，很少有软件工程师和科学家对其有深入了解。这是令人很不解的，因为卡尔曼滤波是如此通用且强大的工具，它能在不确定情况下融合信息。有时，它提取准确信息的能力是令人难以置信的！大家可以看一下之前发布的视频，其中演示了卡尔曼滤波器通过观察自由浮动的速度来确定其方向。

2. 卡尔曼滤波是什么？

你可以在任何地方使用卡尔曼滤波器，只要你对某个动态系统有不确定的信息，你就可以利用 KF 对系统下一步的动作做出有根据的猜测。即使存在着各种各样的干扰，卡尔曼滤波器通常也能清楚的知道实际发生了什么，并且它还能利用各种现象之间的相关性。

卡尔曼滤波器是连续变化系统的理想选择。卡尔曼滤波器的优点是占用内存少（除了先前的状态外，不需要保存任何历史记录），而且速度非常快，非常适合实时问题和嵌入式系统。

你在 Google 上找到的大部分关于卡尔曼滤波的数学描述都是晦涩难懂的。但卡尔曼滤波其实非常简单，只要用正确的方法去看，就很容易理解。因此，接下来我们将尝试使用许多清晰、美观的图片来解释它。阅读后续的内容的前提非常简单，你只需要对概率论和矩阵知识有基本的了解就行。

我们先举一个简单的例子，说明下卡尔曼滤波器可以解决什么问题，但如果你想直接看数学理论推导，也可以直接跳到后面。

3. 我们能用卡尔曼滤波做什么？

举一个简单的小例子：你做了一个能在丛林中随意行走的小机器人，但机器人需要时刻知道它自己的确切位置才能导航前行

我们的机器人每个时刻的状态可以用 $x$ 来表示，并且我们使用位置 $p$ 和速度 $v$ 两个变量来描述这个状态，那么 $k$ 时刻机器人的状态可以表示为 $x_k$，其定义如下：
$$x_k=(p,v)$$
值得注意的是，这个状态只是关于系统中的一些基本属性，它可以是任何东西。在我们的例子中是位置和速度，因为我们要知道机器人具体的位置和前进的速度才能导航让它前往丛林中的任意位置。但也可以是油箱中的油量、汽车发动机的温度、用户手指在触摸板中的位置等任何我们需要跟踪的东西。

我们的机器人一般装有 GPS 传感器，可以精确到 10m 左右，这很好，起码我们知道了机器人的一个大概的位置，虽然它存在误差。但现在我们需要知道比 10m 更精确的位置才行，这是因为在丛林中存在很多沟壑和悬崖，如果机器人的位置误差超过几英尺，就有可能掉下悬崖。因此，仅仅靠 GPS 本身的定位是远远不够的。

我们还知道一些关于机器人如何移动的信息：机器人知道发送到轮子电机的指令，也知道如果在没有任何干扰的情况下朝一个方向前进，那么下一秒它很可能会沿着同一方向前进。当然，它对自己的运动并不是了如指掌：它可能会受到风的冲击，车轮也可能会打滑，或者在颠簸的地形上滚动。因此，车轮转动的幅度可能并不完全能代表机器人实际行驶的距离，但它也提供给了我们一个关于机器人移动的信息，只是这个信息并不那么精确

综上所示，GPS传感器 告诉了我们一些状态信息，但具有一定的不确定性（也就是存在误差或者噪声干扰）。我们的 预测 可以告诉我们机器人的移动情况，但也存在一定的不确定性。

这两个信息都不那么准确，但如果我们利用所有可用信息，能否得到一个比较好的结果呢？当然可以！这就是我们接下来要讲解的卡尔曼滤波器的作用

4. 如何从卡尔曼滤波的角度看待问题？

我们继续之前的例子，机器人的状态仅仅包含位置和速度
$$x=\left[\begin{array}{c}p\\ v\end{array}\right]$$
我们并不知道实际的位置和速度是多少，它们之间有一系列可能的组合，例如 $p=0,v=0$；$p=50,v=10$；$p=500,v=60$ 等等，但其中一些组合的可能性要大于其他组合

卡尔曼滤波器假设两个变量（例子中为位置和速度）都是随机变量且服从 高斯分布。每个变量的 均值 为 $\mu$，它表示随机分布的中心位置（即机器人最可能的状态）；方差 为 ${\sigma }^2$，代表着不确定性。

在上图中，位置和速度是 不相关 的，这意味着通过一个变量的状态不能推测出另一个变量的状态。

下面的例子展示了更有趣的东西：位置和速度是 相互关联 的。观测到某个特定位置的可能性取决于你的速度：

如果我们是根据旧位置来估算新位置，就可能出现这种情况。假设我们的速度很快，我们可能移动得更远，因此我们的位置也会更远。假设我们的移动速度很慢，那么我们就没有走得那么远。

跟踪位置和速度的这种关系非常重要，因为它能为我们提供了更多信息：一个测量结果可以告诉我们其他测量结果的一些信息。这就是卡尔曼滤波器的目标，即我们希望从不确定性的测量结果中获取尽可能多的信息！

这种相关性可以由一个叫做协方差矩阵（covariance matrix）的东西来描述。简单来说，协方差矩阵中的每个元素 ${\Sigma }_{ij}$ 表示第 $i$ 个状态变量与第 $j$ 个状态变量之间的相关程度。（也许你猜到了，协方差矩阵是一个对称矩阵，这意味着 $i$ 和 $j$ 的交换并不重要）。协方差矩阵通常表示为 $\Sigma$，因此其中的元素可以表示为 ${\Sigma }_{ij}$

5. 从矩阵的角度看待问题

我们基于 Gaussian blob 来对状态变量进行建模，因此在时间 $k$ 时我们需要两个信息：最佳状态估计 ${\hat{\mathbf{x}}}_{\mathbf{k}}$（即均值 $\mu$）和协方差矩阵 ${\mathbf{P}}_{\mathbf{k}}$
$$\begin{array}{rl}{\hat{\mathbf{x}}}_{\mathbf{k}}& =\left[\begin{array}{c}\text{ position }\\ \text{ velocity }\end{array}\right]\\ {\mathbf{P}}_{\mathbf{k}}& =\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{\mathbf{vp}}& {\Sigma }_{\mathbf{vv}}\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
Note：当然，我们在这里只使用位置和速度，但记住状态可以包含任意数量的变量，也可以代表任何你想要的东西，这一点很有用

接下来，我们需要某种方法来查看当前 $k-1$ 时刻状态并预测下一个 $k$ 时刻状态。值得注意的是，我们不知道对下一时刻的所有预测中哪个是真实的，我们的预测函数也不关心。它只是对所有状态都起作用，并给出一个新的分布：

我们用矩阵 ${\mathbf{F}}_{\mathbf{k}}$ 来表示这个预测过程，如下图所示：

矩阵 ${\mathbf{F}}_{\mathbf{k}}$ 将我们最初估计中的每个点都移动到了一个新的预测位置，如果最初的估计是正确的，这个新的预测位置就是系统下一步会移动到的位置。

那我们又如何用矩阵 ${\mathbf{F}}_{\mathbf{k}}$ 来预测下一时刻的位置和速度呢？我们将使用一个非常基本的运动学公式（假设机器人的运动是匀速直线运动）：
$$\begin{array}{rl}{p}_k& =p_{k-1}+\Delta t{v}_{k-1}\\ v_k& =v_{k-1}\end{array}$$
用矩阵的方式表示：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]{\hat{\mathbf{x}}}_{k-1}\end{array}\text{\hspace{1em}(2)}$$

$${\hat{\mathbf{x}}}_k={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\text{\hspace{1em}(3)}$$

现在我们有了一个预测矩阵 ${\mathbf{F}}_{\mathbf{k}}$，它给我们提供了下一时刻的状态，我们之前有提到每个时刻都需要两个信息，一个是状态信息，另一个是协方差矩阵，状态信息我们通过预测矩阵 ${\mathbf{F}}_{\mathbf{k}}$ 解决了，那协方差矩阵该如何更新呢？

这时我们需要另一个公式。如果我们用矩阵 $\mathbf{A}$ 乘以分布中的每个点，那么它的协方差矩阵 $\Sigma$ 会发生什么变化呢？

很简单，我们直接给出结论：
$$\begin{array}{rl}Cov(x)& =\Sigma \\ Cov(\mathbf{A}x)& =\mathbf{A}\Sigma \mathbf{A}{}^T\end{array}\text{\hspace{1em}(4)}$$
联合公式 (3) 和 (4)：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T\end{array}\text{\hspace{1em}(5)}$$

6. 外部影响

不过，我们并没有捕捉到一切信息。可能有一些变化与状态本身无关 —— 外部世界可能会影响系统（即可能存在外部控制输入）

例如，如果状态模拟的是列车的运动，那么列车驾驶员可能会踩下油门使列车加速。同样，在我们的机器人示例中，导航软件可能会发出让转动车轮或停止的指令。如果我们知道这些额外的信息，我们就可以将其放入到 $u_k$ 向量中，对其进行处理，并将其作为修正添加到我们的预测中。

假设我们知道油门设置或控制指令导致的预期加速度 $a$，根据运动学方程我们可以得到：
$$\begin{array}{rl}{p}_k& =p_{k-1}+\Delta t{v}_{k-1}+\frac{1}{2}a\Delta t^2\\ v_k& =v_{k-1}+a\Delta t\end{array}$$
其矩阵形式为：
$$\begin{array}{rl}\hat{\mathbf{x}}{}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k{u}_k\end{array}\text{\hspace{1em}(6)}$$
其中 ${\mathbf{B}}_k$ 称为控制矩阵（control matrix），$u_k$ 是控制变量（control vector）（对于没有外部影响的简单系统而言，可以忽略它们）

让我们再在考虑一个问题，如果我们的预测不是一个 100% 准确的模型，那会发生什么呢？

7. 外部不确定性

如果状态是基于自身的特性演变的，那么一切都没问题。如果状态是基于外力演变的，只要我们知道这些外力是什么，一切也都没问题。

但那些我们不知道的力呢？例如，如果我们正在追踪一个四旋翼飞行器，它可能会被风吹得东倒西歪；如果我们追踪的是一个带轮子的机器人，轮子可能会打滑，或者地面的颠簸可能会让它减速。我们无法跟踪这些情况，如果这些情况发生了，我们的预测可能会出现偏差，因为我们没有考虑这些额外的力。

我们可以通过在每个预测步骤之后添加一些新的不确定性，来模拟与 “世界”（即我们没有跟踪的事物）相关的不确定性：

在我们最初的估计中，每个状态都可能移动到一系列状态。Gaussian blob 非常适合这个场景，因此我们可以说 ${\hat{x}}_{k-1}$ 中的每个点都移动到了协方差为 $Q_k$ 的 Gaussain blob 的某个地方。换而言之，我们使用协方差 $Q_k$ 来处理未跟踪的噪声的影响。

这将会产生一个新的 Gaussian blob，其协方差不同，但均值相同。

我们只需要加上 $Q_k$ 即可得到扩展的协方差，从而得到预测步骤的完整表达式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+B_k{u}_k\\ {\mathbf{P}}_k& ={\mathbf{F}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+Q_k\end{array}\text{\hspace{1em}(7)}$$
换句话说，新的最优估计值是根据之前的最优估计值进行预测，再加上对已知外部影响的修正。

而新的不确定性是从旧的不确定性中预测出来的，再加上一些来自环境的额外不确定性。

OK！我们根据 ${\hat{\mathbf{x}}}_k$ 和 ${\mathbf{P}}_k$ 对系统可能的位置有了一个模糊的估计，当我们从传感器获得一些数据时会发生什么呢？（我们是不是还有信息没有利用上呢？当然有！传感器提供的数据我们还没用上呢）

8. 利用外部观测值修正估计量

我们可能有几个传感器，它们可以为我们提供有关系统状态的信息。目前，它们测量的是什么并不重要，也许一个读取的是位置，另一个读取的是速度。每个传感器都会间接地告诉我们一些关于系统状态的信息，换句话说，传感器是在一种状态下工作，并产生一组读数。

值得注意的是，传感器读数的单位和刻度可能与我们所跟踪的状态的单位和刻度不同。因此，我们需要利用一个矩阵 ${\mathbf{H}}_k$ 为传感器建模（即测量空间的转换）。

我们可以按照常规方法计算出传感器读数的分布情况：
$$\begin{array}{rl}{\mu }_{\mathrm{expected}}& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k\\ {\Sigma }_{\mathrm{expected}}& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}\text{\hspace{1em}(8)}$$
卡尔曼滤波器最擅长处理的一件事就是传感器噪声。换句话说，我们的传感器至少在某种程度上是不可靠的，原始估计中的每个状态都可能导致传感器读数的变化。

根据我们观察到的每个读数，我们可以猜测我们的系统处于某种特定状态。但由于存在不确定性，某些状态比其他状态更有可能产生我们看到的读数：

我们将这种不确定性（即传感器噪声）的协方差称为 $R_k$。该分布的均值等于我们观察到的读数，我们称之为 $z_k$

现在我们有了两个 Gaussian blobs：一个围绕着我们转换后的预测平均值，另一个围绕着我们得到的实际传感器读数。

我们必须根据预测状态（粉红色部分）和实际观察到的传感器读数（绿色部分），努力调整我们的估计值，使其与实际值靠近

那么新的最可能的状态是什么？对于任何可能的读数 $(z_1,z_2)$，我们有两个相关的概率：(1) 传感器的测量值；(2) 前一个状态的估计值

如果我们有两个概率，并想知道两个概率都为真的几率，我们只需将它们相乘即可。因此，我们将两个高斯概率相乘：

我们剩下的就是重叠部分，也就是两个圆球都很亮的区域。它比我们之前的估计都要精确得多。这个分布得均值是两个估计值都最优可能出现得值，因此也是我们根据所有信息对真实值的最优估计。

这个重叠的区域看起来像另一个 Gaussian blob：

事实证明，当你将两个具有不同均值和协方差矩阵的高斯分布相乘时，你会得到一个具有自己均值和协方差矩阵的新的高斯分布！下面我们用公式来讲解下

9. 融合高斯分布

我们先拿一维高斯分布来分析，我们知道均值为 $\mu$ 方差为 ${\sigma }^2$ 的一维高斯曲线可以用下面的式子表示：
$$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\text{\hspace{1em}(9)}$$
如果我们把两个都服从高斯分布的函数相乘会得到什么呢？

下面的蓝色曲线代表两条高斯曲线（未归一化）的交叉点：

$$\mathcal{N}(x,{\mu }_0,{\sigma }_0)\cdot \mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,{\mu }^{\prime },{\sigma }^{\prime })\text{\hspace{1em}(10)}$$

将 (9) 代入 (10) 中可以得到：
$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ {\sigma }^{\prime 2}& ={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}(11)}$$
将 （11）中的两个式子相同的部分用 $\mathbf{k}$ 表示：
$$\begin{array}{rl}\mathbf{k}& =\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}(12)}$$

$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\mathbf{k}({\mu }_1-{\mu }_0)\\ {\sigma }^{\prime 2}& ={\sigma }_0^2-\mathbf{k}{\sigma }_0^2\end{array}\text{\hspace{1em}(13)}$$

下面进一步将 （12）和（13）写成矩阵的形式，其中 $\Sigma$ 表示高斯分布的协方差，$u$ 表示每个维度的均值，则：
$$\mathbf{K}={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\text{\hspace{1em}(14)}$$

$$\begin{array}{rl}{\mu }^{\prime }& ={\mu }_0+\mathbf{K}({\mu }_1-{\mu }_0)\\ {\Sigma }^{\prime }& ={\Sigma }_0–\mathbf{K}{\Sigma }_0\end{array}\text{\hspace{1em}(15)}$$

矩阵 $\mathbf{K}$ 称为卡尔曼增益，下面将会用到。

10. 整合

现在我们有两个高斯分布，

预测部分： $({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T)$

测量部分： $({\mu }_1,{\Sigma }_1)=({\mathbf{z}}_k,{\mathbf{R}}_k)$

将它们放到式 （15）中算出它们之间的重叠部分：
$$\begin{array}{rl}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k+\mathbf{K}({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{H}}_k{\mathbf{P}}_k^{\prime }{\mathbf{H}}_k^T& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T-\mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}\text{\hspace{1em}(16)}$$

从式 (14) 中我们知道卡尔曼增益为：
$$\mathbf{K}={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}\text{\hspace{1em}(17)}$$
我们可以在式 (16) 和 (17) 中的两边同时左乘 ${\mathbf{H}}_k^{-}$，并在式 (16) 的第二个等式两边同时右乘 $({\mathbf{H}}_k^T{)}^{-}$ 得到以下等式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+{\mathbf{K}}^{\prime }({\mathbf{z}}_k-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_k-{\mathbf{K}}^{\prime }{\mathbf{H}}_k{\mathbf{P}}_k\end{array}\text{\hspace{1em}(18)}$$

$${\mathbf{K}}^{\prime }={\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}\text{\hspace{1em}(19)}$$

上式给出了完整的更新步骤方差。${\hat{\mathbf{x}}}_k^{\prime }$ 是我们新的最优估计，我们可以继续把它和 ${\mathbf{P}}_k^{\prime }$ 放到新一轮的预测和更新中不断迭代。

卡尔曼滤波器的信息流图如下所示：

11. 总结

在上面所有的数学公式中，你只需要实现公式 (7)、(18) 和 (19)。如果你忘记了上述公式，也可以从公式 (4) 和 (15) 中重新推导出所有内容。

这将使你能够精确地模拟任何线性系统。而对于非线性系统，我们使用扩展卡尔曼滤波（Extended Kalman Filter,EKF），它的工作原来是简单地将预测值和测量值线性化，使其趋近于平均值。

结语

本篇博客从机器人导航的案例出发讲解了卡尔曼滤波器的作用，我们知道通过数学模型即运动方程获得的机器人状态（即位置和速度）信息是不准确的，存在不确定性；而通过传感器获得的机器人状态信息也是不准确的，同样存在不确定性，而卡尔曼滤波器则结合了运动方程模型信息和传感器信息得到了一个更为准确的关于机器人状态的估计值。

卡尔曼滤波器的本质就是通过一个不准确的数学模型和一个不准确的测量结果去估计一个最优值，其中的估计算法就是 Kalman Filter 算法，它包括预测和校正两个部分，总共五个公式。

参考

• symmetric_matrix
• covariance matrix
• 详解卡尔曼滤波（Kalman Filter）原理
• https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures