卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导

这段时间做轴承故障诊断和预测的时候，需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测。卡尔曼滤波器可以实现对过去、当前和未来目标位置的估计，所以想通过卡尔曼滤波器的设计思路找到一些灵感。虽然最后发现：卡尔曼滤波器中的状态量是有具体的物理含义的物理量，而表征轴承故障状态的量只是一种表征量。这两者之间存在着本质的差别，因为轴承的退化过程目前为止还不能建模。虽然如此，我还是想将卡尔曼滤波器详细的推导过程分享给大家。
学习过程中，参考了白巧克力亦唯心的文章：卡尔曼滤波–从推导到应用（一），卡尔曼滤波–从推导到应用（二），在此给出直达连接。他/她的文章有故事，有推导，有例子，有代码，很优秀，向大家推荐。我在这篇文章里希望把很多文章中语焉不详的推导，以及符号的定义阐释清楚，并加入一些自己的理解，希望和大家分享。第一次写博客，markdown编辑器的用法还不熟悉，公式编辑会花去很多时间。所以直接用图片代替了，希望大家能够理解，以后我会慢慢按照标准的格式来编辑公式的。
一、基本卡尔曼滤波器（BKF）
卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。关于这种滤波器的论文最早由Swerling(1958)发表了这种想法。
1、基本动态模型
假设1：k时刻的真实状态是从k-1时刻的真实状态演化而来；
假设2：演化与测量的过程由线性算子来描述。
2、两个基本的方程
四个状态值的定义：

状态转移方程：

状态测量方程：

基于k-1时刻状态对k时刻状态的估计值与真实值之间的差称为估计误差，该估计误差的协方差矩阵的定义为后验估计误差协方差矩阵，用下式表示：

其模型拓扑结构用隐马尔科夫链可以表示成图1（维基百科:卡尔曼滤波）这样：

图1 卡尔曼滤波的隐马尔可夫链式模型
3、两个基本的过程
3.1 预测过程


从公式（3）*推导公式（2）：

3.2 更新过程
我们基于k-1时刻对k时刻状态的估计是否正确，需要用与实际测量值之间的误差来衡量，并且考虑用这个误差来补偿。所以在更新之前，我们应该计 算实际测量值与估计输出值之间的差值及其协方差矩阵。

（3）式的协方差矩阵表示为：

下面推导公式（4）：

然后我们再进行更新的步骤。更新是指：由基于k-1时刻对k时刻状态的估计值应当如何得到k时刻的估计值。卡尔曼的思想就是：用基于k-1时刻对k时刻状态的估计值与预测输出值和实际输出值之间的差进行线性组合得到k时刻的估计值，连接这两者的就是卡尔曼增益。这里体现的就是反馈的思想，更新过程的第一步用下式表示：

那么现在问题来了，如何求取这个卡尔曼增益呢？
这时候我们应该回到我们的出发点，我们希望的是滤除干扰真实状态的噪声，是滤波器的估计状态与真实状态最为接近。最为接近可以理解为k时刻的真实状态与k时刻的估计状态之间的误差二范平方和最小，也就等价于协方差矩阵的迹最小。可以表示为：

在这种情况下求取的卡尔曼增益称为最优卡尔曼增益。求取的过程就是直接上式对卡尔曼增益求一阶导数。
更新过程的第二步，就是计算卡尔曼增益：

下面给出推导过程：


最优卡尔曼增益计算出来之后，我们发现在最优卡尔曼增益情况下可以对后验误差协方差矩阵进行简化。第三步就是计算在最优卡尔曼增益下的后验误差协方差矩阵：

推导过程：

4、总结
卡尔曼在滤波器的推导过程就已经完成了，下面我们再将它们整合到一起，有个更清晰的认识：

将这些方程做成框图：

从中可以看到，只要对初始状态进行设定，卡尔曼滤波器就可以完成迭代了。
下面的图是我用白巧克力亦唯心提到的匀加速的例子做了三张GIF来动态展示这个过程，以示其效果。

图2 理论值

图3 测量值

图4 卡尔曼滤波结果
该卡尔曼滤波器是从第10个时间步才开始测量的，之前保持为0。
二、扩展卡尔曼滤波器（EKF）
EKF只是在KF的基础之上改变了状态转移函数和测量函数，从而将卡尔曼滤波器的线性算子变为非线性算子。下面只给出与KF不同的地方的公式推导。其他部分参考KF的推导。


可以看出，EKF依然是在KF的框架内进行的改进，所以思路与KF是完全一致的。只是与的计算方法不同。
下面给出推导过程。


到这一步，近似最优卡尔曼增益的计算就与KF的推导过程完全一致了，在此不再赘述。
只是需要注意一下的是：此处为什么是近似最优卡尔曼增益而不是最优卡尔曼增益。这是因为计算与的时候理论上应该计算函数f和h的雅可比矩阵。但是实际操作起来非常困难，特别是对于一些复杂的非线性系统。因此往往采用泰勒展开去一阶线性的部分。由于近似，得到的卡尔曼在增益也就不是最优卡尔曼增益，而是近似最优卡尔曼增益。这就直接导致了EKF在高度非线性系统下性能锐减的必然结果。而且系统初始状态估计错误或者说建模不正确，EKF也会迅速发散。所以在第三部分介绍的UKF则避免了求取函数的雅可比矩阵，从而提高了滤波器的性能和鲁棒性。
三、无向卡尔曼滤波器（UKF）
UKF依然没有脱离KF的框架。只不过对下一时刻状态的预测方法变成了sigma点集的扩充与非线性映射。这样做有两个优点：1、避免了复杂非线性函数雅可比矩阵的复杂运算；2、保证了非线性系统的普遍适应性。此外，由于高斯分布sigma点集的扩展，使高斯分布的噪声得到抑制。
预测过程：


更新过程：


在准确建模的前提下，KF，EKF和UKF都有不错的表现。但是对于很多复杂的系统而言，建模就是一个复杂的问题。如果模型参数没办法准确估计，那么卡尔曼滤波器的应用就会受到限制。在不知道模型参数的情况下，可以通过蒙特卡洛采样，特别是粒子滤波的方法来对参数进行估计。这也是笔者继续研究的方向。以上内容，仅供参考。限于水平，难免纰漏。如有不妥之处，还请告知。