【图解】卡尔曼滤波器是如何工作的

• 本文转载于Bzarg所写博客，点击这里可以查看原文！

我忍不住想和您分享一些关于卡尔曼滤波器的事情，因为它的作用实在是太不可思议了。出乎意料的是，几乎没有软件工程师和科学家对此有所了解，这让我感到很难过，因为它真的是在不确定情况下组合信息的通用且强大的工具。有时，它提取准确信息的能力、几乎是神奇的——如果听起来我讲的太多了，那么请看一下以前发布的视频，其中我演示了通过观察自由浮标的速度，借助卡尔曼滤波器来确定其方向的例子。

它是什么？

您可以在不确定某个动态系统信息的任何情况下使用卡尔曼滤波器，并且可以对系统接下来将要做什么做出有根据的猜测。即使现实状况出现了混乱并干扰了您所猜测的预期运动，卡尔曼滤波器通常也可以很好地弄清楚实际发生了什么。同时，它可以充分利用您可能不会想到的疯狂现象之间的相关性！

卡尔曼滤波器是不断变化的系统的理想选择。他们有这样做的好处是它们在内存上比较轻便（除了保留以前的状态，它们不需要保留任何历史记录），而且它们非常快，非常适合实时问题和嵌入式系统。

在Google上的大多数地方，用于实现Kalman过滤器的数学运算看起来都很恐怖且不透明。这是一种糟糕的情况，因为如果您以正确的方式看待卡尔曼滤波器，实际上它非常简单易懂。因此，它是一个很好的文章主题，我将尝试用许多清晰，漂亮的图片和颜色来阐明它。前提很简单；您只需要对概率和矩阵有基本的了解。

我将从卡尔曼滤镜可以解决的问题的一个简单的例子开始，但是如果您想了解闪亮的图片和数学，请随时前进。

卡尔曼滤波器能做什么？

让我们举一个玩具示例：您已经构建了一个可以在树林中四处游荡的小型机器人，并且该机器人需要确切地知道它在哪里，以便它可以导航。

我们会说我们的机器人处于状态$\overrightarrow{x_k}$，这只是位置和速度：
$$\overrightarrow{x_k}=(\vec{p},\vec{v})$$请注意，状态只是有关系统基础配置的数字列表。可能是任何东西。在我们的示例中，它是位置和速度，但它可能是有关油箱中的油量，汽车发动机的温度，用户手指在触摸板上的位置或需要跟踪的任何数量的数据。

我们的机器人还具有一个GPS传感器，该传感器的精确度约为10米，这很好，但是它需要比10米更精确地知道其位置。这些树林中有许多沟壑和悬崖，如果机器人错了几英尺，它可能会掉下悬崖。因此GPS本身还不够好。

我们可能还对机器人的移动方式有所了解：它知道发送给轮式电动机的命令，并且知道如果机器人朝一个方向前进，并且没有任何干扰，那么在下一瞬间它可能会沿着同一方向前进。但是当然，它并不完全了解其运动：它可能会受到风的冲击，车轮可能会打滑一点，或者在崎bump不平的地形上翻滚。因此轮子转动的数量可能无法完全代表机器人实际行驶的距离，因此预测也不是完美的。

GPS 传感器告诉我们有关状态的信息，但只能间接地告诉我们，并且存在一些不确定性或不准确性。我们的预测告诉我们有关机器人如何运行的信息，但仅是间接的，并且存在一定的不确定性或不准确性。

但是，如果我们使用所有可用的信息，是否能获得比任何一种估计本身都能提供的更好的答案？当然答案是肯定的，这就是卡尔曼滤波器的目的。

卡尔曼滤波器如何看待您的问题

让我们看一下我们要解释的风景。我们将继续一个仅具有位置和速度的简单状态。
$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$我们不知道实际的位置和速度是多少。可能存在的位置和速度组合的范围很广，但其中某些组合比其他组合更有可能：

卡尔曼滤波器假设两个变量（在我们的示例中为位置和速度）都是随机的且呈高斯分布。每个变量都具有平均值，这是随机分布（及其最可能的状态）的中心，和一个方差，这是不确定性：$\mu {\sigma }^2$

在上图中，位置和速度是不相关的，这意味着一个变量的状态不会告诉您其他变量的含义。

下面的示例显示了一些更有趣的内容：位置和速度相关。观察特定位置的可能性取决于您的速度：

例如，如果我们基于旧职位来估计新职位，则可能会出现这种情况。如果我们的速度很高，我们可能会走得更远，所以我们的位置会更远。如果我们走得很慢，我们就不会走得太远。

保持这种关系真的很重要，因为它可以为我们提供更多信息：一种度量可以告诉我们一些其他信息。这就是卡尔曼滤波器的目标，我们希望从不确定的测量中尽可能多地获取信息！

这种相关性被称为协方差矩阵的东西捕获。简而言之，矩阵${\Sigma }_{ij}$每个元素是第$i$个状态变量和第$j$个状态变量之间的相关程度。（您也许可以猜测协方差矩阵是对称的，这意味着您交换$i$和$j$都没关系）。协方差矩阵通常标记为"$\Sigma$"，因此我们将它们的元素称为"${\Sigma }_{ij}$"。

用矩阵描述问题

我们将关于状态的知识建模为高斯Blob，因此在$k$时刻， 我们需要两条信息：我们将最佳估计称为${\hat{\mathbf{x}}}_{\mathbf{k}}$（平均值，在其他地方称为$\mu$）及其协方差矩阵${\mathbf{P}}_{\mathbf{k}}$。
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& =\left[\begin{array}{c}\text{position}\\ \text{velocity}\end{array}\right]\\ {\mathbf{P}}_k& =\left[\begin{array}{cc}{\Sigma }_{pp}& {\Sigma }_{pv}\\ {\Sigma }_{vp}& {\Sigma }_{vv}\end{array}\right]\end{array}$$
（当然，这里我们仅使用位置和速度，但是记住状态可以包含任意数量的变量并表示您想要的任何值很有用）。

接下来，我们需要某种方式查看当前状态（在时间k-1）并预测在时间k的下一个状态。请记住，我们不知道哪个状态是“真实”状态，但是我们的预测功能并不在乎。它仅适用于所有这些，并为我们提供了新的发行版：

我们可以用矩阵表示此预测步骤：$F_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}\\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}$$

现在，我们有了一个预测矩阵，该矩阵提供了我们的下一个状态，但是我们仍然不知道如何更新协方差矩阵。

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

好吧，这很容易。我只是给你一个标识：
$$\begin{array}{rl}Cov(x)& =\Sigma \\ Cov(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}$$

因此，将方程(4)与（3）结合：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\\ {\mathbf{P}}_k& ={\mathbf{F}}_k{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T\end{array}$$

外部影响

但是，我们还没有捕获所有内容。可能会有一些与国家本身无关的变化-外界可能正在影响系统。

例如，如果状态模拟火车的运动，则火车操作员可能会推油门，从而导致火车加速。同样，在我们的机器人示例中，导航软件可能会发出使车轮转动或停止的命令。如果我们知道有关世界动态的其他信息，则可以将其填充到名为$\overrightarrow{{\mathbf{u}}_k}$，对其进行处理，然后将其添加到我们的预测作为更正。

假设我们知道由于油门设置或控制命令而产生的预期加速度$a$。从基本运动学中，我们可以得到：以矩阵形式：
$$\begin{array}{rlrl}{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\overrightarrow{{\mathbf{u}}_k}\end{array}$$

${\mathbf{B}}_k$被称为控制矩阵，$\overrightarrow{{\mathbf{u}}_k}$被称为控制向量。（对于没有外部影响的非常简单的系统，可以忽略这些）。

让我们再添加一个细节。如果我们的预测不是实际情况的100％准确模型，会发生什么？

外部不确定性

如果状态基于其自身的属性而发展，则一切都很好。一切都还是好的，如果基于外部力量，状态演变，只要我们知道这些外力。

但是我们不知道的力量呢？例如，如果我们要跟踪四轴飞行器，它可能会随风摇晃。如果我们跟踪的是轮式机器人，则车轮可能会打滑，或者地面上的颠簸可能会使其减速。我们无法跟踪这些事情，如果发生任何此类情况，我们的预测可能会落空，因为我们没有考虑这些额外的力量。

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

在我们原来的估计每个状态可能已经移动到一个范围状态。因为我们非常喜欢高斯blobs，所以我们会说${\hat{\mathbf{x}}}_{k-1}$中的每一个点以协方差${\mathbf{Q}}_k$移动到高斯Blob内的某个位置。换句话说，我们将未跟踪的影响视为具有协方差的噪声${\mathbf{Q}}_k$。

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

我们可以通过简单地添加${\mathbf{Q}}_k$来获得扩展的协方差 为预测步骤提供完整的表达方式：
$$\begin{array}{rl}{\hat{\mathbf{x}}}_k& ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_k\overrightarrow{{\mathbf{u}}_k}\\ {\mathbf{P}}_k& ={\mathbf{F}}_k{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$

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

而新的不确定性的预测从旧的不确定性，一些从环境额外的不确定性。

好吧，这很容易。我们对系统可能的位置有一个模糊的估计，在预先给定${\hat{\mathbf{x}}}_k$和${\mathbf{P}}_k$的前提下。当我们从传感器中获取一些数据时会发生什么？

通过测量完善估算

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

请注意，读数的单位和小数位可能与我们跟踪的状态的单位和小数位不同。您也许可以猜到这是怎么回事：我们将使用矩阵${\mathbf{H}}_k$对传感器进行建模。

我们可以用通常的方式找出我们希望看到的传感器读数的分布：
$$\begin{array}{rl}{\vec{\mu }}_{\text{expected}}& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k\\ {\mathbf{\Sigma }}_{\text{expected}}& ={\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T\end{array}$$
卡尔曼滤波器的优点之一是处理传感器噪声。换句话说，我们的传感器至少有些不可靠，并且我们原始估计中的每个状态都可能导致一定范围的传感器读数。

从我们观察到的每次阅读中，我们可能会猜测我们的系统处于特定状态。但是由于存在不确定性，因此某些州比其他州更可能产生我们看到的读数：

我们称这种不确定性（即传感器噪声）的协方差${\mathbf{R}}_k$。分布的平均值等于我们观察到的读数，我们将其称为${\vec{\mathbf{z}}}_k$。

因此，现在有两个高斯斑点：一个围绕我们的转换预测的平均值，另一个围绕我们获得的实际传感器读数。

我们必须设法使基于预测状态（粉红色）的读数推测与基于实际观察到的传感器读数（绿色）的推测有所不同。

那么，我们最可能的新状态是什么？对于任何可能的传感器读数$(z_1,z_2)$，我们有2个相关的概率：

(1). 传感器${\vec{\mathbf{z}}}_k$读数是$(z_1,z_2)$的（错误）测量量的概率
(2). 我们预先的估计认为$(z_1,z_2)$正是我们应该看到的读数的概率

如果我们有两个概率，并且我们想知道两个概率都为真的机会，则将它们相乘。因此，我们采用两个高斯斑点并将它们相乘：

我们剩下的就是overlap，这两个斑点都是亮的/可能的区域。而且比我们之前的任何一个估计都精确得多。这种分布的均值是最可能出现两种估计的配置，因此，鉴于我们拥有的所有信息，它是对真实配置的最佳猜测。

嗯 这看起来像另一个高斯斑点。

事实证明，当你乘两个高斯斑点具有独立的手段和协方差矩阵，你会得到一个新的高斯斑点与它自己的均值和方差矩阵！也许您可以看到进展：必须有一个公式可以从旧参数中获取这些新参数！

结合高斯

让我们找到那个公式。在一维中首先看这是最简单的。期望为$\mu$方差为${\sigma }^2$的一维高斯分布曲线被定义如下：
$$\begin{array}{r}\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x–\mu {)}^2}{2{\sigma }^2}}\end{array}$$
我们想知道将两个高斯曲线相乘会发生什么。下面的蓝色曲线表示两个高斯总体的（未归一化）交集：

$$\begin{array}{r}\mathcal{N}(x,{\mu }_0,{\sigma }_0)\cdot \mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,\mu ’,\sigma ’)\end{array}$$
您可以将方程式（9）替换掉（10)中，然后进行一些数学简化（需要注意的是重新进行归一化，然后使得总的概率和为1），然后得到：

$$\begin{array}{rl}\mu ’& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1–{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ {\sigma ’}^2& ={\sigma }_0^2–\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$
我们可以通过删除一小部分并将其命名为$\mathbf{k}$来简化 ：
$$\begin{array}{rl}\mathbf{k}& =\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\\ \mu ’& ={\mu }_0+\mathbf{k}({\mu }_1–{\mu }_0)\\ {\sigma ’}^2& ={\sigma }_0^2–\mathbf{k}{\sigma }_0^2\end{array}$$

请注意如何进行以前的估算并添加一些内容以进行新的估算。看看这个公式有多简单！

但是矩阵版本呢？好吧，让我们重新编写方程式(12)和(13)以获得矩阵形式。如果$\Sigma$是高斯Blob的协方差矩阵，且$\vec{\mu }$是其沿着每个轴的均值（期望），那么：
$$\begin{array}{rl}\mathbf{K}& ={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\\ \vec{\mu }’& =\overrightarrow{{\mu }_0}+\mathbf{K}(\overrightarrow{{\mu }_1}–\overrightarrow{{\mu }_0})\\ \Sigma ’& ={\Sigma }_0–\mathbf{K}{\Sigma }_0\end{array}$$

$\mathbf{K}$是一个称为Kalman增益的矩阵，我们将在稍后使用它。

简单！我们快完成了！

放在一起

我们有两个分布：

预测的测量分布——$({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{P}}_k{\mathbf{H}}_k{\mathbf{H}}_k^T)$；
观测的测量分布——$({\mu }_1，{\Sigma }_1)=({\vec{\mathbf{z}}}_k，{\mathbf{R}}_k)$；

将二者插入等式(15)可以得到他们的重叠部分：
$$\begin{array}{rlrl}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k’& ={\mathbf{H}}_k{\hat{\mathbf{x}}}_k& +& \mathbf{K}(\overrightarrow{{\mathbf{z}}_k}–{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{H}}_k{\mathbf{P}}_k’{\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}$$

从等式（14）中计算得到的卡尔曼增益为：

$$\begin{array}{r}\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}\end{array}$$

观察到有一个${\mathbf{H}}_k$隐含在$\mathbf{K}$中，因此我们可以从（16）、（17）的每个式子中去掉开头的${\mathbf{H}}_k$，然后在计算${\mathbf{P}}_k’$的公式两边去掉尾端的${\mathbf{H}}_k^T$。

$$\begin{array}{rlrl}{\hat{\mathbf{x}}}_k’& ={\hat{\mathbf{x}}}_k& +& \mathbf{K}’(\overrightarrow{{\mathbf{z}}_k}–{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{P}}_k’& ={\mathbf{P}}_k& –& \mathbf{K}’{\mathbf{H}}_k{\mathbf{P}}_k\end{array}$$

$$\begin{array}{rl}\mathbf{K}’& ={\mathbf{P}}_k{\mathbf{H}}_k^T({\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+{\mathbf{R}}_k{)}^{-1}\end{array}$$

…为我们提供了更新步骤的完整方程式。

就是这样！${\hat{\mathbf{x}}}_k’$是我们新的最佳估算值，我们可以继续反馈它（以及${\mathbf{P}}_k’$）返回到我们希望的另一轮预测或更新中。

总结一下

在上述所有数学中，您需要实现的只是方程式（7)、(18)和(19)（或者，如果您忘记了这些内容，则可以从方程式(4)、(15)中重新推导一切）。

这将使您能够准确地对任何线性系统建模。对于非线性系统，我们使用扩展的卡尔曼滤波器，该滤波器通过简单地线性化预测和测量值的均值来工作。（将来我可能会对EKF进行第二次撰写）。

如果我做得很好，希望外面的人会意识到这些东西有多酷，并想出一个意想不到的新地方将它们付诸实践。

搬运工说

感谢Bzarg写的这篇博客，读起来确实是非常通俗易懂，让人感觉学习是一种享受。

更多卡尔曼滤波相关参考文献

1.卡尔曼滤波以及扩展卡尔曼滤波优秀博文汇总
2.图说卡尔曼滤波，一份通俗易懂的教程
3.Kalman滤波学习笔记一《绪论：最优估计》
4.Kalman滤波学习笔记二《数学基础：向量与矩阵》