原文: https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

1 概述

你可以对某个动态系统有不确定信息的任何地方使用卡尔曼滤波器，并且对系统下一步的状态做出有根据的猜测。即使出现混乱的现实状态，卡尔曼滤波器都会给出一个合理的结果。

卡尔曼滤波器非常适合连续变化的系统。它具有占用内存少的优点，而且速度非常快，这使得它非常适合处理实时问题和应用在嵌入式系统中。

卡尔曼滤波器实际上非常简单，如果你以正确的方式看待它，非常容易理解。我将尝试用大量图片来阐明它。先决条件很简单：你只需要对概率和矩阵有基本的了解。

我将以卡尔曼滤波器可以解决的问题的简单示例开始。

2 我们可以用卡尔曼滤波器做什么?

让我们举一个玩具示例：你建造了一个可以在树林中漫步的小型机器人，并且机器人需要准确地知道它在哪里，以便可以导航。

我们会说机器人有一个状态 $\overrightarrow{x_k}$，状态包含了位移 (position) 和速度 (velocity):

$$\overrightarrow{x_k}=(\vec{p},\vec{v})$$

需要注意，状态只是关于系统底层配置的数字列表，它可以是任何变量。在我们的示例中，他是位移和速度，但它可以是油箱中的液体量、汽车发动机的温度、用户手指在触摸板上的位置数据，或者是你需要跟踪的任何数量的事物。

我们的机器人配备了 GPS 传感器，其精度约为 10 米，这很不错，但它需要比 10 米更精确地知道自己的位置。在树林里有很多沟壑和悬崖，如果机器人的误差超过几英尺它就可能会掉下悬崖，因此仅靠 GPS 是不够的。

我们可能还知道一些有关机器人如何移动的信息：它知道发送给车轮马达的命令，并且知道如果它朝一个方向前进并且没有任何东西干扰，那么在下一刻它很可能会沿着同一个方向走得更远。但是它当然并不了解其运动得全部：它可能会受到风得冲击，车轮可能会稍微打滑，或者在颠簸得地形上滚动，因此车轮转动的量可能并不准确代表机器人实际行驶了多远，并且预测不会是完美的。

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

但是，如果我们利用所有可用的信息，我们能否得到比单独估计更好的答案呢？答案当然是肯定的，这就是卡尔曼滤波器的用途。

3 卡尔曼滤波器分析问题的视角

让我们看看我们试图描述的场景。我们继续使用只有位置和速度的简单状态。

$$\vec{x}=\left[\begin{array}{c}p\\ v\end{array}\right]$$

我们不知道实际的位移和速度是多少；位移和速度的可能组合范围可能很广，但其中一些组合比其他组合更有可能：

卡尔曼滤波器假设两个变量（在我们的例子中是位移和速度）都是随机的，并且服从高斯分布 (Gaussian distributed)。每个变量都有一个平均值 $\mu$，即随机分布的中心（也是最可能的状态），以及方差 ${\sigma }^2$，即不确定性：

在上图中，位移和速度是不相关的 (uncorrelated)，这意味这一个变量的状态并不能告诉你另一个变量的状态。

下面的例子显示了更有趣的东西：位移和速度是相关的。观察到特定位移的可能性取决于你的速度：

例如，如果我们根据旧位置估计新位置，则可能会出现这种情况。如果我们的速度很高，我们可能会移动得更远，因此位移会更远；如果我们移动得很慢，就不会走那么远。

跟踪这种关系确实很重要，因为它能给我们提供更多信息：一次测量可以告诉我们其他测量可能是什么。这就是卡尔曼滤波器得目标，我们希望从不确定得测量中尽可能多地获得信息。

这种相关性可以用协方差矩阵 (covariance matrix) 来表示。简而言之，矩阵的每个元素 ${\Sigma }_{ij}$ 是第 i 个变量和第 j 个变量之间的相关程度。可以猜到，协方差矩阵是对称的，这意味着交换 i 和 j 无关紧要。协方差矩阵通常标记为 “$\Sigma$”，所以我们称他们的元素为 “${\Sigma }_{ij}$”。

4 用矩阵描述问题

我们将关于状态的知识建模为高斯斑点 (Gaussian blob)，因此我们同时需要两条信息，我们将给出最佳估计值 ${\hat{\mathbf{x}}}_k$，(在其他地方成为 $\mu$ ) 及其协方差矩阵 ${\mathbf{P}}_k$ 。

$$\begin{array}{c}\begin{array}{r}{\hat{\mathbf{x}}}_k=\left[\begin{array}{c}\text{position}\\ \text{velocity}\end{array}\right]\\ {\mathbf{P}}_k=\left[\begin{array}{c}{\Sigma }_{pp}{\Sigma }_{pv}\\ {\Sigma }_{vp}{\Sigma }_{vv}\end{array}\right]\end{array}\end{array}$$

当然，我们这里只使用位移和速度，但状态可以包含任意数量的变量并表示你想要的任何内容。

接下来，我们需要某种方法来查看当前状态(时间 k-1)，并预测在时间 k 的下一个状态。其实我们不知道哪个状态是真实的状态，我们的预测函数也并不关心，它只对所有的状态起作用，并为我们提供一个新的分布：

我们可以用矩阵 ${\mathbf{F}}_k$ 来表示这个预测步骤：

它会将我们原始估计中的每个点映射到新的预测位置，如果原始估计是正确的，系统会移动到该位置。

让我们应用一下，如何使用矩阵来预测未来一刻的位移和速度，我们将使用一个非常基本的运动学公式：

$$\begin{array}{rlr}& 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}{c}1\Delta t\\ 01\end{array}\right]{\hat{\mathbf{x}}}_{k-1}\\ & \\ & ={\mathbf{F}}_k{\hat{\mathbf{x}}}_{k-1}\end{array}$$

我们现在有一个预测矩阵，它将当前状态映射到下一个状态，但我们仍然不知道如何更新协方差矩阵。

这时我们需要另一个公式。如果我们将分布中的每个点乘以一个矩阵，那么协方差矩阵会发生什么？

这个很简单，我们直接写公式：

$$\begin{array}{rl}Cov(x)& =\Sigma \\ Cov(\mathbf{A}x)& =\mathbf{A}\Sigma {\mathbf{A}}^T\end{array}$$

将方程 (4) 与 方程 (3) 结合：

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

4.1 外部影响

不过，我们还没有捕捉到任何东西。可能有一些与状态本身无关的变化，外部世界可能正在影响系统。

例如，如果用状态模拟火车的运动，火车操作员可能会按下油门导致火车加速，同样，在我们的机器人示例中，导航软件可能会发出车轮转动或者停止的命令。如果我们知道关于外界正在发生事情的额外信息，我们可以在向量 $\overrightarrow{u_k}$ 里加入一个成员，用它做点什么，并把它添加到我们的预测中作为更正。

比方说，我们知道预期的加速度 $a$ 是由油门或者控制指令控制。从基本的运动学中，我们得到：

$$\begin{array}{rlr}{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{u_k}$称为控制向量。（对于没有外部影响的非常简单的系统，你可以省略这些）。

让我们再添加一个细节。如果我们的预测不是 100% 准确的模型会发生什么。

4.2 外部不确定性

如果状态根据自身属性演变一切都还好。如果状态是根据外力演变的，只要我们知道这些外力是什么，一切也都会还好。

但是我们不知道的力量呢？例如，如果我们在追踪一架四轴飞行器，它可能会被风吹来吹去。如果我们跟踪一个轮式机器人，轮子可能会打滑，或者地面上的颠簸可能会让它变慢。我们无法跟踪这些事情，如果发生任何这种情况，我们的预测可能会出错，因为我们没有考虑到这些额外的力量。

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

在我们最初的估计中，每个状态都可能转移到一系列状态。因为我们非常喜欢高斯斑点，所以我们会说每个点${\hat{x}}_{K-1}$被移动到具有协方差 ${\mathbf{Q}}_k$ 的高斯斑点中的某个点。另一种说法是，我们将未跟踪的影响视为具有协方差的噪声 ${\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}}_{\mathbf{k}}{\mathbf{P}}_{k-1}{\mathbf{F}}_k^T+{\mathbf{Q}}_k\end{array}$$

换句话说，新的最佳估计是从以前的最佳估计中得出的预测，并加上对已知外部影响的修正。

新的不确定性是从旧的不确定性中预测的，还有一些来自环境的额外不确定性。

好吧，那就够了。我们对这个系统可能在哪里有一个模糊的估计，给出了 ${\hat{\mathbf{x}}}_k$ 和 ${\mathbf{P}}_k$ 。那么当我们从传感器获取一些数据时又会发生什么？

5 通过测量完善估算

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

请注意，读数的单位和尺度可能与我们正在跟踪的状态的单位和尺度不同。你也许能猜到接下来要做什么：我们将用矩阵 ${\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{P}}_k$ 。分布的平均值等于我们观察到的读数，我们将称之为 $\overrightarrow{{\mathbf{z}}_k}$ 。

所以现在我们有两个高斯点：一个围绕我们变换预测的平均值，一个围绕我们得到的实际传感器读数。

我们必须尝试将我们对基于预测状态（粉红色）的读数的猜测与我们实际观察到的传感器读数（绿色）的不同猜测相调和 (reconcile)。

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

• (1) 我们的传感器读取的概率 $\overrightarrow{{\mathbf{z}}_k}$ 是一个（错误的）测量 $(z_1,z_2)$；
• (2) 我们之前的估计认为的概率 $(z_1,z_2)$ 是我们应该看到的读数。

如果我们有两个概率，并且我们想知道两个概率都是真的，我们只需将它们相乘即可。因此，我们取两个高斯斑点并乘以它们：

剩下的是重叠，即两个斑点都明亮（可能）的区域。它比我们之前的任何一个估计都要精确得多。这个分布的平均值是两种估计最有可能的配置，因此，鉴于我们掌握的所有信息，是对真实配置的最佳猜测。

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

事实证明，当你用单独的均值和协方差矩阵乘以两个高斯点时，你会得到一个新的高斯斑点，它有自己的平均值和协方差矩阵。也许你可以看到这会去哪里：必须有一个公式来从旧参数中获取那些新参数。

6 组合高斯

让我们找到那个公式。首先从一个维度看这个是最简单的。一个协方差为 ${\sigma }^2$，均值为 $\mu$ 的高斯曲线定义 为：

$$\begin{array}{c}\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}\end{array}$$

我们想知道当你把两条高斯曲线相乘时会发生什么。下面的蓝色曲线代表了两个高斯曲线（未归一化）的交集：

$$\begin{array}{c}\mathcal{N}(x,{\mu }_0,{\sigma }_0)\cdot \mathcal{N}(x,{\mu }_1,{\sigma }_1)\stackrel{?}{=}\mathcal{N}(x,{\mu }^{\prime },{\sigma }^{\prime })\end{array}$$

你可以将方程 (9) 代入方程 (10) 并做一些代数变换（小心重新归一化，使总概率为1）以获得：

$$\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}$$

我们可以提取出系数 $\mathbf{k}$：

$$\begin{array}{c}\mathbf{k}=\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\end{array}$$

$$\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}$$

记下如何进行之前的估算，并添加一些东西来进行新的估算。看看那个公式有多简单！

但是矩阵版本呢？好吧，我们把 (12) 和 (13) 写成矩阵形式。如果 $\Sigma$ 是高斯斑点的协方差矩阵，$\vec{\mu }$ 是它沿着每个轴线的平均数，然后：

$$\begin{array}{c}\mathbf{K}={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\end{array}$$

$$\begin{array}{rl}\overrightarrow{{\mu }^{\prime }}& =\overrightarrow{{\mu }_0}+\mathbf{K}(\overrightarrow{{\mu }_1}-\overrightarrow{{\mu }_0})\\ {\Sigma }^{\prime }& ={\Sigma }_0-\mathbf{K}{\Sigma }_0\end{array}$$

$\mathbf{K}$ 是一个叫做卡尔曼增益的矩阵，我们一会儿就会使用它。

放松，我们快完成了！

7 综合

我们有两种分布：

• The predicted measurement: $({\mu }_0,{\Sigma }_0)=({\mathbf{H}}_k{\hat{\mathbf{x}}}_k,{\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T)$ ;
• The observed measurement: $({\mu }_1,{\Sigma }_1)=(\overrightarrow{{\mathbf{z}}_k},{\mathbf{R}}_k)$ ;

我们可以把这些插入到方程中(15) 找到他们的重叠：

$$\begin{array}{rlr}{\mathbf{H}}_k{\hat{\mathbf{x}}}_k^{\prime }& ={\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^{\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}$$

并且从 (14) 得到，卡尔曼增益是：

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

我们可以把 (16) 和 (17) 中的 ${\mathbf{H}}_k$ 替换掉（注意夹在中间的 $\mathbf{K}$），以及 ${\mathbf{P}}_k$ 后面的 ${\mathbf{H}}_k^T$ .

$$\begin{array}{rl}{\hat{\mathbf{x}}}_k^{\prime }& ={\hat{\mathbf{x}}}_k+\mathbf{K}(\overrightarrow{{\mathbf{z}}_k}-{\mathbf{H}}_k{\hat{\mathbf{x}}}_k)\\ {\mathbf{P}}_k^{\prime }& ={\mathbf{P}}_k-\mathbf{K}{\mathbf{H}}_k{\mathbf{P}}_k\end{array}$$

$$\begin{array}{c}{\mathbf{K}}^{\prime }={\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^{\prime }$ 进行反馈，来到另一轮预测或更新，想多少次就多少次。

8 总结

在上述所有数学推导中，你只需要实现方程 (7)，(18) 和 (19).（或者如果你忘记了那些，你可以从方程中重新推导一切 (4) 和 (15) ）

这将允许您准确地对任何线性系统进行建模。对于非线性系统，我们使用扩展卡尔曼滤波器，它通过简单地将有关其平均值的预测和测量线性化来工作。（我将来可能会做第二份关于 EKF 的文章）。

如果我的工作做得很好，希望其他人会意识到这些事情有多酷，并想出一个意想不到的新地方把它们付诸行动。