📝笔记：图解卡尔曼滤波

译者注：这恐怕是全网有关卡尔曼滤波最简单易懂的解释，如果你认真的读完本文，你将对卡尔曼滤波有一个更加清晰的认识，并且可以手推卡尔曼滤波。原文作者使用了漂亮的图片和颜色来阐明它的原理（读起来并不会因公式多而感到枯燥），所以请勇敢地读下去！

本人翻译水平有限，如有疑问，请阅读原文；如有错误，请在评论区指出。

背景

关于滤波

首先援引来自知乎大神的解释。

一位专业课的教授给我们上课的时候，曾谈到：filtering is weighting（滤波即加权）。滤波的作用就是给不同的信号分量不同的权重。最简单的loss pass filter， 就是直接把低频的信号给1权重，而给高频部分0权重。对于更复杂的滤波，比如维纳滤波, 则要根据信号的统计知识来设计权重。

从统计信号处理的角度，降噪可以看成滤波的一种。降噪的目的在于突出信号本身而抑制噪声影响。从这个角度，降噪就是给信号一个高的权重而给噪声一个低的权重。维纳滤波就是一个典型的降噪滤波器。

关于卡尔曼滤波

Kalman Filter 算法，是一种递推预测滤波算法，算法中涉及到滤波，也涉及到对下一时刻数据的预测。Kalman Filter 由一系列递归数学公式描述。它提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大：它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。

Kalman Filter 也可以被认为是一种数据融合算法（Data fusion algorithm），已有50多年的历史，是当今使用最重要和最常见的数据融合算法之一。Kalman Filter 的巨大成功归功于其小的计算需求，优雅的递归属性以及作为具有高斯误差统计的一维线性系统的最优估计器的状态⁴。

Kalman Filter 只能减小均值为0的测量噪声带来的影响。只要噪声期望为0，那么不管方差多大，只要迭代次数足够多，那效果都很好。反之，噪声期望不为0，那么估计值就还是与实际值有偏差。

问题描述

上面的描述可能把大家绕晕了，下面来点轻松的。

我们会有一个疑问：卡尔曼滤波到底是如何解决实际问题的呢？

我们以机器人为例介绍卡尔曼滤波的原理，我们的任务是要预测机器人的状态\(\vec{x}\)，包括位置\(p\)与速度\(v\)，即可表示为：

\[\vec{x}=\begin{bmatrix} p\\ v \end{bmatrix}\]

某个时刻，我们不知道真实的位置与速度到底是多少，二者存在一个所有可能性的组合，大致如下图所示。

卡尔曼滤波假设状态所有的变量都是随机的且都服从高斯分布，每个变量都有其对应的均值\(\mu\)以及方差\(\sigma^2\)（它代表了不确定性）。

在上图中，位置和速度是不相关（uncorrelated）的，这意味着某个变量的状态不会告诉你其他变量的状态是怎样的。即虽然我们知道现在的速度，但是无法从现在的速度推测出现在的位置。但实际上并非如此，我们知道速度和位置是有关系的（correlated），这样一来二者之间的组合关系变成了如下图所示的情况。

这种情况是很容易发生的，例如，如果速度很快，我们可能会走得更远，所以我们的位置会更大。如果我们走得很慢，我们就不会走得太远。

这种状态变量之间的关系很重要，因为它可以为我们提供更多信息：One measurement tells us something about what the others could be。这就是卡尔曼滤波器的目标，我们希望从不确定的测量中尽可能多地获取信息！

这种状态量的相关性可以由协方差矩阵表示。简而言之，矩阵\(\Sigma_{ij}\)的每个元素是第i个状态变量和第j个状态变量之间的相关度。（显然地可以知道协方差矩阵是对称的，这意味着您交换i和j都没关系）。协方差矩阵通常标记为“ \(\Sigma\)”，因此我们将它们的元素称为“\(\Sigma_{ij}\)”。

状态预测

问题的矩阵形式表示

我们把状态建模成高斯分布（Gaussian blob，由于二维高斯分布长得像一个个小泡泡，之所以长这个样子，可参考链接²）。我们需要求解/估计在时间\(k\)时刻的两个信息：1. 最优估计\(\mathbf{\hat{x}_k}\)以及它的协方差矩阵\(\mathbf{P_k}\)，我们可以写成下面矩阵形式：

\[ \begin{equation} \label{eq:statevars} \begin{aligned} \mathbf{\hat{x}}_k &= \begin{bmatrix} \text{position}\\ \text{velocity} \end{bmatrix}\\ \mathbf{P}_k &= \begin{bmatrix} \Sigma_{pp} & \Sigma_{pv} \\ \Sigma_{vp} & \Sigma_{vv} \\ \end{bmatrix} \end{aligned} \end{equation} \]

（当然，这里我们仅使用位置和速度，但是请记住状态可以包含任意数量的变量，并且可以表示所需的任何变量）

接下来，我们需要某种方式来查看当前状态（\(k-1\)时刻）并预测在时刻\(k\)处的状态。请记住，我们不知道哪个状态是“真实”状态，但是我们提到的预测（prediction）并不在乎这些。

我们可以用一个矩阵\(\mathbf{F_k}\)来表示这个预测过程：

这个矩阵\(\mathbf{F_k}\)将原始估计中的每个点移动到新的预测位置。

那么问题来了，应该如何使用上述矩阵来预测下一时刻的位置和速度呢？为了阐述这个过程，我们使用了一个非常基础的运动学公式（初中物理中就学过）进行描述：

\[\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + \Delta t &\color{royalblue}{v_{k-1}} \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} \end{split}\]

写成矩阵形式：

\[ \begin{align} \color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \label{statevars} \end{align} \]

现在我们有了一个预测矩阵或者叫做状态转移矩阵，该矩阵可以帮助我们计算下一个时刻的状态。但我们仍然不知道如何更新状态的协方差矩阵，其实过程也是很简单，如果我们将分布中的每个点乘以矩阵\(A\)，那么其协方差矩阵\(\Sigma\)会发生什么？

\[ \begin{equation} \begin{split} Cov(x) &= \Sigma\\ Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T \end{split} \label{covident} \end{equation} \]

将公式\(\eqref{covident}\)带入公式\(\eqref{statevars}\)，我们可以得到：

\[ \begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T \end{split} \end{equation} \]

External influence

不过我们并没有考虑到所有的影响因素。可能有一些与状态本身无关的变化——如外界因素可能正在影响系统。

例如，我们用状态对列车的运动进行建模，如果列车长加大油门，火车就加速。同样，在我们的机器人示例中，导航系统软件可能会发出使车轮转动或停止的命令。如果我们很明确地知道这些因素，我们可以将其放在一起构成一个向量\(\color{darkorange}{\vec{\mathbf{u}_k}}\)，对这个量进行处理，然后将其添加到我们的预测中对状态进行更正。

假设我们知道由于油门设置或控制命令而产生的预期加速度\(\color{darkorange}{a}\)。根据基本运动学，我们得到下式：

\[\begin{split} \color{deeppink}{p_k} &= \color{royalblue}{p_{k-1}} + {\Delta t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {\Delta t}^2 \\ \color{deeppink}{v_k} &= &\color{royalblue}{v_{k-1}} + & \color{darkorange}{a} {\Delta t} \end{split}\]

矩阵形式：

\[\begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{darkorange}{a} \\ &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \end{split} \end{equation}\]

其中\(\mathbf{B}_k\)被称为控制矩阵，\(\color{darkorange}{\vec{\mathbf{u}_k}}\)被称为控制向量。（注意：对于没有外部影响的简单系统，可以忽略该控制项）。

如果我们的预测并不是100％准确模型，这会发生什么呢？

External uncertainty

如果状态仅仅依赖其自身的属性进行演进，那一切都很好。如果状态受到外部因素进行演进，我们只要知道那些外部因素是什么，那么一切仍然很好。

但在实际使用中，我们有时不知道的那些外部因素到底是如何被建模的。例如，我们要跟踪四轴飞行器，它可能会随风摇晃；如果我们跟踪的是轮式机器人，则车轮可能会打滑，或者因地面颠簸导致其减速。我们无法跟踪这些外部因素，如果发生任何这些情况，我们的预测可能会出错，因为我们并没有考虑这些因素。

通过在每个预测步骤之后添加一些新的不确定性，我们可以对与“世界”相关的不确定性进行建模（如我们无法跟踪的事物）：

这样一来，由于新增的不确定性原始估计中的每个状态都可能迁移到多个状态。 因为我们非常喜欢用高斯分布进行建模，此时也不例外。我们可以说\(\color{royalblue}{\mathbf{\hat{x}}_{k-1}}\)的每个点都移动到具有协方差\(\color{mediumaquamarine}{\mathbf{Q}_k}\)的高斯分布内的某个位置，如下图所示：

这将产生一个新的高斯分布，其协方差不同（但均值相同）：

所以呢，我们在状态量的协方差中增加了额外的协方差\(\color{mediumaquamarine}{\mathbf{Q}_k}\)，所以预测阶段完整的状态转移方程为：

\[ \begin{equation} \begin{split} \color{deeppink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{royalblue}{\mathbf{\hat{x}}_{k-1}} + \mathbf{B}_k \color{darkorange}{\vec{\mathbf{u}_k}} \\ \color{deeppink}{\mathbf{P}_k} &= \mathbf{F_k} \color{royalblue}{\mathbf{P}_{k-1}} \mathbf{F}_k^T + \color{mediumaquamarine}{\mathbf{Q}_k} \end{split} \label{kalpredictfull} \end{equation} \]

换句话说：新的最佳估计是根据先前的最佳估计做出的预测，再加上对已知外部影响的校正。

新的不确定度是根据先前的不确定度做出的预测，再加上来自环境额外的不确定度。

上述过程描绘了状态预测过程，那么当我们从传感器中获取一些测量数据时会发生什么呢？

状态更新

利用测量进一步修正状态

假设我们有几个传感器，这些传感器可以向我们提供有关系统状态的信息。就目前而言，测量什么量都无关紧要，也许一个读取位置，另一个读取速度。每个传感器都告诉我们有关状态的一些间接信息（换句话说，传感器在状态下运作并产生一组测量读数）。

请注意，测量的单位可能与状态量的单位不同。我们使用矩阵\(\mathbf{H}_k\)对传感器的测量进行建模。

所以我们期望传感器的度数可以被建模成如下形式：

\[\begin{equation} \begin{aligned} \vec{\mu}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{\hat{x}}_k} \\ \mathbf{\Sigma}_{\text{expected}} &= \mathbf{H}_k \color{deeppink}{\mathbf{P}_k} \mathbf{H}_k^T \end{aligned} \end{equation}\]

卡尔曼滤波器的伟大之处就在于它能够处理传感器噪声。换句话说，传感器本身的测量是不准确的，且原始估计中的每个状态都可能导致一定范围的传感器读数，而卡尔曼滤波能够在这些不确定性存在的情况下找到最优的状态。

根据传感器的读数，我们会猜测系统正处于某个特定状态。但是由于不确定性的存在，某些状态比其他状态更可能产生我们看到的读数：

我们将这种不确定性（如传感器噪声）的协方差表示为\(\color{mediumaquamarine}{\mathbf{R}_k}\)，读数的分布均值等于我们观察到传感器的读数，我们将其表示为\(\color{yellowgreen}{\vec{\mathbf{z}_k}}\)

这样一来，我们有了两个高斯分布：一个围绕通过状态转移预测的平均值，另一个围绕实际传感器读数。

因此，我们需要将基于预测状态（粉红色）的推测读数与基于实际观察到的传感器读数（绿色）进行融合。

那么融合后最有可能的新状态是什么？ 对于任何可能的读数\((z_1,z_2)\)，我们都有两个相关的概率：（1）我们的传感器读数\(\color{yellowgreen}{\vec{\mathbf{z}_k}}\)是\((z_1,z_2)\)的（误-）测量值的概率，以及（2）先前估计值的概率认为\((z_1,z_2)\)是我们应该看到的读数。

如果我们有两个概率，并且想知道两个概率都为真的机会，则将它们相乘。因此，我们对两个高斯分布进行了相乘处理：

两个概率分布相乘得到的就是上图中的重叠部分。而且重叠部分的概率分布会比我们之前的任何一个估计值/读数都精确得多，这个分布的均值就是两种估计最有可能配置（得到的状态）。

事实证明，两个独立的高斯分布相乘之后会得到一个新的具有其均值和协方差矩阵的高斯分布！下面开始推公式。

合并两个高斯分布

首先考虑一维高斯情况：一个均值为\(\mu\)，方差为\(\sigma^2\)的高斯分布的形式为：

\[\begin{equation} \label{gaussformula} \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x – \mu)^2 }{ 2\sigma^2 } } \end{equation}\]

我们想知道将两个高斯曲线相乘会发生什么。下图中的蓝色曲线表示两个高斯总体的（未归一化）交集：

\[\begin{equation} \label{gaussequiv} \mathcal{N}(x, \color{fuchsia}{\mu_0}, \color{deeppink}{\sigma_0}) \cdot \mathcal{N}(x, \color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\sigma_1}) \stackrel{?}{=} \mathcal{N}(x, \color{royalblue}{\mu’}, \color{mediumblue}{\sigma’}) \end{equation}\]

将公式\(\eqref{gaussformula}\)代入公式\(\eqref{gaussequiv}\)，我们可以得到新的高斯分布的均值和方差如下所示：

\[\begin{equation} \label{fusionformula} \begin{aligned} \color{royalblue}{\mu’} &= \mu_0 + \frac{\sigma_0^2 (\mu_1 – \mu_0)} {\sigma_0^2 + \sigma_1^2}\\ \color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – \frac{\sigma_0^4} {\sigma_0^2 + \sigma_1^2} \end{aligned} \end{equation}\]

我们将其中的一小部分重写为\(\color{purple}{\mathbf{k}}\)：

\[\begin{equation} \label{gainformula} \color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{equation}\]

\[\begin{equation} \begin{split} \color{royalblue}{\mu’} &= \mu_0 + &\color{purple}{\mathbf{k}} (\mu_1 – \mu_0)\\ \color{mediumblue}{\sigma’}^2 &= \sigma_0^2 – &\color{purple}{\mathbf{k}} \sigma_0^2 \end{split} \label{update} \end{equation}\]

这样一来，公式的形式就简单多了！我们顺势将公式\(\eqref{gainformula}\)和\(\eqref{update}\)的矩阵形式写在下面：

\[\begin{equation} \label{matrixgain} \color{purple}{\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} \end{equation}\]

\[\begin{equation} \begin{split} \color{royalblue}{\vec{\mu}’} &= \vec{\mu_0} + &\color{purple}{\mathbf{K}} (\vec{\mu_1} – \vec{\mu_0})\\ \color{mediumblue}{\Sigma’} &= \Sigma_0 – &\color{purple}{\mathbf{K}} \Sigma_0 \end{split} \label{matrixupdate} \end{equation}\]

其中\(\Sigma\)表示新高斯分布的协方差矩阵，\(\vec{\mu}\)是每个维度的均值，\(\color{purple}{\mathbf{K}}\)就是大名鼎鼎的“卡尔曼增益”（Kalman gain）。

公式汇总

我们有两个高斯分布，一个是我们预测的观测\((\color{fuchsia}{\mu_0}, \color{deeppink}{\Sigma_0}) = (\color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k}, \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T})\)，另外一个是实际的观测(传感器读数)\((\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})\)，我们将这两个高斯分布带入公式\(\eqref{matrixupdate}\)中就可以得到二者的重叠区域：

\[\begin{equation} \begin{aligned} \mathbf{H}_k \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \mathbf{H}_k \color{royalblue}{\mathbf{P}_k’} \mathbf{H}_k^T &= \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} & – & \color{purple}{\mathbf{K}} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} \end{aligned} \label {kalunsimplified} \end{equation}\]

从公式\(\eqref{matrixgain}\)我们可以知道，卡尔曼增益是：

\[ \begin{equation} \label{eq:kalgainunsimplified} \color{purple}{\mathbf{K}} = \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \end{equation} \]

然后我们将公式\(\eqref{kalunsimplified}\)与公式\(\eqref{eq:kalgainunsimplified}\)中的\(\mathbf{H}_k\)去除，同时将\(\color{royalblue}{\mathbf{P}_k’}\)后面的\(\mathbf{H}_k^T\)去除，我们可以得到最终的化简形式的更新方程：

\[\begin{equation} \begin{split} \color{royalblue}{\mathbf{\hat{x}}_k’} &= \color{fuchsia}{\mathbf{\hat{x}}_k} & + & \color{purple}{\mathbf{K}’} ( \color{yellowgreen}{\vec{\mathbf{z}_k}} – \color{fuchsia}{\mathbf{H}_k \mathbf{\hat{x}}_k} ) \\ \color{royalblue}{\mathbf{P}_k’} &= \color{deeppink}{\mathbf{P}_k} & – & \color{purple}{\mathbf{K}’} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \end{split} \label{kalupdatefull} \end{equation}\]

\[\begin{equation} \color{purple}{\mathbf{K}’} = \color{deeppink}{\mathbf{P}_k \mathbf{H}_k^T} ( \color{deeppink}{\mathbf{H}_k \mathbf{P}_k \mathbf{H}_k^T} + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} \label{kalgainfull} \end{equation}\]

图说

大功告成，\(\color{royalblue}{\mathbf{\hat{x}}_k’}\)就是更新后的最优状态！接下来我们可以继续进行预测，然后更新，重复上述过程！下图给出卡尔曼滤波信息流。

总结

在上述所有数学公式中，你需要实现的只是公式\(\eqref{kalpredictfull}, \eqref{kalupdatefull}\)和\(\eqref{kalgainfull}\)。（或者，如果你忘记了这些，可以从等式\(\eqref{covident}\)和\(\eqref{matrixupdate}\)重新推导所有内容。）

这将使你能够准确地对任何线性系统建模。对于非线性系统，我们使用扩展卡尔曼滤波器，该滤波器通过简单地线性化预测和测量值的均值进行工作。

参考文献

---

1. 1.How a Kalman filter works, in pictures, 图解卡尔曼滤波是如何工作的↩︎
2. 2.A geometric interpretation of the covariance matrix, 协方差矩阵的几何解释↩︎
3. 3.Kalman Filter 卡尔曼滤波↩︎
4. 4.R. Faragher, “Understanding the Basis of the Kalman Filter Via a Simple and Intuitive Derivation [Lecture Notes]”, IEEE Signal Processing Magazine, vol. 29, no. 5, pp. 128–132, Sep. 2012.↩︎
5. 5.G. Welch and G. Bishop, “An Introduction to the Kalman Filter”, p. 16, 2006.↩︎
6. 6.Fitzgerald, Robert J. “Divergence of the Kalman filter”, Automatic Control IEEE Transactions on 16.6(1971):736-747.↩︎