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

其实接触KF很长一段时间以来，我听过相应的课程，并推导了公式，但总是有一种感觉，总是在门外，没有开明，整合的感觉，所以我仍然写一个博客，毕竟，通过强迫自己输出，也会帮助理解和记忆。

举个小例子，假设我们有一个小机器人（头上有一条天线，叫它一毛），在森林里四处游荡，一毛需要导航，所以它必须知道它在哪里，就像我们去景区一样，每个路标都会指出我们当前的位置，一毛也需要知道。

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

也就是说，一毛的现状 x k ? \stackrel{\longrightarrow}{x_k} xk?它包含相应的位置信息和速度信息

请注意，在这个例子中，状态是位置和速度，并将其放入其他问题中。它可以提到水箱中的液体、汽车发动机的温度或任何其他数据。例如，在跟踪中，可能是：

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 μ，它是代表随机分布的中心，其实就是它最有可能出现的状态，还有一个是方差 σ 2 \sigma^2 σ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）来进行表示。简而言之，在协方差矩阵中 Σ i j \Sigma_{ij} Σij​就可以表示第i个状态变量和第j个状态变量之间的相关程度，我们也可以从这个定义中推测出协方差矩阵是对称的，也就是说我们沿着对角线进行翻转，值是不会发生变化的。

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

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

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

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

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

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

写成矩阵的形式就是：

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

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

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

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

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

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

写成对应矩阵的形式

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

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

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

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

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

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

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

我们可以通过将 Q k \mathbf{Q}_k Qk​进行简单的叠加，就可以得到完整的预测方程： 从式（7）中我们可以得出这个结论，新的最佳观测是由上一个时间的最佳观测，加上已知的外部修正得到的 新的不确定性可以通过老的不确定性，加上一些环境中的不确定性得到。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

我们想知道如果两个高斯曲线相乘，那么会发生什么状态呢？下图中蓝色的曲线代表两个高斯曲线（没有归一化）的交集。 我们把式（9）带入到式（10）中，并做一些变化（要注意进行归一化的问题，来保证总的概率值为1），可以得到如下：

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

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

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

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

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

我们有两个分布：

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

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

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

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

我们将式（16）和式（17）中的公共项 H k \mathbf{H}_k Hk​从两边约去，注意式（17）的 K \mathbf{K} K中也有这个公共项 H k \mathbf{H}_k Hk​，并且在 P k ′ \mathbf{P}{'_k} Pk′​中有 H k T \mathbf{H}{^T_k} HkT​这个公共