https://mp.weixin.qq.com/s/cr1u840FeBLalrmtJQs8TQ
本篇文章分上、中、下三篇，上篇从标准卡尔曼滤波开始，中篇加入更真实的系统模型，下篇从传感器的数据融合中实现扩展卡尔曼滤波。
8. 一个更真实的模型
现在重新回顾一下描述系统的状态方程和观测方程：
表示系统的当前状态，
表示系统的前一个状态；
是一个常量，
是系统当前状态的观测值；
是系统观测噪声；
这两个方程已经适用于大多数系统，但仍然不够普适性；现在依然以飞机的飞行为例，我们并没有考虑到飞行员对飞机的操作和控制，飞行员操作控制杆向前或向后移动，对飞机输入控制量，最终对飞机产生控制。我们将这个控制量称为，表示飞行员发送到飞机的控制信号的当前值，并且我们对这个控制量加上一个缩放因子
，因此整个状态方程变为：
一般而言，除了噪声以外的任何信号都可以通过常量进行一定比例的缩放，因此观测方程也可以这样表示：
9. 调整估计值
通过以上的方程我们更真实的描述一个系统的状态和观测，由此预测和更新方程也需要做相应的调整：
预测：
更新：
仍然以飞机飞行为例，我们增加一个控制信号，表示飞行员稳步拉回控制杆，并提高飞机飞行高度，原始信号为蓝色表示，观测值以红色表示，绿色信号为卡尔曼滤波后的值。
image
10. 向系统中添加速度
现在回想一下飞机下降时飞行高度的原始方程：
以更通用的方程表示为：
我们都知道飞机飞行的高度即海拔，可以看做是一种距离，距离总是和速度与时间相关的：
再进一步思考，当前距离和前一时刻的距离的关系：
下标代表当前，
代表前一个。也就是说，飞机现在的位置等于前一时刻的位置加上刚刚飞行过的垂直距离。如果按照固定的时间间隔进行计算，比如说1秒，或者1分钟等，那么可以将上式简化为：
貌似与我们的状态方程更接近了一步，但是还差一些，接下来我们引入向量和矩阵。
11. 向量和矩阵
通常情况下系统的状态并不是一个变量，比如飞机的高度和速度，那么我们就可以使用向量表示系统的状态：
符号表示一个定义，系统状态定义为一个距离和速度的矢量。 当我们用一个矩阵去乘以一个向量时得到的结果依然是一个向量，比如：
因此，我们将系统状态方程中的常量定义为一个矩阵，因为系统的状态并非是一种：
再次回顾一下系统状态方程：
将系统状态矢量代入：
从公式中可以清晰的看出距离等于前一刻的距离与速度和观测时间间隔的乘积之和，而速度和上一时刻的速度相等；这表明飞机是匀速上升或下降的，但现实中速度不可能做到匀速。当速度是变化的，即系统中还存在一个加速度变量，那么我们的系统状态方程可以表示为：
12. 再看预测和更新
系统状态的修正公式：
为一个向量，
是一个矩阵；大家可能还记得，这个方程的原始形式是这样的：
其中是一个控制信号，
是一个比例因子，系统的观测方程为：
其中是观测量，
是由于传感器的不准确所带来的噪声。那么我们如何修正这些方程以适应新的向量矩阵形式呢？是的，线性代数让这些变得尤其简单。我们使用大写的字母写出缩放因子
和
，使他们成为矩阵，而不是单一的标量值：
然后所有的变量包括状态、观测、噪声、控制等都被认为是向量，回顾一下我们前面的系统预测和更新方程：
预测：
更新：
大家可能会想，将这里所有的缩放因子都用大写表示，然后就可以了。然而事实并非如此，矩阵的乘法没有那么简单，这就是前面提到的为什么是而不是
。
有时候不等于
，有时候需要转置矩阵，通过在矩阵的旁边加上一个上标
来表示转置矩阵。转置矩阵是通过把每一行变成一列，每一列变成一行来实现的。看个实际例子：
由此，我们可以重写我们的预测方程: (关于卡尔曼滤波五个公式的由来，我会专门写一篇文章进行推导与分析)
我们已经知道为何是一个矩阵，但是
为何也是一个矩阵呢？
我们的第二个更新方程可以表示为：
再看增益方程的原始形式：
它包含一个除法，我们瞬间想到了除法可以用乘法代替：
那么上面的方程可以写为：
我们现在把这些常量替换成矩阵：
如何计算呢，求逆矩阵吗？是的，一个矩阵乘以它的逆矩阵后结果为一个单位矩阵
单位矩阵的定义如下，我们以一个 3x3 的矩阵举例：
13. 传感器融合简单介绍
现在我们的卡尔曼滤波方程完全以矩阵形式表示，
模型：
预测：
更新：
如果想在状态变量中添加以下额外的项似乎是一项很艰巨的任务。卡尔曼滤波最有价值的地方就是用在传感器的融合中。
回到飞机飞行的例子中，飞行员可以访问更多的信息，不仅仅是飞机的高度，仪表上可以显示飞机的水平速度、航向、经纬度、室外温湿度等信息。想象一下，假如一架飞机只有三个传感器，每一个传感器都有给定的状态，一个用于测量高度的气压计，一个用于航向的电子罗盘，一个测量空速的空速指示器等。
假设这些传感器是完全准确的，即没有任何噪声。那么我们的观测方程为：
将会变为：
假如我们向系统中加入了一个GPS，同样用于测量高度，那么我们的观测方程将会变为：
因此可以将多个传感器的读数结合起来，从而推断出某个状态的信息。就比如我们看病一样，对于重要的疾病总是寻求多个医生的诊断，对于一些重要的事情，最好有多个信息来源。
本篇先介绍这些，下一篇会以一个真实的传感器融合实例开始，并最终实现扩展卡尔曼滤波。
网友评论