图解卡尔曼滤波工作原理(翻译)

原文地址: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)}

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

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

4

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

5

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

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

pk=pk1+Δtvk1vk=vk1\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^k1(2)=Fkx^k1(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^k1Pk=FkPk1FkT(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=pk1+Δtvk1+12aΔt2vk=vk1+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^k1+[Δt22Δt]a=Fkx^k1+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^k1\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^k1+BkukPk=FkPk1FkT+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=σ02kσ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)Σ=Σ0KΣ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(zkHkx^k)HkPkHkT=HkPkHkTKHkPkHkT(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^TPk\mathbf{P}_k^{'}方程中所有项的末尾敲掉。

x^k=x^k+K(zkHkx^k)(18)Pk=PkKHkPkK=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上再写一次)。

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


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