图解卡尔曼滤波

from

• How a Kalman filter works, in pictures
  www.bzarg.com/p/how-a-kal…
• Improving IMU attitude estimates with velocity data
  www.bzarg.com/p/improving…

卡尔曼滤波的作用

可以在动态系统中任何有不确定信息的地方使用卡尔曼滤波器，也可以对系统下一步将要干什么做出猜测。即使你想要得到的动作 (motion ) 伴随着大量干扰，卡尔曼滤波器通常都能够得出到底发生了什么。而且它可以利用人们无法读懂的杂乱现象间的相关性。

下面的内容需要对概率和矩阵有一些基本的理解。

一个具体的例子————我们能用卡尔曼滤波做什么

我们举个玩具的例子：你做了一个小机器人，它可以在树林里散步，所以机器人想要导航就需要知道它的精确位置。

我们说机器人有一个状态 $\overrightarrow{x_k}$,包含速度与位置

在这个例子中，状态就是位置和速度，但是它可以是油罐中液体的总量数据、引擎的温度、用户手指在平板上的位置，或者任何你想要观察的数据。

我们的机器人有一个 GPS ，精确约为 10米，状态良好，但是我们对机器人定位的精度要求高于10米。树林里有很多沟和崖，如果机器人走错几英尺，就可能跌落悬崖。所以仅有 GPS 是不够的。

我们还需要知道机器人是怎样移动的：它知道发送给马达的命令，知道是否朝向某个方向，没有障碍，下一步，它可能就沿着这个方向前进。当然，它不可能知道运动过程中的所有状况：它可能被风刮倒，车轮可能打滑，或者在颠簸的路面侧翻；所以轮子的转角并不能精确的表示机器人走了多远，这个预测不是完美的。

GPS 告诉我们一些状态，但只是间接的，并且带有一些不确定性和干扰。我们 预测 机器人是如何运动的，但也是间接的，带有不确定性和干扰。

但是，如果我们使用所有能够获取的信息，我们可以得到一个比这两个由各自传感器得到的估计更好的答案吗？答案当然是肯定的，这就是卡尔曼滤波器要做的。

卡尔曼滤波如何看待这个例子

让我们看看试图解释的场景。继续使用只有位置和速度的状态。

我们并不知道实际的位置和速度；有一系列可能的位置与速度的组合，但是其中的一些可能性更高：

卡尔曼滤波器假设所有的变量（本例中为：位置、速度）都是随机高斯分布（正态分布）的。每个变量都有一个期望$\mu$，方差$\sigma^2$：

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

下面的例子中：位置和速度是相关的。一个指定位置的可能观测值取决于速度：

这种情况一般发生在：我们根据之前的位置估计当前位置。如果速度很快，就会移动很远。反之，就会很近。

这种关系在跟踪定位时非常重要，因为这会提供更多信息：一个测量值告诉我们其他测量值的可能范围。这就是卡尔曼滤波器的目标，我们希望从不确定的测量值中得到尽可能多的信息。 这种相关性由协方差矩阵给出。简单来说，协方差矩阵的每个元素$\sum_{ij}$都是 第i个状态变量和第j个状态变量的相关程度。（所以协方差矩阵是对称的，交换i和j并不影响）。协方差矩阵通常用“$\sum_{}$”表示，所以将其元素称为“$\sum_{ij}$”。

用矩阵描述问题

对高斯斑点进行建模，我们需要两个信息：在k时刻，最佳估计 $\hat{x}$（期望$\mu$），协方差矩阵$P_k$

(公式中只使用了本例中的位置和速度。)

接下来，我们看一下当前状态（时刻：k-1 ）和预测下一状态 （时刻：k ）。我们并不知道哪个状态是真实的，但是预测方程并不关心。它根据所有可能状态，给出一个新的分布：

我们可以用矩阵$F_k$表示该预测步骤：

它根据原始状态的每一个点，得到新的预测。（如果原始状态中包含真实状态，则预测状态中也包含真实状态）。

我们怎样使用一个矩阵来预测下一时刻的位置和速度？我们将使用基本的运动学方程：

即：

我们现在有了预测下一状态的预测矩阵，但是任然不知道怎样更新协方差矩阵。

我们需要另一个方程。如果我们对分布中的每一个点乘以一个矩阵 AA ，那么协方差矩阵 ΣΣ

这比较简单，我将直接给出：

由 $(4)$式和$(3)$式可得：

外部影响

不过，我们还没有捕捉到所有信息。可能有一些与状态本身无关的变化————外部世界可能会影响系统。

例如，如果状态模拟火车的运动，火车操作员可能会踩油门，导致火车加速。如果我们知道这个关于世界上正在发生的事情的额外信息，我们可以将它填充到一个名为$\overrightarrow{u_k}$的向量中，将它作为修正添加到我们的预测中。

假设我们知道由于油门设置或控制命令导致的预期加速度a。从基本运动学我们得到(偷个懒，直接截图公式)：

矩阵形式：

$B_{k}$被称为控制矩阵，$\overrightarrow{u_k}$被称为控制向量。

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

外部的不确定性

如果状态只根据自身的属性变化，则没有问题。如果状态同时根据外部作用变化，只要我们知道外部作用力的属性，也没有问题。

但是，如果是我们不知道的力呢？比如，四旋翼在飞行中会受到阵风的影响。轮式机器人的轮子可能打滑。如果这种情况发生，而我们没有考虑这些外部因素，我们将无法继续保持对机器的追踪。

我们可以在每一步预测之后增加一些新的不确定因素，以完成对机器人相关的不确定性建模。

在原始估计中的每一个状态都能够转移到一定状态范围。可以说，因为高斯斑，在 $\hat{x_{k-1}}$的每一个点，都移动到了一个协方差为$Q_k$的高斯斑中。换一种说法就是，我们将不确定的影响看作是协方差$Q_k$的噪声。

这会产生一个新的高斯斑，有新的协方差（相同的期望）：

这个扩展的协方差通过加$Q_k$得到，所以完整的预测表达式：

也就是说，新的最优估计是由上一时刻的最优估计进行预测，加上已知外部影响的修正得到的。

新的不确定性是由上一时刻的不确定性进行预测，加上一些环境中额外的不确定性。

现在，我们有了系统当前状态的模糊估计$\hat{x_k}$ 和$P_{k}$。接下来看看从传感器获取数据时发生了什么。

测量值校正估计

我们可能有多个能反映系统状态的传感器。目前，它们测量的是什么并不影响；可能一个测量位置，其他测量速度。每个传感器都间接的给出了一些状态信息，也可以说是传感器作用于一个状态，并产生了一系列读数。

读数的单位和量程可能与状态的不同，我们用矩阵$\boldsymbol{H_k}$对传感器建模。

我们能够得到传感器读数的分布：

卡尔曼滤波适合于处理传感器的噪声。也就是说，传感器多少都会有些不可靠性，原始估计中的每一个状态的传感器读数都会是一个范围值。

从我们观测到的每一个读数，我们可能猜测系统在一个特定的状态。但是因为存在不确定性，观测到的数据中某些状态的可能性会比较高。

称这个不确定性（传感器噪声）的协方差为$\boldsymbol{R_k}$。该分布的期望和观测到的读数相同 ，称为$\overrightarrow{z_k}$

现在，我们得到两个高斯斑：一个表示系统状态预测；一个表示传感器读数。

我们必须设法协调基于预测状态的猜测和基于传感器读数的猜测。

所以，我们的新状态是什么呢？ 对于任何可能的读数 $(z_1,z_2)$ ，有两个相关的概率：（1）传感器读数 $\overrightarrow{z_k}$是$(z_1,z_2)$的（误）测量，（2）之前的估计认为 $(z_1,z_2)$就是我们应该看到的读数。

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

剩下的是重叠部分，原来两个高斯斑都较 亮/可能 的部分。这个结果比之前两个高斯斑都更精确。这个分布的期望是根据两个估计都是最可能的配置得到的，因此，这就是根据我们所有信息能够给出的真实配置的最佳猜测。

恩。。。看起来像是另一个高斯斑。

事实上，当你将两个不同的高斯斑相乘的时候，就得到了一个全新的高斯斑！ 我们正在做的就是：得到一个方程，可以从之前的状态获取新的状态。

与高斯分布的结合

首先，来看最简单的一维情况。一维高斯曲线（方差：$\sigma^2$， 期望： $\mu$）：

两个高斯曲线相乘的情况，下面的蓝色曲线表示两个高斯总体的（未归一化的）交集：

将(9)式带入 (10)式中，得到：

简化为：

注意你怎样通过之前的估计添加修正，获得当前的估计。看看这个方程多么简单！

将式(12)和(13)重写为矩阵形式。假设 Σ 是协方差矩阵，$\overrightarrow{\mu}$为期望。

$\boldsymbol{k}$ 被称为卡尔曼增益，我们会在迭代的过程中使用它。

综上所述

总结

文中所有的数学公式，你需要使用的是：(7)(18)(19)。（如果你忘了这些，还可以从式(4)(15)重新推导）

这可以对任何线性系统进行精确建模。对于非线性系统，可以使用扩展卡尔曼滤波，就是对期望值的预测和测量线性化。（我可能在以后再写一个 EKF的文章）。

如果这篇文章写的还不错，希望人们能够意识到卡尔曼滤波有多么酷，并把它应用到一些新的领域。

---

相关的推导和证明可以参看这篇文章
文中使用了相似的方法介绍重叠的高斯分布。如果感兴趣的话，可以在文章中找到深入的推导。