图解卡尔曼滤波工作原理（翻译）

原文地址：How a Kalman filter works, in pictures

中文版 + 英文版离线PDF下载地址：蓝奏 5M

正文

我要告诉你卡尔曼滤波器的事，因为它的作用实在太神奇了。

令人惊讶的是，似乎很少有软件工程师和科学家知道这一点，这让我感到悲伤，因为在不确定的情况下结合信息，它是一个如此普遍和强大的工具。有时，它提取准确信息的能力似乎几乎是神奇的-如果听起来我说得太多了，那就看看这个以前发布的视频（国内可以访问，但是无法观看视频），在那里我演示了一个卡尔曼滤波器，通过观察它的速度来找出一个自由漂浮的身体的方向。完全整洁！

它是什么？

您可以在任何有一些动态系统的不确定信息地方使用卡尔曼滤波器，您可以对系统下一步将做什么进行有教育的猜测。即使混乱的现实出现并干扰了你猜测的干净运动，卡尔曼滤波器通常会做一个非常好的工作，找出实际发生了什么。它还可以利用你可能不会想利用的疯狂现象之间的相关性。

卡尔曼滤波器是理想的系统，不断变化。他们的优点是他们对内存依赖很小（他们不需要保留任何历史，除了以前的状态），而且他们非常快，使他们非常适合实时问题和嵌入式系统。

你在谷歌上发现的大多数地方，实现卡尔曼滤波的数学看起来相当可怕和晦涩。这是一个糟糕的状况，因为如果你以正确的方式看待它，卡尔曼滤波器实际上是超级简单和容易理解。因此，它成为一个伟大的文章主题，我将尝试用许多清晰，漂亮的图片和颜色来说明它。先决条件很简单：你所需要的只是对概率和矩阵的基本理解。

我将从卡尔曼滤波器可以解决的那种事情的一个松散的例子开始，但是如果你想正确地获得闪亮的图片和数学，请随时向前跳。

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

让我们做一个玩具例子：你已经建造了一个小机器人，可以在树林里闲逛，机器人需要知道它在哪里，这样它才能导航。

robot

假设我们的机器人有一个状态xk⃗\vec{x_k}，这只是一个位置和一个速度：

xk⃗=(p⃗,v⃗)\vec{x_k} = (\vec{p}, \vec{v})

请注意，状态只是一个关于系统底层配置的数字列表；它可能是任何东西。在我们的例子中，它是位置和速度，但它可以是关于油箱中流体量的数据，汽车发动机的温度，用户手指在触摸屏上的位置，或者你需要跟踪的任何数量的东西。

我们的机器人还有一个GPS传感器，它精确到10米左右，这是很好的，但它需要更精确地知道它的位置超过10米。这些树林里有很多沟壑和悬崖，如果机器人错了几英尺多，它就可能从悬崖上掉下来。 所以GPS本身还不够好。

fail

我们也可能知道机器人是如何移动的：它知道发送给车轮马达的命令，而且它知道，如果它朝一个方向前进，没有任何干扰，在下一个瞬间，它可能会沿着同一个方向进一步前进。但它当然不知道它运动的一切：它可能被风冲击，车轮可能会滑一点，或者在崎岖的地形上滚动；所以车轮转动的数量可能并不完全代表机器人实际上走了多远，预测也不会是完美的。

GPS传感器告诉我们一些关于状态的事情，但只是间接的，而且有一些不确定性或不准确。我们的预测告诉我们一些关于机器人是如何移动的，但只是间接的，并且有一些不确定性或不准确。

但是，如果我们使用我们所掌握的所有信息，我们能得到一个比任何一种估计本身都能给我们的更好的答案吗？当然答案是肯定的，这就是卡尔曼滤波器的作用。

卡尔曼滤波如何看待你的问题？

让我们看看我们试图解释的景观。我们将继续一个简单的状态，只有位置和速度。

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

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

0

卡尔曼滤波假设两个变量（位置和速度，在我们的情况下）都是随机并且符合高斯分布的。每个变量都有一个均值μ\mu，它是随机分布的中心（及其最可能的状态），一个方差σ2\sigma^2，即不确定性：

1

在上面的图片中，位置和速度是不相关的，这意味着一个变量的状态不会告诉你另一个变量可能是什么。

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

2

例如，如果我们根据旧的位置来估计一个新的位置，就可能出现这种情况。如果我们的速度很高，我们可能会移动得更远，所以我们的位置会更远。如果我们走得慢，我们就走不远了。

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

这种相关性被称为协方差矩阵的东西捕获。简而言之，矩阵Σij\Sigma_{ij}的每个元素都是第i状态变量和第j状态变量之间的相关程度。(您可能可以猜测协方差矩阵是对称的，这意味着如果交换i和j不重要)。 协方差矩阵通常被标记为“Σ\Sigma”，因此我们称其元素为“Σij\Sigma_{ij}”。

3

用矩阵描述问题

我们将我们关于状态的知识建模为高斯点（Gaussian blob），所以我们需要在时间kk的两个信息：我们将我们的最佳估计x^k\mathbf{\hat{x}_k}（平均值，其他地方称为μ\mu），以及它的协方差矩阵Pk\mathbf{P_k}。

x^k=[positionvelocity]Pk=[ΣppΣpvΣvpΣvv](1)\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} \text{(1)}

（当然，我们在这里只使用位置和速度，但记住状态可以包含任意数量的变量，并表示您想要的任何东西）。

接下来，我们需要一些方法来查看当前状态(在时间k−1k-1)，并在时间k处预测下一个状态。记住，我们不知道哪个状态是“真实的”状态，但我们的预测函数不在乎。它对所有这些都有效，并给我们一个新的分布：

4

我们可以用矩阵Fk\mathbf{F_k}表示这个预测步骤：

5

如果原始估计是正确的，系统将选中了我们原始估计中的每一点，并将其移动到一个新的预测位置。

让我们应用这个。我们将如何使用矩阵来预测未来下一刻的位置和速度？我们将使用一个真正基本的运动学公式：

pk=pk−1+Δtvk−1vk=vk−1\begin{aligned} \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{aligned}

换句话说：

x^k=[1Δt01]x^k−1(2)=Fkx^k−1(3)\begin{aligned} \color{deeppink}{\mathbf{\hat{x}}_k} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \color{royalblue}{\mathbf{\hat{x}}_{k-1}} &\text{(2)}\\ &= \mathbf{F_k} \color{royalblue}{\mathbf{\hat{x}_{k-1}}} &\text{(3)} \end{aligned}

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

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

嗯，这很容易。我只给你公式：

Cov(x)=ΣCov(Ax)=AΣAT(4)\begin{aligned} Cov(x) &= \Sigma\\ Cov(\color{firebrick}{\mathbf{A}}x) &= \color{firebrick}{\mathbf{A}} \Sigma \color{firebrick}{\mathbf{A}}^T \end{aligned} \text{(4)}

结合公式（3）和（4）：

x^k=Fkx^k−1Pk=FkPk−1FkT(5)\begin{aligned} \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{aligned} \text{(5)}

外部影响

不过，我们没有抓住一切。可能会有一些与状态本身无关的变化——外部世界可能会影响系统。

例如，如果状态模拟列车的运动，列车操作员可能会推动油门，导致列车加速。同样，在我们的机器人示例中，导航软件可能会发出一个命令来转动车轮或停止。如果我们知道这个关于世界上发生的事情的额外信息，我们可以把它塞进一个叫做uk⃗\vec{u_k}的向量里，它做一些事情，并将它添加到我们的预测中作为修正。

假设我们知道由于节气门设置或控制命令而产生的预期加速度a\color{darkorange}{a}，从基本运动学公式我们得到：

pk=pk−1+Δtvk−1+12aΔt2vk=vk−1+aΔt\begin{aligned} \color{pink}{p_k} &= \color{blue}{p_{k-1}} + {\Delta t} &\color{royalblue}{v_{k-1}} + &\frac{1}{2} \color{darkorange}{a} {\Delta t}^2 \\ \color{pink}{v_k} &= &\color{blue}{v_{k-1}} + & \color{orange}{a} {\Delta t} \end{aligned}

改为矩阵形式：

x^k=Fkx^k−1+[Δt22Δt]a=Fkx^k−1+Bkuk⃗(6)\begin{aligned} \color{pink}{\mathbf{\hat{x}}_k} &= \mathbf{F}_k \color{blue}{\mathbf{\hat{x}}_{k-1}} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix} \color{orange}{a} \\ &= \mathbf{F_k} \color{blue}{\mathbf{\hat{x}_{k-1}}} + \mathbf{B}_k \color{orange}{\vec{\mathbf{u}_k}} \end{aligned} \text{(6)}

BkB_k称为控制矩阵，uk⃗\color{darkorange}{\vec{\mathbf{u}_k}}为控制向量。（对于没有外部影响的非常简单的系统，可以省略这些）。

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

外部不确定性

如果状态是根据自身的属性演化的，一切都很好。如果状态是基于外力进化的，只要我们知道那些外力是什么，那么一切都是好的。

但是我们不知道的力量呢？例如，如果我们在跟踪一架四翼飞机，它可能会被风刮来刮去。如果我们跟踪一个轮式机器人，轮子可能会滑动，或者地面上的颠簸可能会减慢它的速度。我们不能跟踪这些事情，如果发生任何这种情况，我们的预测可能会失败，因为我们没有考虑到这些额外的力量。

我们可以通过在每个预测步骤之后添加一些新的不确定性来对“世界”（比如说：我们没有保持跟踪的事情）的不确定性进行建模：

6

我们最初的估计中的每一个状态都可能移动到一系列状态。因为我们非常喜欢高斯点，所以我们会说x^k−1\color{royalblue}{\mathbf{\hat{x}}_{k-1}}中的每个点都被移动到具有协方差Qk\color{mediumaquamarine}{\mathbf{Q}_k}的高斯BLOB内部的某个地方。另一种说法是，我们将未跟踪的影响视为具有协方差Qk\color{mediumaquamarine}{\mathbf{Q}_k}的噪声。

7

这产生了一个新的高斯点，具有不同的协方差（但有相同的平均值）：

8

我们通过简单地添加Qk{\color{mediumaquamarine}{\mathbf{Q}_k}}来获得扩展协方差，给出了***预测步骤***的完整表达式：

x^k=Fkx^k−1+Bkuk⃗Pk=FkPk−1FkT+Qk(7)\begin{aligned} \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{aligned} \text{(7)}

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

而新的不确定性是从旧的不确定性中预测出来的，环境也会带来一些额外的不确定性。

好吧，那就简单了。我们有一个模糊的我们的系统可能在哪里的计，由x^k\color{deeppink}{\mathbf{\hat{x}}_k}和Pk\color{deeppink}{\mathbf{P}_k}给出。当我们从传感器得到一些数据时会发生什么？

用测量值细化估计值

我们可能有几个传感器给我们提供关于我们系统状态的信息。暂时，它们测量什么并不重要；也许一个读取位置，另一个读取速度。每个传感器告诉我们一些关于状态的间接信息-换句话说，传感器在一个状态上工作，并产生一组读数。

9

请注意，读取的单位和规模可能与我们正在跟踪的状态的单位和规模不一样。你也许能猜出这是怎么回事：我们将用矩阵Hk\mathbf{H}_k对传感器进行建模。

10

我们可以用通常的方式计算出传感器读数的分布：

μ⃗expected=Hkx^kΣexpected=HkPkHkT(8)\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} \text{(8)}

卡尔曼滤波器最适合处理传感器噪声。换句话说，我们的传感器至少有点不可靠，我们最初估计的每一个状态都可能导致一系列传感器读数。

11

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

12

我们将称之为这种不确定性的协方差(即：传感器噪声)Rk\color{mediumaquamarine}{\mathbf{R}_k}。分布的平均值等于我们观察到的读数，我们将其称为zk⃗\color{yellowgreen}{\vec{\mathbf{z}_k}}。

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

13

我们必须试图调和我们对基于预测状态（粉红色）的读数的猜测，以及基于我们实际观察到的传感器读数（绿色）的不同猜测。

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

如果我们有两个概率，我们想知道两者都是真的，我们就把它们相乘。所以，我们取两个高斯点并乘以它们：

14

我们剩下的是重叠，这两个斑点都是明亮的。这比我们之前的估计要精确得多。这种分布的平均值是两种估计最有可能实现的配置，因此，考虑到我们拥有的所有信息，这是对真实配置的最佳猜测。

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

15

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

结合高斯

让我们找到那个公式。首先从一个维度来看这个问题是最简单的。一个具有方差σ2σ^2和均值μ\mu高斯钟曲线被定义为：

N(x,μ,σ)=1σ2πe−(x−μ)22σ2(9)\begin{aligned} \mathcal{N}(x, \mu,\sigma) = \frac{1}{ \sigma \sqrt{ 2\pi } } e^{ -\frac{ (x - \mu)^2 }{ 2\sigma^2 } } \end{aligned} \text{(9)}

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

joint

N(x,μ0,σ0)⋅N(x,μ1,σ1)=?N(x,μ′,σ′)(10)\begin{aligned} \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{aligned} \text{(10)}

你可以把方程(9)代入方程(10)，做一些代数（注意重整化，使总概率为1），得到：

μ′=μ0+σ02(μ1−μ0)σ02+σ12σ′2=σ02−σ04σ02+σ12(11)\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} \text{(11)}

我们可以通过分解出一个小片段并称之为k\color{purple}{\mathbf{k}}来简化：

k=σ02σ02+σ12(12)\begin{aligned} \color{purple}{\mathbf{k}} = \frac{\sigma_0^2}{\sigma_0^2 + \sigma_1^2} \end{aligned} \text{(12)}

μ′=μ0+k(μ1−μ0)σ′2=σ02−kσ02(13)\begin{aligned} \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{aligned} \text{(13)}

注意你如何接受你以前的估计，并添加一些东西来做一个新的估计。看看这个公式有多简单！

但是矩阵版本呢？那么，让我们用矩阵形式重写方程（12）和(13）。如果Σ\Sigma是高斯点的协方差矩阵，并且μ⃗\vec{\mu}为沿每个轴的平均值，则：

K=Σ0(Σ0+Σ1)−1(14)\begin{aligned} \color{purple}{\mathbf{K}} = \Sigma_0 (\Sigma_0 + \Sigma_1)^{-1} \end{aligned} \text{(14)}

μ⃗′=μ0⃗+K(μ1⃗−μ0⃗)Σ′=Σ0−KΣ0(15)\begin{aligned} \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{aligned} \text{(15)}

K\color{purple}{\mathbf{K}}是一个称为卡尔曼增益的矩阵，我们将在后续使用它。

别激动！我们快完成了！

把所有的东西放在一起

我们有两个分布：预测量(μ0,Σ0)=(Hkx^k,HkPkHkT)(\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})和观察量(μ1,Σ1)=(zk⃗,Rk)(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})。我们可以把这些代入方程（15），以找到它们的重叠：

Hkx^k′=Hkx^k+K(zk⃗−Hkx^k)HkPk′HkT=HkPkHkT−KHkPkHkT(16)\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} \text{(16)}

而由（14）可知，卡尔曼增益为：

K=HkPkHkT(HkPkHkT+Rk)−1(17)\begin{aligned} \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{aligned} \text{(17)}

我们可以将(16)和(17)中每个项的前面敲掉一个Hk\mathbf{H}_k(注意一个隐藏在K内)，并将一个HkT\mathbf{H}_k^T从Pk′\mathbf{P}_k^{'}方程中所有项的末尾敲掉。

x^k′=x^k+K′(zk⃗−Hkx^k)(18)Pk′=Pk−K′HkPkK′=PkHkT(HkPkHkT+Rk)−1(19)\begin{aligned} \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} ) &\text{(18)} \\ \color{royalblue}{\mathbf{P}_k'} &= \color{deeppink}{\mathbf{P}_k} & - & \color{purple}{\mathbf{K}'} \color{deeppink}{\mathbf{H}_k \mathbf{P}_k} \\ \color{purple}{\mathbf{K}'} &= \color{deeppink}{\mathbf{P}_k} \mathbf{H}^T_k ( \mathbf{H}_k\mathbf{P}_k \mathbf{H}^T_k + \color{mediumaquamarine}{\mathbf{R}_k})^{-1} &\text{(19)} \end{aligned}

得到更新步骤的完整方程。

就这样！x^k′\color{royalblue}{\mathbf{\hat{x}}_k'}是我们新的最佳估计，我们可以继续并将其(连同Pk′\color{royalblue}{\mathbf{P}_k'})反馈到另一轮预测或更新中，只要我们愿意，就可以进行多次更新。

flow

总结

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

这将允许您精确地建模任何线性系统。对于非线性系统，我们使用扩展卡尔曼滤波器，它的工作是简单地线性化预测和测量的平均值。(我将来可能会在EKF上再写一次)。

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

OpenCV
#OpenCV #卡尔曼滤波
图解卡尔曼滤波工作原理（翻译）
https://blog.jackeylea.com/opencv/how-a-kalman-filter-works-in-picture/
作者
JackeyLea
发布于
2020年10月28日
许可协议