详解卡尔曼滤波（Kalman Filter）原理

前言

看过很多关于卡尔曼滤波的资料，发现很多资料写的都很晦涩，初学者很难看懂。在网上找了很多资料之后，发现了这篇博文讲的非常清晰易懂，特此翻译记录，以备后用。为保证翻译的原滋原味，以下均用第一人称表述。

背景

我不得不说一说卡尔曼滤波，因为它能做到的事情简直令人惊叹。

很可惜的是，很少有软件工程师和科学家对此有深入了解。这让我感到很沮丧，因为卡尔曼滤波是如此通用且强大的工具，它能在不确定情况下组合信息。有时，它提取准确信息的能力似乎几乎是不可思议的。如果听起来我讲的太多了，那么请看一下之前发布的视频，其中演示了一个利用卡尔曼滤波观察自由浮动物体的速度来确定它的方向。真棒！

卡尔曼滤波是什么？

你可以在任何含有不确定信息的动态系统中的使用卡尔曼滤波，对系统的下一步动作做出有根据的猜测。即使伴随着各种干扰，卡尔曼滤波总是能指出真实世界发生的情况。它可以利用怪异现象之间的关联，你可能不会想到利用这些关联！

卡尔曼滤波对于持续变化的系统是理想的选择。由于卡尔曼滤波除了记忆前一个状态而不需要保留其他的历史记忆信息，因此卡尔曼滤波具有轻量化的特点，运行速度非常快，非常适合处理实时的问题和嵌入式系统。

你在Google上找到的大部分关于卡尔曼滤波的数学描述是晦涩难懂的。那是非常糟糕的状况！因为卡尔曼滤波能被简单和容易的方式所理解的。因此，本文是一个非常不错的文章主题，本文将尝试用许多清晰、美观的图片来阐明它。本文的前提很简单，你仅仅需要对概率和矩阵有基本的了解。

本文将从一个简单的例子开始，说明卡尔曼滤波可以解决的问题。但如果你想直接接触精美的图片和数学，请随时跳转。

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

举一个简单的小例子：你已经做了一个能在丛林中游走的小机器人，为确保其能导航，机器人需要知道它所在的位置。

将机器人的运动状态表示为

，其仅仅包含了位置和速度：

请注意，状态只是关于系统底层配置的数字列表，它可以是任何东西。在我们的例子中，它是位置和速度，但它可以是油箱中液体量的数据、汽车引擎的温度、用户手指在触控板上的位置或者任何你需要跟踪的东西。

我们的机器人也有GPS传感器，精确大约10米，但它需要更精确地知道自己的位置。在树林中有很多沟壑和悬崖，如果机器人的误差超过几英尺，它可能会从悬崖上掉下去。所以仅依赖GPS进行定位是远远不够的。

我们可能还知道机器人是如何移动的：机器人知道发送给车轮马达的指令，如果它朝一个方向前进，没有任何干扰，下一刻它可能会继续朝同一方向前进。当然，它对自己的运动并不完全了解：它可能会受到风的冲击，车轮可能会打滑，或者在崎岖不平的地形上滚动；所以轮子转动的数量可能不能准确地代表机器人实际行走了多远，这个预测也不会完全准确。

GPS传感器告诉我们一些关于状态的信息，但只是间接的，带有一些不确定性而且并不精准。我们的预测告诉了机器人是如何移动的，但只是间接的，并且也是不确定和不精准的。

但是，如果我们利用所有可用的信息，我们能得到一个比这两个估计本身更好的答案吗?当然，答案是肯定的，这就是卡尔曼滤波器的作用。

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

让我们来看看我们想要诠释的一个场景。我们继续上一个例子，机器人仅仅包含一个位置和速度的简单状态。

我们不知道实际的位置和速度是什么；有一系列可能的位置和速度组合可能是正确的，但其中一些状态比其他的状态更可能：

卡尔曼滤波器假设两个变量（例子中为位置和速度）都是随机变量且服从高斯分布的。随机变量的均值为

，它表示随机分布的中心位置，即机器人最可能的状态，不确定性用方差

表示：

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

更有趣的是下面的例子：位置和速度是呈相关性的。观察特定位置的可能性取决于你的速度：

这种情况可能会出现。例如：根据一个旧的位置估计一个新的位置，如果速度很快，可能会走得更远，所以位置会更远。如果走得很慢，那么就不会走的很远。

这种关系非常重要，因为它给我们提供了更多的信息：一个测量值告诉我们其他测量值可能是什么。我们要尽可能多地从不确定的目标中压缩卡尔曼滤波器的信息！

这种相关性被称为协方差矩阵。简而言之，矩阵的每个元素

是第

个状态变量和

个状态变量之间的相关程度。（由于协方差矩阵是对称的，也就是交换

和

不会影响最终的结果)。协方差矩阵往往用“

”来表示，其中的元素则表示为“

”。

矩阵化描述问题

我们基于高斯分布来建立状态变量，所以在时间

需要两条信息：将最佳估计称为

（即均值

）和它的协方差矩阵

。

\hat{\mathbf{x}}_{\mathbf{k}} = \begin{bmatrix} \text{position} \\ \text{velocity} \end{bmatrix} \\ \rm{P_k} = \begin{bmatrix} \sum\nolimits_{pp} & \sum\nolimits_{pv} \\ \sum\nolimits_{vp} & \sum\nolimits_{vv} \end{bmatrix} \tag{1}

(在这里本文只使用了位置和速度，但是该状态可以包含任意数量的变量，并表示任何需要表示的东西，这对于处理其他问题是非常有益的)。

接下来，我们需要某种方式来知道目前状态（时刻

）并预测下一个时刻

的状态。记住，我们不知道哪个状态是“真实的”状态，但我们的预测函数并不关心。它适用于所有的情况，并给出了一个新的分布：

用矩阵

表示这个预测步骤：

它将我们原始估计中的每个点都移动到了一个新的预测位置，如果原始估计是正确的话，这个新的预测位置就是系统下一步会移动到的位置。那我们又如何用矩阵来预测下一个时刻的位置和速度呢？下面用一个基本的运动学公式来表示：

外部控制变量

\begin{equation} \begin{aligned} \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} \overrightarrow{\mathbf{u}_{k}} \end{aligned} \end{equation}

外部干扰

\begin{aligned} \hat{\mathbf{x}}_{k} &=\mathbf{F}_{k} \hat{\mathbf{x}}_{k-1}+\mathbf{B}_{k} \overrightarrow{\mathbf{u}_{k}} \\ \mathbf{P}_{k} &=\mathbf{F}_{\mathbf{k}} \mathbf{P}_{k-1} \mathbf{F}_{k}^{T}+\mathbf{Q}_{k} \end{aligned}

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

联合高斯分布

\begin{equation} \begin{aligned} \mu^{\prime} &=\mu_{0}+\frac{\sigma_{0}^{2}\left(\mu_{1}-\mu_{0}\right)}{\sigma_{0}^{2}+\sigma_{1}^{2}} \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\frac{\sigma_{0}^{4}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned} \end{equation}

整合上述公式

\begin{aligned} \mathbf{H}_{k} \hat{\mathbf{x}}_{k}^{\prime} &=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} &+\mathbf{K}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \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{aligned}

\begin{aligned} \hat{\mathbf{x}}_{k}^{\prime} &=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathrm{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{P}_{k}^{\prime} &=\mathbf{P}_{k^{-}} \mathbf{K}^{\prime} \mathbf{H}_{k} \mathbf{P}_{k} \end{aligned}

总结

对于上述所有的数学公式，你仅仅需要实现公式（7）、（18）和（19）。（如果你忘记了上述公式，你也能从公式（4）和（5）重新推导。）

这将允许你精确地建模任何线性系统。对于非线性系统，需要用到扩展卡尔曼滤波，区别在于EKF多了一个把预测和测量部分进行线性化的过程。