目标跟踪：卡尔曼滤波（Kalman Filter）到底是怎么工作的？

Kalman filter到底是怎么工作的？

本文主要参考的文章：https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/，图片也基本来自上述博客

其实接触KF已经很久了，听过对应的课程，也对着公式进行过推导，但总有一种感觉，始终在门外，没有醍醐灌顶，融会贯通的感觉，所以还是写篇博客，毕竟通过强行让自己输出一下，也会帮助理解和记忆。

一、什么是Kalman filter？

说到KF，就LZ目前有限的接触中来说有SLAM，tracking等一些领域。但是实际上，卡尔曼滤波的应用远大于此，甚至在卫星导航等领域，也是用到这个技术，那么这么牛的技术到底是什么呢？

假设存在一个动态系统，这个系统中存在很多不确定的因素，那么我们就可以通过使用卡尔曼滤波，根据系统之前的一些数据（后面会解释有哪些）来进行预测，预测这个系统下一步到底会做什么。如果用到卫星导航上，也就是我们可以预测下一步卫星的位置大概率会在哪呢？

当然针对一个比较复杂的系统，会存在很多不确定因素，会存在噪声等，但是按照原文和实际操作的工程经验来说，似乎卡尔曼也能很好的解决此类问题呢。当然卡尔曼滤波也不是万能的，它会有一定的假设前提，比如说跟踪，也许预测一帧到两帧是还不错的，如果预测比较多的帧，那预测不准也是情理之中了。

那为什么工程上还是很喜欢用它呢？
因为在上述的应用场景中，都是存在连续不断的变化，卡尔曼滤波是非常使用这种场景的。此外，它的内存占用很小（只需要保留前一个状态），速度非常快，是实时问题和嵌入式系统的理想选择。所以在无人驾驶中，也会经常使用卡尔曼滤波来做一些应用，毕竟不能在车上放台3090吧，哈哈，LZ倒是想呢。。。

二、我们可以用卡尔曼滤波干什么呢？

举个小例子，假设我们有个小机器人（头顶一个天线，就叫它一毛吧），在树林里四处游走，一毛需要进行导航，所以它得知道它到底在哪才行，就像我们去景区，每个路标指示牌都会指明我们当前位置在哪一个道理，一毛也需要知道。

我们可以假设一毛在k时刻的状态是 $\stackrel{⟶}{x_k}$, （这个latex的写法是这样的：\stackrel{\longrightarrow}{x_k}）

也就是一毛现在的状态 $\stackrel{⟶}{x_k}$包含对应的位置信息和速度信息

注意在这个例子中，状态是位置和速度，放进其他问题，它可以是水箱里液体的提及，汽车引擎的温度，或者是任何其他的数据。例如在跟踪中，就可能是：

class KalmanFilter(object):
    """
    A simple Kalman filter for tracking bounding boxes in image space.

    The 8-dimensional state space

        x, y, a, h, vx, vy, va, vh

    contains the bounding box center position (x, y), aspect ratio a, height h,
    and their respective velocities.

    Object motion follows a constant velocity model. The bounding box location
    (x, y, a, h) is taken as direct observation of the state space (linear
    observation model).

    """

包括的是bbox的中心位置，宽高比a，和高度h，还有它们对应的速度。

还是回到一毛这里，假设我们的一毛有一个GPS定位装置，定位的精度为10m，可能在树林里定位容易不太准，现在GPS的精度实际上应该可以高很多吧。但是10m的误差还是让一毛不太放心，万一在树林里突然出现个悬崖峭壁，我们一毛不久就地阵亡了嘛，所以单纯的依赖GPS提供的信息还是不够好

当然我们也可以预测一毛是怎么移动的：它自己把指令发送给控制轮子的马达，如果它始终朝着一个方向走，并且当中没有遇到什么障碍，那么下一个时刻很有可能还是朝着这个方向行驶。但是一毛对自己的状态也不是完全清楚的，它有可能会轮子打滑，或者上个坡再下个坡。。。那么如果出现这种情况，一毛就不能通过轮子转动的次数来确定实际的距离了，所以基于这个距离的预测也是不完美的（举个实际的例子吧，之前LZ玩小车的时候，如果四个轮胎的胎压不同，那么小车也是走不了直线的，这种都是会存在误差的）

按照上述的情形，我们可以知道，GPS为一毛提供了一些关于状态的信息，但是这个信息是间接的，不准确的，一毛自食其力，根据自己之前的状态预测了自己的位置，但那也是间接的，存在不确定性的，万一轮胎漏个气呢，是吧。

但是，上述的信息就是我们能够获得的全部的信息了，在这些信息的基础上，我们怎么能利用这些信息，给出一个完整的预测呢，让我们预测的准确度比一毛用GPS或者自食其力预测的准确度更高呢？

此时，就是卡尔曼滤波上场的时刻啦！！！

三、卡尔曼滤波是怎么看待一毛这个问题的呢？

我们继续上面一毛的问题，假设有一个简单的状态，只包含位置和速度

实际上，我们并不知道真实的位置和速度是什么，所以位置和速度会有大量不同的组合，其中某些组合的可能性会更加高一些，如下图所示，越亮的位置代表可能性越高。

卡尔曼滤波假设它的变量都是随机的，并且都是符合高斯分布的。在本文的这个例子中，其实可以看作一毛的位置和速度其实都是随机的，并且符合高斯分布的。每个变量都会有一个均值$\mu$，它是代表随机分布的中心，其实就是它最有可能出现的状态，还有一个是方差${\sigma }^2$，它是用来表示不确定性的，画个比较夸张的图吧，不确定性越大，那么高斯分布的那个钟型越胖一些，这个是可以理解的，因为出现其他数值的可能变大了嘛，看图加理解，不需要死记硬背哦！

要是想自己画一下的小伙伴，LZ这里也提供一下代码吧

import numpy as np
import matplotlib.pyplot as plt
import math


def normal_distribution(x, mean, sigma):
    return np.exp(-1 * ((x - mean) ** 2) / (2 * (sigma ** 2))) / (math.sqrt(2 * np.pi) * sigma)


mean1, sigma1 = 0, 0.5
x1 = np.linspace(mean1 - 6 * sigma1, mean1 + 6 * sigma1, 100)

mean2, sigma2 = 0, 2
x2 = np.linspace(mean2 - 6 * sigma2, mean2 + 6 * sigma2, 100)

mean3, sigma3 = 5, 0.5
x3 = np.linspace(mean3 - 6 * sigma3, mean3 + 6 * sigma3, 100)

y1 = normal_distribution(x1, mean1, sigma1)
y2 = normal_distribution(x2, mean2, sigma2)
y3 = normal_distribution(x3, mean3, sigma3)

plt.plot(x1, y1, 'r', label='m=0,sig=0.5')
plt.plot(x2, y2, 'g', label='m=0,sig=2')
plt.plot(x3, y3, 'b', label='m=5,sig=0.5')
plt.legend()
plt.grid()
plt.savefig('./test_gaussian.jpg')
plt.show()

在下面这幅图中，位置和速度是不相关的，也就是说状态中一个变量的信息并不能决定另外一个变量。

如果位置和速度是相关的，观测到一个特定位置的可能性取决于其对应的速度

这个情况应该不是很难理解，如果一毛跑得飞快，那么一毛移动的更远，那么一毛距离上一个时刻的位置就会比较远啦，但是如果一毛是刚吃完饭那种悠闲散步，那可能速度很慢，那么对应的位置移动也会比较小了。

跟踪变量直接的潜在关系是非常重要的，因为它能够给我们提供更多的信息：通过一个观则可以告诉我们其他观测的可能信息。卡尔曼滤波的目的就是这个：从不确定的观测中榨取尽可能多的信息！

这种变量之间的相关信息可以通过协方差矩阵（covariance matrix）来进行表示。简而言之，在协方差矩阵中${\Sigma }_{ij}$就可以表示第i个状态变量和第j个状态变量之间的相关程度，我们也可以从这个定义中推测出协方差矩阵是对称的，也就是说我们沿着对角线进行翻转，值是不会发生变化的。

有一点需要说的是我们通常使用“$\mathbf{\Sigma }$”来表示协方差矩阵，所以它的元素就可以用“${\Sigma }_{ij}$”来表示啦。

四、使用矩阵来进行问题描述

我们可以把从状态中得到的信息建模成一个高斯斑（Gaussian blob），所以在k时刻我们需要两个信息：最佳估计${\hat{\mathbf{X}}}_k$(也就是我们刚刚分析高斯分布的均值，上文中我们称为$\mu$),还有它对应的协方差矩阵${\mathbf{P}}_{\mathbf{k}}$，虽然在这个例子中，我们还是只使用位置和速度这两个变量，实际上我们可以使用任意数量的变量来进行数学建模。

接下来，我们需要某种方式来查看当前的状态（time: k-1)，并且对下一时刻（time： k）的状态进行预测。值得注意的是，我们不知道到底哪一个状态才是真实值，因为我们预测函数并不关注这个问题，它适用于所有的情况，并且给出一个新的分布。

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

从上图可知，它把原始预测中的每个点都移到的新的预测的位置中，如果原始预测是正确的话，系统就会移到新的预测的位置。

我们是如何使用矩阵来预测未来下一时刻的位置和速度的呢？我们将使用一个非常基本的运动学公式

写成矩阵的形式就是：

我们现在有一个可以给出下一个状态的预测矩阵，但是我们仍然不知道怎么更新协方差矩阵？因为上面预测的公式相当于只是更新了$\mu$这个值

这里，我们就需要另外一个公式，如果我们把分布中的每个点乘以了一个矩阵$\mathbf{A}$，那么它对应的协方差矩阵$\mathbf{\Sigma }$会怎么变化呢？

恩，这个会有以下的恒等式

将等式（4）和等式（3）相结合，可以得到下列等式：

五、外部影响

实际上，事情也并不总是一帆风顺的，总会有一些外在的因素来影响到这个系统的状态，例如模拟火车运动的时候，除了列车自动驾驶之外，列车操作员可能会手动调速。在一毛的例子中，导航软件也可能发出停止的命令。对于上述的这些外界因素，我们可以再引入一个向量$\stackrel{⟶}{{\mathbf{u}}_{\mathbf{k}}}$，作为对系统的修正来加入系统的预测中去。

假设我们可以通过控制油门或者指令来产生一个加速度a，由基本运动学我们可以知道：

写成对应矩阵的形式

${\stackrel{⟶}{\mathbf{B}}}_{\mathbf{k}}$被称作是控制矩阵，${\stackrel{⟶}{\mathbf{u}}}_{\mathbf{k}}$被称作是控制向量，如果存在一个非常简单的外部系统，我们也可以省略掉对应的外部影响。

我们还要考虑一个问题，如果我们的预测并不是一个100%准备的模型，那么又会发生什么呢？

六、外部的不确定性

如果这个状态是基于自身的属性而发展，那么事情发展会很好。但是如果状态是基于外力进行发展的，那么事情的发展也不会超出预期，只要我们能够知道这些外力是什么。

但是，如果我们不知道外力是什么呢？举个例子，如果我们在跟踪一个四轴飞行器，它可能会受到风的冲击，或者我们的一毛在冰上或者泥里打滑了，我们就没有办法来跟踪到这些情况来。如果上述的情况发生了，我们的预测就很有可能出错，因为我们没有考虑到这个新的不确定性。

通过在每个预测步骤后添加一些新的不确定性，我们可以对与“世界”相关的不确定性进行建模（即，我们无法跟踪的事物）

如上图所示，我们最初估计的每个状态可能移动到其他多个状态，因为我们的假设都是高斯分布，这次也不例外，我们也可以说在状态${\hat{x}}_{k-1}$中每个点可能会移动到一个高斯斑中的某个位置，并且符合方差为${\mathbf{Q}}_k$，换句话说我们可以将无法确定的外部因素描述成方差为${\mathbf{Q}}_k$的高斯噪声。

这会产生一个新的高斯斑，有相同的均值，但是方差不同。

我们可以通过将${\mathbf{Q}}_k$进行简单的叠加，就可以得到完整的预测方程：

从式（7）中我们可以得出这个结论，新的最佳观测是由上一个时间的最佳观测，加上已知的外部修正得到的
新的不确定性可以通过老的不确定性，加上一些环境中的不确定性得到。

到此，我们得到了系统对应的一个模糊的估计，并且由${\hat{\mathbf{x}}}_k$和${\mathbf{P}}_k$来确定对应的均值和方差。这些是一毛自食其力，获得的数据，那么如果我们还能获得一些传感器的数据呢？

七、通过测量值来改善估计

我们也许还会有一些传感器能我们提供一些系统的信息，目前我们也不关注测量值到底是什么？对于一毛来说，也许是GPS读到的位置，或者有什么其他测速仪器的读数。每个数据都间接地告诉我们一些有关状态的信息，传感器是在某个状态上运行，并且产生一系列读数。

值得注意的是传感器测量的读数的单位和尺度可能和我们跟踪状态的单位和尺度并不一致，我们需要通过矩阵${\mathbf{H}}_k$来对传感器进行建模。

我们可以用常见的方式来表达传感器读数的分布

卡尔曼滤波的一个优点就是能够处理传感器噪声的问题。换句话说，我们的传感器的数据多多少少会有些噪声，不是绝对可靠的，在我们最初估计的每个状态其实有可能存在一系列的传感器读数。

从我们观察到的每个读数，我们可以推测到系统应该会处于某个状态。但是由于我们的读数存在不确定性，有些状态的可能性会比其他一些状态的可能性更高一些。

我们把测量值（例如传感器的噪声）的不确定性的方差称作${\mathbf{R}}_k$，我们观测到的读数同样也是服从高斯分布的，假设这个分布的均值为$\stackrel{⟶}{{\mathbf{z}}_k}$

所以我们就会有两个高斯斑，一个是一毛自食其力，根据自己的速度之类的推测出的一个状态，还有一个是通过GPS给定的位置的一个状态，也就有了这两个高斯斑，换句话说，我们会有两个预测值，但是我们使用哪一个，或者怎么把这两个预测的状态融合起来呢？

我们必须尝试把所有得到的信息都利用起来，这样，才能预测到一个更好的状态。我们来回顾一下，具体有哪些信息呢？以一毛为例，我们可以通过代码设置或者轮子的转速和半径得到它的速度和位置（并不是100%准确的，同是还会出现外界因素影响），上图中通过这个信息预测的状态用粉色进行表示，一毛高大上的GPS定位装置，可以直接读出它的位置（也不是100%准确的），上图中通过这个信息预测的状态用 绿色进行表示。

所以，我们将这个两类信息进行融合后，我们又会得到怎样的状态呢？对于任何可能的读数$(z_1,z_2)$，我们有两个相关的概率：

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

说点人话吧，其实就是我们能得到两个状态k的估计$(z_1,z_2)$，也就是上面说的两个高斯斑，但是它们也会出现一个问题，就是哪个高斯斑更加符合系统实际的情况呢？换句话说是传感器的读数更接近系统的真实状态呢？还是说通过之前状态进行预测的值更接近系统的真实状态呢？

实际上，如果我们有两个概率分布，并且我们想知道两个都为真的概率，我们只需要把他们相乘，就可以得到对应的概率

按照上图显示，通过将两个高斯斑相乘，我们可以得到两个斑点重叠的部分，即两个斑点都很亮的部分（概率比较大的位置），可以看到中间重叠的区域比我们之前估计的范围要小很多，所以估计会比之前的估计精确很多。这个分布的均值是两种估计都最有可能出现的设置，因此，在知道所有信息的情况下，它是对状态最佳的猜测。

恩，从下图看出新产生的预测又像另外一个高斯斑

实际证明，如果将两个高斯斑相乘（两个高斯斑对应各自的均值和方差），我们就可以得到一个新的高斯斑，新的高斯斑的均值和方差可以从原来的两个高斯斑的参数推导得到。

八、高斯乘法

我们从一维的数据开始，一个一维高斯曲线，均值为$\mu$， 方差为${\sigma }^2$，可以写成如下的表达式

我们想知道如果两个高斯曲线相乘，那么会发生什么状态呢？下图中蓝色的曲线代表两个高斯曲线（没有归一化）的交集。

我们把式（9）带入到式（10）中，并做一些变化（要注意进行归一化的问题，来保证总的概率值为1），可以得到如下：

我们可以提取一部分公有因子$k$，来简化公式：

我们之前是怎么估计对应的预测值的呢？仅仅是获取之前的估计，加上一些新的项就可以得到新的估计，上述公式还是非常简单的。

上面举的例子是一维的例子，如果是矩阵的版本呢，我们来重新改写一下式（12）和式（13）。假设$\Sigma$表示高斯斑的协方差矩阵，$\stackrel{⟶}{\mu }$是代表它沿每个轴的均值

矩阵$\mathbf{K}$我们就称做是卡尔曼增益，后面我们会使用到它的

加油，小伙伴们，胜利就在眼前啦！！！

九、综合所有的信息

我们有两个分布：

• 通过预测数据满足下面这个分布：
  

• 通过观测数据满足下面这个分布：

我们按照式（15），将上面这两个分布相乘，得到新的分布（重叠区域）的均值和方差

其中从式（14）中我们可以得到对应的卡尔曼增益为：

我们将式（16）和式（17）中的公共项${\mathbf{H}}_k$从两边约去，注意式（17）的 $\mathbf{K}$中也有这个公共项${\mathbf{H}}_k$，并且在$\mathbf{P}{}_k^{\prime }$中有$\mathbf{H}{}_k^T$这个公共项，约分简化后的表达式如下：

目前为止，我们得到了每个状态的更新步骤，$\tilde{x}{}_k^{\prime }$是我们的最佳的预测值，我们可以持续迭代（连同$\mathbf{P}{}_k^{\prime }$）输入到下一轮的预测或者更新中

总的流程大图：

• 按照上面大图，其实总的流程应该是这样的：

1）我们先通过上一轮时刻的状态进行估计，会得到对应的一个预测状态
2）然后我们会获得当前状态传感器的读数，也会获得一个状态
3）然后我们将两个状态的估计进行融合，更新得到最后的最佳状态分布

十、总结

在上面所有的数学公式中，我们只需要实现的是方程（7），（18）和（19），如果有兴趣的话也可以自己推导当中的过程。

卡尔曼滤波使得我们可以准确对任意线性系统进行建模，对于非线性系统，我们需要使用扩展卡尔曼滤波（EKF），这个滤波器通过简单地线性化预测和测量的均值来进行后续的状态估计。

十一、感想

小伙伴看完一遍，如果觉得云里雾里的这是很正常的，毕竟卡尔曼滤波还是需要花费一定的时间理解的。如果第一遍没有理解，不妨调整好心态，再来一遍，毕竟胖子也不是一口吃成的吧。