卡尔曼滤波器的一种形象表达

1、卡尔曼滤波器是什么？

卡尔曼滤波器是一个信息不确定情况下的强大的信息处理工具，它可以在任何有关动态系统的不确定信息的地方使用，可以对系统下一步做什么进行有根据的猜测。它是不断变化的系统的立项选择，具有很快的速度，非常适合解决实时问题以及嵌入式系统。要了解卡尔曼滤波器，需要具备一定的概率与矩阵分析基础。

卡尔曼滤波器可以用来做什么？

举个简单的例子，有一个可以在森林中漫步的机器人，那机器人就需要知道它自身在什么位置，然后它才能去它想去的地方。

假设机器人有个状态：$\overrightarrow{{{x}_{k}}}$，包含位置和速度：$\vec{x_k} = (\vec{p}, \vec{v})$，请注意，系统状态可以是任何状态量，可以是流体罐的含量，可以是发动机温度，可以是显示屏上触碰的位置数据，或者需要观测的任何数据。我们的机器人本身还有一个GPS传感器，精确到10米左右，但是在森林中，这个精度显然不够，因为可能会发生碰撞或者跌入悬崖。因而GPS本身还不够好。

我们可能会知道机器人怎么移动，没有干扰的情况下，机器人会朝着一个方向移动，但下一个瞬间，机器人也许会受到风的影响，车轮打滑的影响，因而轮子转动的距离并不能代表机器人实际走过的距离，因而我们的预测也就不完美。

我们通过GPS传感器也可以间接的知道机器人走了多少的距离，但因GPS本身的精度，也有一些不确定性。但是我们能不能使用这些所有可用信息，估计得到比我们观测到的信息更准确的数据呢？答案是肯定的，这就是**卡尔曼滤波器**kalman filter（KF）。
我们继续使用速度和位置这两个简单状态解释KF：

$\vec{x} = \begin{bmatrix} p\\ v \end{bmatrix}$

我们不知道机器人实际位置和速度是什么，我们不知道具体哪种位置与速度的组合更接近真实值

KF假设两个变量是随机变量并且服从高斯分布，每个变量具有平均值μ和方差σ2（不确定度）：

上面的图片中，位置与速度不相关，下面的例子，位置与速度呈现相关性，我们观察到的机器人的特定位置跟机器人的速度相关。

如果我们根据机器人的上一个位置估计下一个位置，如果机器人速度很快，那机器人就移动的很远，我们的估计也就会很远，如果机器人移动的很慢，那我们的估计也就不会那么远。这种关系，对于跟踪机器人来说，是非常重要的，它给了我们这样一种思路：跟一个测量值有关的其他测量值可能是什么？这就是卡尔曼滤波器的目标，我们希望从尽可能多的信号中，估计出尽可能多的信息！

这种相关性被称为协方差矩阵。 简而言之，矩阵Σij的每个元素是第i个状态变量和第j个状态变量之间的相关度。 （此处的协方差矩阵是对称矩阵，交换ij的位置并不影响其性质）。 协方差矩阵通常被标记为“Σ”，所以我们称之为“Σij”。

使用矩阵描述问题

我们对描述的问题进行数学建模，在时刻k我们称最优估计为
$\mathbf{\hat{x}_k}$协方差矩阵为Pk。

$$\begin{equation} \label{eq:statevars} \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} \end{equation}$$
此处我们只是用了速度与位置作为状态变量，但是状态别变量可以包含任意数量的变量。
接下来我们使用一些方法查看当前状态k-1时刻，并预测下一时刻k的状态。我们并不知道哪个状态是真实状态，我们的预测过程也不管这些。

我们可以用矩阵Fk表示这个预测步骤：

如果我们的估计是正确的，系统将会正确的移动。
我们用矩阵预测下一时刻的位置和速度，基本的运动学公式如下：

$$\begin{split} \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{split}$$
也就是：

预测矩阵能让我们预测系统下一个状态，但是我们仍然不知道如何更新协方差矩阵。
那我们就需要另外一个公式。如果我们将这个分布中的每个点诚意矩阵 $\color{firebrick}{\mathbf{A}}$,那么它的协方差矩阵Σ如何变化？
那么就有如下公式：

联合方程（3）和（4）：

外部因素影响

我们还没能预测出系统状态，仍然会有许多与系统状态无关的外部变化影响着系统。比如火车运行时，司机推动助力器，导致火车加速，类似的，我们的机器人例子中，导航软件发出指令使机器人加速或停止。如果我们知道相关环境中发生了别的变化，我们可以使用一个向量$\color{darkorange}{\vec{\mathbf{u}_k}}$来代表这个变化，并把它添加到预测系统中。
我们假设我们知道了预期加速度a，从运动学理论可以得到：

矩阵形式为：

其中，$\mathbf{B}_k$被称为控制矩阵，$\color{darkorange}{\vec{\mathbf{u}_k}}$为控制向量。（对于非常简单的系统可忽略。）假如我们的并不是100%的精确，那会发生什么呢？

外部不确定性

如果系统状态根据自身属性进行演变，或者根据已知的外部变化进行演变，那么系统状态依然可控。但是如果是我们不知道的外部变化呢？例如我们预测四旋翼无人机的状态，会受到未知的风力的影响，轮式移动机器人会受车轮打滑的影响而改变速度，我们并不能准确预测这些事情，因而我们对系统状态的预测会崩溃，因为我们并没有将外界未知变化考虑到系统内。
但我们仍然可以在每个预测步骤之后增加新的不确定性模拟与真实情况相关（即那些我们无法准确跟踪的事务）的情况。

我们原本估计的每个状态都可能会转到另一个状态。我们都喜欢高斯分布，所以我们会说，$\color{royalblue}{\mathbf{\hat{x}}_{k-1}}$中的每个点都被移动到具有协方差Qk的高斯块内的某个地方；另一种说法是，我们将未经考虑的影响，视作协方差Qk的噪声。

这就产生新的高斯分布，具有不同的协方差（但是具有相同的平均值）：

我们通过简单地添加Qk来获得扩展的协方差，为预测步骤提供完整的表达式：

换句话说，新的最优估计是先前的最优估计加上已知外部影响之后的校正。
而且新的不确定度是由旧的不确定度预测而来，还有一些来自环境的不确定度。
我们对$\overrightarrow{{{x}_{k}}}$和Pk给出了我们的系统可能在哪里的模糊估计，那我们从传感器获取数据之后呢？

用测量数据提高估计值精度

我们可能有多个传感器，能够提供有关系统状态的信息，也许一个读取位置，一个读取速度，每个传感器告诉我们关于状态的事情，换句话说，传感器在一个状态下运行并产生一组读数。

值得注意的是，读数的尺度和单位与我们所追踪的状态的单位和尺度不一样，我们使用矩阵$\mathbf{H}_k$对传感器进行建模：

我们可以清楚的知道传感器读数分布情况，通用的方式表述：

卡尔曼滤波器的一大优点就是处理传感器噪声。 换句话说，我们的传感器至少在某种程度上是不可靠的，而我们原始估计的每个状态可能会导致一系列传感器读数。

从我们观察到的每次读数，我们会猜到我们系统处于某种特定状态，但由于不确定性，也许有些状态比其他状态更符合我们所看到的读数。

我们称这种不确定性（即传感器噪声）为$\color{mediumaquamarine}{\mathbf{R}_k}$分布的平均值等于我们观察到的读数，我们称之为$\color{yellowgreen}{\vec{\mathbf{z}_k}}$。
所以现在我们有两个高斯斑点：一个围绕着我们的变换预测的平均值，一个围绕着我们得到的实际的传感器读数。

我们必须尝试根据我们实际观察到的传感器读数（绿色），根据预测状态（粉红色）与不同的猜测来调和我们所看到的读数的猜测。
那么我们的系统的最新状态最可能是什么？对任何可能的读数$(z_1,z_2)$我们有两个相关概率：（1）是传感器读数$\color{yellowgreen}{\vec{\mathbf{z}_k}}$是$(z_1,z_2)$的（误）测量的概率；（2）我们先前估计是我们看到的读数的概率。

如果我们有两个概率，而我们想知道两者都是真实的概率，那我们把她们加在一起，所以，把两个高斯斑点乘以它们。

剩下的就是重合部分，两个斑点都很亮的区域。这就比我们之前的估计要精确的多。这种分布的均值是两个估计值最可能的配置，鉴于我们拥有的诸多信息，我们认为这是我们的最优估计。

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

事实证明，当使用单独的均值（方法）和协方差矩阵乘以两个高斯斑点时，您将获得一个具有自己的均值和协方差矩阵的新的高斯斑点！ 也许你可以看到这里发生了什么：必须有一个公式来获得这些新的参数！

Combining Gaussians

让我们找到那个公式。 在一个维度上首先看这个是最简单的。 具有方差σ2和平均μ的1D高斯钟形曲线定义为：

我们想知道当将两条高斯曲线相乘时会发生什么。 下面的蓝色曲线表示两个高斯群体的（非正规化）交集：


您可以将方程（9）代入方程（10），并进行一些代数（小心重新归一化，使总概率为1）得到：

我们简化分解上述公式：

记下你如何处理以前的估计，并增加一些新的估计。 看看这个公式是多么简单！

但是矩阵版本呢？ 嗯，让我们以矩阵形式重写方程式（12）和（13）。 如果Σ是高斯斑点的协方差矩阵，并且$\vec{\mu}$是其沿每个轴的平均值，则：

$\color{purple}{\mathbf{K}}$ 是一个称为卡尔曼增益的矩阵，我们将在稍后使用它。

总结在一起

我们有两个分布，一个是$(\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})$的预测测量，一个是$(\color{yellowgreen}{\mu_1}, \color{mediumaquamarine}{\Sigma_1}) = (\color{yellowgreen}{\vec{\mathbf{z}_k}}, \color{mediumaquamarine}{\mathbf{R}_k})$的观测测量， 我们可以将它们插入等式（15）以找到它们的重叠：

由公式14，卡尔曼增益为：

由公式16,17可得（$\mathbf{H}_k$, $\mathbf{H}_k^T$）：

这就给我们整个步骤的完整的方程式。
就是这样！ $\color{royalblue}{\mathbf{\hat{x}}_k’}$是我们新的最佳估计，我们可以继续将其（与$\color{royalblue}{\mathbf{P}_k’}$一起）重新投入到我们喜欢的多次预测或更新中。

在上面所有的数学中，你需要实现的所有方程是等式（7），（18）和（19）。 （或者如果你忘记了这些，你可以从等式（4）和（15）中重新推导出所有的东西。）

这将允许您准确地对任何线性系统建模。 对于非线性系统，我们使用扩展卡尔曼滤波器，其通过简单地线性化关于它们的平均值的预测和测量来工作。 （我可能会在将来对EKF进行第二次写入）。