【实用版】卡尔曼滤波及其扩展方法的区别与定位系统中的应用

源自于学校课题，主要用卡尔曼滤波KF及其扩展方法（包括扩展卡尔曼滤波EKF、Sigma点卡尔曼滤波SPKF和集合卡尔曼滤波EnKF）实现多传感器数据的融合。

卡尔曼滤波的原理和几个经典方程其他博客中已经讨论的很详细了，在这里简单整理一下个人的理解，包括各种卡尔曼滤波的不同和具体的使用方法等，以定位系统为例。

如有错误，欢迎指正。

卡尔曼滤波

传统的卡尔曼滤波（KF）适用于线性系统（也就是转移方程和观测方程都是线性的），如下图所示。

1. X是系统的状态量，比如定位系统中状态量可以是待测目标的坐标、速度等，也就是系统最终要求的东西。第一个式子是转移方程，从上一时刻的系统状态估计值推导出该时刻的系统状态估计值；
2. 第二个式子是观测方程，通过该时刻的系统状态估计值推导出观测量的估计值，这里的Y就是观测量 。其实这个式子就是表明了系统状态X和观测量Y的关系，观测量可以来源于不同的传感器，比如GPS测到的坐标，惯性传感器转换得到的速度、角度等。

状态量和观测量的概念要分清，状态量是要求的目标，观测量可以理解为是用来修正状态量的辅助信息

下面具体介绍一下KF的执行步骤。
KF分为预测（也叫时间更新）和更新（也叫状态更新）两个步骤，公式其实就五个，如下图所示。

简单来说，前面两个式子先根据第一张图中的转移方程，从上一时刻的估计值得到该时刻的系统状态估计值。P是误差协方差，也是根据上一时刻的结果得到的。Q是系统状态量的噪声。

而更新步骤（下面三个式子）中，K是卡尔曼增益，这个值其实决定了该时刻的估计值是更相信上一时刻的估计值，还是更相信该时刻的观测量（R是观测量的噪声），这从第四个公式可以看到。所以更新步骤的实质就是通过该时刻的观测量去修正纯粹通过转移方程推导得到的该时刻的状态估计值。

通过第四个公式得到了该时刻系统状态的更新值，然后在下一时刻中，又可以带入公式1，去推导下一时刻的状态估计值，这样KF就跑起来了。

所以要跑KF其实只要先确定X和Y，然后确定转移和观测方程就行了，后面只需要代进去。

卡尔曼滤波的扩展

由于KF只能用于线性系统，为了处理非线性的转移和观测方程，有很多方法对KF进行了扩展，包括扩展卡尔曼滤波EKF、Sigma点卡尔曼滤波SPKF和集合卡尔曼滤波EnKF。
这几种KF的不同如下图所示。

EKF

扩展卡尔曼滤波就是把非线性方程泰勒一阶展开（就是求导啦）转换成线性方程，然后用KF一样的方法区处理，很简单，很快速。但是，显然，精度丢失也很严重。适合非线性不强的系统。

SPKF

无迹卡尔曼滤波（UKF）、中心差分卡尔曼滤波（CDKF）、容积卡尔曼滤波（CKF）都属于这类，就是利用数量一定的固定采样点（Sigma点），计算这些Sigma点经由非线性函数的传播,通过非线性状态方程获得更新后的值。

这类方法没有直接把非线性方程线性化，而是通过加权的Sigma点带入到非线性方程中，逼近实际的结果。其实就是固定采样思想啦。

sigma点的个数和系统的维数相关，也就是和系统状态量的个数相关。

EnKF

这个和上面的区别就是，这个用的是无权重的随机采样点，集合点个数按需要设置，其实就是用更多的点就能更好地反映真实的结果。

这种随机采样的思想有点类似粒子滤波（PF），只不过它的核心还是KF，所以只支持高斯噪声的系统，复杂度也比PF低。目前在气象学这种高维系统中用的比较多。

定位系统中的应用

下面举一个简单的定位例子，比如我的系统状态量设置为三维的坐标和速度，即

X = [x; y; z; Vx; Vy; Vz]  % 6维系统

观测量来自于多种传感器，比如有来自于GPS测得的坐标

Y1 = [x; y; z]

比如有wifi等传感器得到的接收信号强度（RSS）值

Y2 = [rss1; rss2; rss3; rss4]

然后只要确定转移方程和观测方程就行了。比如转移方程采用普通的随机游走方程

然后观测方程则根据不同的观测量，找到X和Y之间的关系式即可。比如

（Y2涉及到RSS定位的相关知识，其中d是估计值坐标和信标真实坐标的距离，F(d)是RSS和距离的关系）

几种KF扩展方法的具体公式有需要的话再介绍吧。