https://mp.weixin.qq.com/s/II7xYWtgG6cQd-kP-O47mA

本篇文章分上、中、下三篇，上篇从标准卡尔曼滤波开始，中篇加入更真实的系统模型，下篇从传感器的数据融合中实现扩展卡尔曼滤波。

1. 一个例子

想象一架飞机正准备着陆，虽然有很多方面需要考虑，比如风速、燃料等，但是大家最关心的应该是飞机的高度。理想情况下，我们假设当前的飞机高度是前一时刻飞机飞行高度的一部分，我们假设飞机每次观测时都降低 2%，那么它在当前时刻的高度是上一次飞行高度的 98%，因此写成公式是这个样子：

上面就是一个典型的递归计算，要计算当前值，必须引用前一个值。

由此可以得出一条高度和时间的曲线：

image

2. 噪声处理

实际应用中飞机的飞行高度是通过“GPS”或者气压计等传感器获取的，这些传感器的精度往往不同，存在一定的误差，也会在不同的时刻引入不同的噪声，因此我们通过传感器观察到的飞机的实际飞行高度是存在一定噪声的：

下图表示当噪声占观测高度的10%时的曲线

image

3. 组合方程

现在我们有两个方程描述飞机的状态：

这两个方程很容易理解，但是他们还不足以处理复杂的系统，为了使方程更通用，我们用、表示变量，用、表示常量，下标表示当前时刻，因此上面的方程可以表示为：

方程中的表示系统的当前状态， 表示系统的前一个状态，是一个固定值，我们这里是 0.98；表示系统的当前观测值， 是当前噪声测量值。

卡尔曼滤波一个非常流行的原因就是它允许我们通过观测值 ，常量，还有测量噪声等得到一个较好的当前状态的估计值。

大家都知道飞机在实际飞行中，飞机高度所走过的路径不可能完全平滑，飞机通常会遇到一些湍流的影响，我们将这种湍流定义为噪声，因此这种噪声可以视为另一种噪声信号：

用更通用的方程表示：

方程中的称为过程噪声，因为像湍流一样，它是过程的固有部分，不是观测或测量的伪影。

接下来我们将忽略过程噪声一会儿，以便于更好的讨论其他部分，在传感器融合部分，我们将继续探讨过程噪声。

4. 状态估计

我们的目标是从观测值获取系统当前状态，因此将方程观测方程写为：

问题是我们不知道当前的观测噪声，它是不可预知的。幸运的是，卡尔曼滤波具有强大的洞察力，我们可以通过当前观测值以及前一个状态估计值来综合考虑当前状态的估计值。我们使用表示系统的当前状态估计值:

因此，我们可以将当前状态的估计值表示为前一个状态的估计值与当前状态的观测值之间的一个折中，表示增益，在这里可以理解为权重，这个方程将会是我们直接实现卡尔曼滤波的一个重要方程。

看起来有点复杂，接下来我们将给定两个极端值再来看一下，当 时：

也就是说，当增益是零的时候，观测是没有影响的。当前状态与前一个状态是相等的；如果设定$g_k=1，那么：

也就是说，如果，系统的当前状态是和前一个状态无关的。我们得到的系统当前状态完全来自于观测。

实际系统中，增益值是介于0-1之间的。

比如我们假定，，，那么

5. 增益计算

从上面的公式我们可以通过系统的前一个状态估计值，以及系统的当前观测值 和当前增益
获取系统当前状态的估计值：

那么如何计算这个增益值 呢？目前只能间接的从噪声中获取这个值。

回忆一下每一个观测值与一个特定噪声值的关系：

我们不知道观测时的单个噪声，但是我们通常可以知道平均噪声值。比如某个传感器公布的精度，告诉我们它的输出是多么的嘈杂。 暂且将这个平均噪声值称为 ，这个值是不随时间改变的，因此不必给它下标，它是传感器的固有属性。然后我们可以依据这个计算当前增益值。

实际上是噪声信号的方差，如果允许这个值随时间变化，卡尔曼滤波器也可以工作，但在大多数应用中，这个值通常假定为常数。

式中的是一个递归计算的预测误差，它实际上是时刻的估计过程的协方差，它是我们预测方差的平均值。实际上，状态是一个随机的变量或矢量，一个随机过程的瞬时值，它根本没有一个“真”值，估计仅仅是状态描述的过程模型最有可能的值，或者是最接近真实值的值。

先看这两个公式的含义，假设我们前一个时刻预测的误差为0即，那么当前增益。

因此由上面公式可以推出下一个系统状态的估计和当前系统状态是一样的。这也恰恰说明了如果我们的预测是准确的我们就不需要调整状态估计。

我们考虑另一个极端，假如预测误差为1，即，那么。如果为零，我们的系统仅存在很小的噪声，那么增益， 我们的状态估计将会受到观测值的强烈影响。但是当增大时，增益可以变的任意小，也就是说当系统足够嘈杂时，一个坏的预测将不得不被忽略。

我们再看最后那个公式，当时，$p_k=p_{k-1}， 因此与状态估计一样，零增益意味着对预测误差没有更新。

另一方面，当时，，因此最大增益对应于零预测误差，此时的当前观测值仅用于更新当前状态。

6. 预测和更新

到这里，你可能想知道之前方程中的常量 哪去了？

看起来已经消失在我们的状态估计方程中：

实际上我们需要这两个方程去估计系统的状态，这两个方程都是基于不同类型的信息来表示系统状态的估计。原始方程表示一个关于状态的预测称为先验，第二个方程表示基于观测的系统状态预测的更新称为后验，那么将第一个方程重新写为：

最后我们也用常量来预测误差：

这两个方程表示卡尔曼滤波器的预测阶段，它是周期的预测和更新。

为什么，而不是? 这个原因将在后续介绍。

7. 运行卡尔曼滤波器

现在已经有了卡尔曼滤波器的所有方程；

预测阶段：

更新阶段：

在更新阶段的两个方程中我们使用了赋值符号，即箭头符号，它的意义表明和的更新是基于预测阶段对当前值的更新，而不是基于之前的预测的定义。

运行卡尔曼滤波器还需要一些其他初始值：

• 观测序列

• 状态估计的初始值
  它可以是我们的初始观测值

• 预测误差的初始值
  这个预测误差的初始值不能为0，否则从上面的公式中可以看出，一旦为零，乘积将永远是0。

这里我们将初始值设置为1。

我们伪造一些数据作为观察值，仍然以飞机着陆为例，并假设噪声。假设飞机每次降落25%，即：

假定初始高度为

image

运行后：

image

绿色是卡尔曼滤波的结果，红色是带噪声的系统观测值，原始信号为蓝色，可以清楚的看到卡尔曼滤波后的曲线非常平滑，且非常接近原始信号。 虽然我们加入的是均匀分布的噪声，而没有加入符合高斯分布的噪声，但从这个滤波效果来看，好像并没有太大的差别。

本篇先介绍这些，后续将继续分析现实中更真实的模型。