卡尔曼滤波【部分】

状态预测

External influence外部影响

不过我们并没有考虑到所有的影响因素。可能有一些与状态本身无关的变化——如外界因素可能正在影响系统。

例如，我们用状态对列车的运动进行建模，如果列车长加大油门，火车就加速。同样，在我们的机器人示例中，导航系统软件可能会发出使车轮转动或停止的命令。如果我们很明确地知道这些因素，我们可以将其放在一起构成一个向量，我们可以对这个量进行某些“处理”，然后将其添加到我们的预测中对状态进行更正。

假设我们知道由于油门设置或控制命令而产生的预期加速度。根据基本运动学原理，我们可以得到下式：

以矩阵形式：

其中被称为控制矩阵，被称为控制向量。（注意：对于没有外部影响的简单系统，可以忽略这个控制项）。

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

External uncertainty外部不确定性

如果状态仅仅依赖其自身的属性进行演进，那一切都会很好。如果状态受到外部因素进行演进，我们只要知道这些外部因素是什么，那么一切仍然很好。

但在实际使用中，我们有时不知道的这些外部因素到底是如何被建模的。例如，我们要跟踪四轴飞行器，它可能会随风摇晃；如果我们跟踪的是轮式机器人，则车轮可能会打滑，或者因地面颠簸导致其减速。我们无法跟踪这些外部因素，如果发生任何这些情况，我们的预测可能会出错，因为我们并没有考虑这些因素。

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

这样一来，由于新增的不确定性原始估计中的每个状态都可能迁移到多个状态。因为我们非常喜欢用高斯分布进行建模，此处也不例外。我们可以说的每个点都移动到具有协方差的高斯分布内的某个位置，如下图所示：

这将产生一个新的高斯分布，其协方差不同（但均值相同）：

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

换句话说，新的最佳估计是在先前的最佳估计的基础上做出的预测，加上对已知外部影响的修正。

新的不确定性是从旧的不确定性中预测出来的，还有一些来自环境的额外不确定性。

通过给出的和,我们对我们的系统有了一个模糊的估计（状态预测过程）。

那么当我们从传感器中获取一些测量数据时会发生什么呢？

状态更新

利用测量进一步修正状态

假设我们有几个传感器，这些传感器可以向我们提供有关系统状态的信息。就目前而言，测量什么量都无关紧要，也许一个读取位置，另一个读取速度。每个传感器都告诉我们有关状态的一些间接信息（换句话说，传感器在状态下运作并产生一组测量读数）。

注意，读数的单位和比例可能与我们要跟踪的状态的单位和比例不一样。我们将用矩阵对传感器的测量进行建模。

我们可以用通常的方式计算出我们期望看到的传感器读数的分布：

卡尔曼滤波器的伟大之处就在于它能够处理传感器噪声。换句话说，传感器本身的测量是不准确的，且原始估计中的每个状态都可能导致一定范围的传感器读数，而卡尔曼滤波能够在这些不确定性存在的情况下找到最优的状态。

根据传感器的读数，我们会猜测系统正处于某个特定状态。但是由于不确定性的存在，某些状态比其他状态更可能产生我们看到的读数：

我们将这种不确定性的协方差（即传感器噪声）称为协方差，读数的分布均值等于我们观察到传感器的读数，我们将其表示为

这样一来，我们有了两个高斯分布：一个围绕通过状态转移预测的平均值，另一个围绕实际传感器读数。

因此，我们需要将基于预测状态（粉红色）的推测读数与基于实际观察到的传感器读数（绿色）进行融合

那么我们最有可能的新状态是什么？对于任何可能的读数(z1,z2)，我们有两个相关的概率：（1）我们的传感器的读数是测量值的概率(z1,z2)，以及 （2） 我们先前估计认为的概率(z1,z2)是我们应该看到的读数。

如果我们有两个概率，并且我们想知道两个概率都为真的可能性，我们只需将它们相乘即可。因此，我们对两个高斯分布进行了相乘处理：

两个概率分布相乘得到的就是上图中的重叠部分。而且重叠部分的概率分布会比我们之前的任何一个估计值/读数都精确得多，这个分布的均值就是两种估计最有可能配置（得到的状态）。

 事实证明，两个独立的高斯分布相乘之后会得到一个新的具有其均值和协方差矩阵的高斯分布！下面开始推公式。

合并两个高斯分布

首先考虑一维高斯情况：一个均值为，方差为的高斯分布的形式为：

我们想知道将两个高斯曲线相乘会发生什么。下图中的蓝色曲线表示两个高斯总体的（未归一化）交集：

将公式(9)代入公式(10)，我们可以得到新的高斯分布的均值和方差如下所示：

我们将其中的一小部分重写为：

我们顺势将公式(12)和(13)的矩阵形式写在下面。这样一来，公式的形式就简单多了！

以矩阵形式重写方程式(12)和(13)，表示新高斯分布的协方差矩阵，是其沿每个轴的平均值，则：

 K是一个称为卡尔曼增益的矩阵。

 将一切整合在一起

参考资料

[1]: How a Kalman filter works, in pictures, 图解卡尔曼滤波是如何工作的：

How a Kalman filter works, in pictures | Bzarg

[2]:图解卡尔曼滤波(Kalman Filter)

图解卡尔曼滤波(Kalman Filter)-CSDN博客