卡尔曼滤波

tip

本文主要参考国外博主Bzarg在2015年写的一篇图解再配合自己的对kalman滤波的理解 英文文章链接：https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/

什么是卡尔曼滤波

如果你现在一个动态的系统当中去预测不确定的信息，那么你就可以使用卡尔曼滤波。使用卡尔曼滤波你可以对系统的下一步动向做出一个有根据的预测。 即使系统受到了一定的外部扰动卡尔曼滤波依然能够在大多数时候很出色地预测出结果。它可以利用混乱现象中你可能都想不到的相关性。 对于预测连续变化的系统的下一步行为时kalman滤波是一个好帮手。它的优势在于他在占用很少内存（不需要存储任何历史和先前状态）的同时，计算输出非常快。这使得卡尔曼滤波可以很好的应用在实时的问题和嵌入式系统当中。

但是呢，网络上的卡尔曼滤波的数学证明和公式都非常的恐怖。实际上，如果你用正确的方式去学习卡尔曼滤波就会发现它真的非常的简洁和简单。这是一个很不错的文章主题（doge）。我将会尝试用清晰和漂亮的图片来解释卡尔曼滤波。预备知识也非常的简单，只要你对概率论有基础的了解，你就可以看懂这篇文章。

卡尔曼滤波有什么用？

让我们用一个玩具作为例子：你现在制作了一个小机器人，它可以在树林里自由的奔跑，但是这个小机器人需要知道自己的确切位置，这样它才可以进行导航。

我们将小机器人的位置状态用$\hat{x}_k$ （postion和velocity）来表示：

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

tip

我们使用一堆数字来表示系统的基本状态，不同的系统的状态都不一样。这个例子中是位置和速度，但是它也可以是油箱中的液体量，汽车引擎的温度，使用者手指在触摸板上的位置，或者任何你需要记录的东西。

我们的机器人配备了可以精确到10米以内的GPS导航，这已经很好了。但是我们需要知道精度小于10米的位置。在这个树林当中有很多的沟壑和悬崖，如果我们的机器人多走错几部，它可能就要嗝屁了。所以只靠GPS定位是完全不够的。

我们还需要知道机器人是如何移动的：机器人知道发送到车轮发动机的指令，它也知道如果它朝着一个方向前进而没有任何干扰，它就可以沿着这个方向继续移动。当然了，它并不知道它移动的中的所有信息，比如：它可能会受到分的冲击，轮子可能会打滑，有可能在崎岖不平的道路上移动。所以转动轮子的数量并不能精准的表示小机器人实际移动的距离。

GPS可以告诉我们一些间接的状态信息而是有一定的不确定性和不准确性。预测可以告诉我们机器人如何移动，但也只是简单的信息且具有不确定性和不准确性。

但是如果我们使用所有可以获取的信息，我们能不能得到一个更好的方法呢？答案是肯定的，这就是Kalman滤波的作用。

tip

Kalman滤波可以利用系统能够获取的所有信息来对系统的行为做出更加准确的预测

Kalman是“看”问题的

让我们继续从只有位置速度信息的和简单状态开始；

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

我们并不知道机器人准确的位置和速度信息。有一系列可能的位置和速度的组合可能是正确的，这些组合当中有的组合是更准确的。

卡尔曼滤波算法认为中所有的变量都是随机的且符合高斯分布，每一个变量都有一个平均值$\mu$, 这个平均值是随机分布的中心（更加像真实的状态）。方差$\sigma^2$用来衡量不确定度， 如下图所示：

在上面的图片中，位置和速度信息是不准确的（它是一团范围）。这意味着一个状态中的一个变量并不能告诉你另一个变量。

下面的例子中展示了一个更加有趣的东西。位置和速度信息是准确的。观察一个特定位置的可能性只取决于你的速度:

这种情况可能会发生，比如我们正在根据老的位置信息来推测一个新的位置信息。如果我们的速度非常的快，我们在预测下一步的位置时就会预测在更远的位置。

这样的关系对于保持对机器人位置的追踪是非常重要的，因为它给了我们更多的信息。它提供了一个可以衡量可能性的标准。这就是卡尔曼滤波的目标：从不确定信息中挤出尽可能多的信息！

我们可以使用协方差矩阵来表述这种相关性。简单来说，每一个矩阵中的元素表明第i个变量和第j个变量的相关程度。我们使用$\sum$来表示协方差矩阵，在这个例子中协方差矩阵可以表示为$\sum_{i,j}$.

使用协方差矩阵来描述问题

我们使用高斯分布来对系统状态进行建模。我们需要在时间k时需要获取两个信息：最佳估计距离$\hat{x}_k$ （均值，也就是 $\mu$ )和 协方差矩阵$P_k$。

$$\begin{aligned} \hat{\mathbf{x}}_{k} &=\left[\begin{array}{l} \text { position } \\ \text { velocity } \end{array}\right] \\ \mathbf{P}_{k} &=\left[\begin{array}{ll} \Sigma_{p p} & \Sigma_{p v} \\ \Sigma_{v p} & \Sigma_{v v} \end{array}\right] \end{aligned}$$

tip

我们在这里还是只使用了位置和速度信息，但是这对于状态数组可以包含任意数量的变量，表示任意你像表示的东西。

接下来我们需要一些方法去观察当前的状态（时间为k-1）然后去初步预测出一个在时间k时的状态。请记住我们并不知道哪一个状态是真实的，但是我们的初步预测并不关注这一点，可以直接给出新的分布。

我们可以使用矩阵$F_k$来表示这个预测步骤：

它把我们初始位置的每一个点移动到一个新的预测位置，如果原来的估计是正确的，系统就会移动到这个位置。

让我们来实现这个事情，我们应该如何使用矩阵来预测未来下一个时刻的位置和速度？我们将使用最基础的运动学公式：

$$\begin{array}{lr} p_{k}=p_{k-1}+\Delta t v_{k-1} \\ v_{k}= v_{k-1} \end{array}$$

换一个形式就是：

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

现在我们有一个预测矩阵，它将会给出我们下一步的状态。但是我们还不不知道如何去更新协方差矩阵。

这里我们需要另一个公式。首先我们先想一下，如果使用矩阵A去乘以分布中的每一个点，协方差矩阵将会怎么样嘞？

这很简单，它的定义如下：

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

结合上述的两个公式我们将会得到：

$$\begin{aligned} &\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{aligned}$$

外部影响

但是，除了速度和位置，外因也会对系统造成影响。比如模拟火车运动时，除了列车自驾系统，列车操作员可能会手动调速。在我们的机器人示例中，导航软件也可以发出停止指令。对于这些信息，我们把它作为一个向量$\vec{u}_{k}$ ， 纳入预测系统作为修正。

假设油门设置和控制命令是已知的，我们知道火车的预期加速度a， 根据运动学基本定理，我们可得：

$$\begin{aligned} p_{k} &=p_{k-1}+\Delta t v_{k-1}+\frac{1}{2} a \Delta t^{2} \\ v_{k} &=\quad v_{k-1}+a \Delta t \end{aligned}$$

其中$p_k$表示第k时刻的位置，$v_k$表示第k时刻的速度，我们把它转成矩阵形式：

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

$B_k$是控制矩阵， $\vec{u}_{k}$是控制向量。如果外部环境异常简单，我们可以忽略这部分内容，但是如果添加了外部影响后，模型的准确率还是上不去，这又是为什么呢？

外部的不确定性

当一个国家只按照自己的步子发展时，它会自生自灭。当一个国家开始依赖外部力量发展时，只要这些外部力量是已知的，我们也能预测它的存亡。

但是，如果存在我们不知道的力量呢？当我们监控无人机时，它可能会受到风的影响；当我们跟踪轮式机器人时，它的轮胎可能会打滑，或者粗糙地面会降低它的移速。这些因素是难以掌握的，如果出现其中的任意一种情况，预测结果就难以保障。

这要求我们在每个预测步骤后再加上一些新的不确定性，来模拟和“世界”相关的所有不确定性：

如上图所示，加上外部不确定性后， $\hat{x}_k$都可能会移动到另一点，也就是蓝色的高斯分布会移动到紫色高斯分布的位置，并且具有协方差 $Q_k$ 换句话说，我们把这些不确定影响视为协方差$Q_k$的噪声。

这个紫色的高斯分布拥有和原分布相同的均值，但协方差不同。

我们在原式上加上$Q_k$

$$\begin{aligned} &\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{aligned}$$

简而言之， 新的最佳估计是基于原最佳估计和已知外部影响校正后得到的预测。 新的不确定性是基于原不确定性和外部环境的不确定性得到的预测。

现在，有了这些概念介绍，我们可以把传感器数据输入其中。

通过测量来细化估计值

我们可能有好几个传感器，它们一起提供有关系统状态的信息。传感器的作用不是我们关心的重点，它可以读取位置，可以读取速度，重点是，它能告诉我们关于状态的间接信息(sensor reading)。

请注意，通过传感器得到的状态间接信息的规模和状态信息不一定相同，所以我们把传感器读数矩阵设为$H_k$。

把这些分布转换为一般形式：

$$\begin{aligned} \vec{\mu}_{\text {expected }} &=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} \\ \boldsymbol{\Sigma}_{\text {expected }} &=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T} \end{aligned}$$

卡尔曼滤波的一大优点是擅长处理传感器噪声。换句话说，由于种种因素，传感器记录的信息其实是不准的，一个状态事实上可以产生多种读数。

对于每一个我们观察到的读数，我们可能会猜测我们的系统正处于一个特殊的状态。但是由于系统是具有不确定的，有些状态的间接参数可能更接近我们说观察到的间接参数。

我们将这种不确定性（即传感器噪声）的协方差设为$R_k$，读数的分布均值设为$z_k$。现在我们得到了两块高斯分布，一块围绕预测的均值，另一块围绕传感器读数。

我们必须根据预测的状态（粉色）和实际观察到的传感器读数（绿色）之间的不同来调整我们的最终的预测结果。

所以我们要如何得到最接近真实情况的状态呢？ 对于任意可能的状态$(z_1,z_2)$, 我们有两个相关概率 （1）我们的传感器读数$\overrightarrow{z_{k}}$是$(z_1,z_2)$的错误量度指标的概率。（2）我们之前的估计认为$(z_1,z_2)$是我们应该看到的读数的概率。

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

如上图所示，我们留下了两个分布重合的部分（两个分布都是亮的）。这要比我们之前估计的要精准的多。这个分布的平均值是这两种估计最有可能的情况，因此在给定所有信息的情况下，这是对真实情况的最佳猜测。

这看起来像是另一个高斯分布。

事实证明，当你用具有不同的平均值和协方差矩阵的Gaussian blob相乘时，你将会得到一个新的 Gaussian blob，它将具有它自己的均值和方差。也许你可以看到这个问题的发展方向: 一定有一个公式可以从旧的参数中得到这些新的参数！

结合高斯分布

让我们来找到那个公式，最简单的方法就是从一维的角度来看待这个问题。一个一维的高斯曲线具有方差$\sigma^2$和平均值$\mu$的定义如下（这熟悉的概率论公式）：

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

我们想知道将两个高斯曲线相乘后悔发生什么。下图中蓝色的曲线表示两个高斯分布相乘后所得的结果。

$$\mathcal{N}\left(x, \mu_{0}, \sigma_{0}\right) \cdot \mathcal{N}\left(x, \mu_{1}, \sigma_{1}\right) \stackrel{?}{=} \mathcal{N}\left(x, \mu^{\prime}, \sigma^{\prime}\right)$$

将上面两个公式整理一下，做一些代数运算(小心地归一化，使得总概率为1) 我们可以得到 :

$$\begin{aligned} \mu^{\prime} &=\mu_{0}+\frac{\sigma_{0}^{2}\left(\mu_{1}-\mu_{0}\right)}{\sigma_{0}^{2}+\sigma_{1}^{2}} \\ \sigma^{\prime 2} &=\sigma_{0}^{2}-\frac{\sigma_{0}^{4}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \end{aligned}$$

对上式进行一定的整理我们可以得到：

$$\begin{aligned} \mathbf{k} &=\frac{\sigma_{0}^{2}}{\sigma_{0}^{2}+\sigma_{1}^{2}} \\ \mu^{\prime} &=\mu_{0}+\mathbf{k}\left(\mu_{1}-\mu_{0}\right) \\ \sigma^{\prime^{2}} &=\sigma_{0}^{2}-\mathbf{k} \sigma_{0}^{2} \end{aligned}$$

记下你可以如何利用你之前的估算，然后加上一些东西，做出一个新的估算。看看这个公式有多简单！

但是我们要如何得到矩阵形式呢？我们只需要重写一下上面这个公式即可。设$\sum$是高斯曲线的协方差矩阵，$\vec{\mu}$表示每一个轴上的平均值。我们就可以得到：

$$\begin{aligned} \mathbf{K} &=\Sigma_{0}\left(\Sigma_{0}+\Sigma_{1}\right)^{-1} \\ \vec{\mu}^{\prime} &=\overrightarrow{\mu_{0}}+\mathbf{K}\left(\overrightarrow{\mu_{1}}-\overrightarrow{\mu_{0}}\right) \\ \Sigma^{\prime} &=\Sigma_{0}-\mathbf{K} \Sigma_{0} \end{aligned}$$

K这个矩阵就叫做卡尔曼增益矩阵，我们将会在接下来使用它。

整合所有

我们有两种分布：预测量度为$\left(\mu_{0}, \Sigma_{0}\right)=\left(\mathbf{H}_{k} \hat{\mathbf{x}}_{k}, \mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}\right)$, 观察量度是$\left(\mu_{1}, \Sigma_{1}\right)=\left(\overrightarrow{\mathrm{z}_{k}}, \mathbb{R}_{k}\right)$。我们可以将其输入上一个公式当中然后获取其重合的部分。

$$\begin{aligned} \mathbf{H}_{k} \hat{\mathbf{x}}_{k}^{\prime} &=\mathbf{H}_{k} \hat{\mathbf{x}}_{k} +\mathbf{K}\left(\overrightarrow{z_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \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{aligned}$$

由上述的公式我们可以计算出：

$$\mathbf{K}=\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1}$$

考虑到K中还包含有一个$\mathbf{H}_k$所以我们可以对上式进行再化简。

$$\begin{gathered} \hat{\mathbf{x}}_{k}^{\prime}=\hat{\mathbf{x}}_{k}+\mathbf{K}^{\prime}\left(\overrightarrow{\mathbf{z}_{k}}-\mathbf{H}_{k} \hat{\mathbf{x}}_{k}\right) \\ \mathbf{P}_{k}^{\prime}=\mathbf{P}_{k}-\mathbf{K}^{\prime} \mathbf{H}_{k} \mathbf{P}_{k} \\ \mathbf{K}^{\prime}=\mathbf{P}_{k} \mathbf{H}_{k}^{T}\left(\mathbf{H}_{k} \mathbf{P}_{k} \mathbf{H}_{k}^{T}+\mathbf{R}_{k}\right)^{-1} \end{gathered}$$

其中$\hat{\mathbf{x}}_k'$就是最终的预测结果。我们可以将这一轮的计算结果作为下一轮的计算输入，但是随着时间的增加，预测结果的准确度也在降低。