卡尔曼滤波算法简要

   　　卡尔曼滤波在传感器数据融合 ，状态预测里占据了很重要的地位，今天有空稍微整理一下，整理自己的思路，加深理解记忆。要逐渐的把KF，EKF，ＰＦ，ＵＫＦ都理解清楚，熟悉他们的优劣势，明确应用场合，熟能生巧，善莫大焉。

　　　最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算。

 线性随机微分方程为：

X(k)=A X(k-1)+B U(k)+W(k)   

Z(k)=H
X(k)+V(k)    

X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。A和B是系统参数，对于多模型系统，它们为矩阵。Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声(White
Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。对于满足(线性随机微分系统，过程和测量都是高斯白噪声)的条件，卡尔曼滤波器是最优的信息处理器。

 
 （一）预测过程 

根据系统的模型，可以基于系统的上一状态而预测出现在状态：

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

式(1)中，X(k|k-1)是利用上一状态预测的结果，X(k-1|k-1)是上一状态最优的结果，U(k)为现在状态的控制量，如果没有控制量，它可以为0。

对应于X(k|k-1)的covariance，我们用P表示covariance，更新公式如下：

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

式(2)中，P(k|k-1)是X(k|k-1)对应的covariance，P(k-1|k-1)是X(k-1|k-1)对应的covariance，A’表示A的转置矩阵，Q是系统过程的covariance。式子1，2就是卡尔曼滤波器5个公式当中的前两个，也就是对系统的预测。

   （二）升级过程

　　我们有了现在状态的预测结果，然后我们再收集现在状态的测量值。结合预测值和测量值，我们可以更新得到现在状态(k)的最优化估算值X(k|k)：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

其中Kg为卡尔曼增益(Kalman Gain)：

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

我们已经得到了k状态下最优的估算值X(k|k)。为了要另卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：

P(k|k)=（I-Kg(k) H）P(k|k-1) ………　　　　 (5)

其中I 为1的矩阵，对于单模型单测量，I=1。当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。这样，算法就可以自回归的运算下去。

（三）设线性时变系统的离散状态防城和观测方程为：

X(k) = F(k,k-1)·X(k-1)+T(k,k-1)·U(k-1)

 

Y(k) = H(k)·X(k)+N(k)

其中

 

X(k)和Y(k)分别是k时刻的状态矢量和观测矢量

 

F(k,k-1)为状态转移矩阵

 

U(k)为k时刻动态噪声

 

T(k,k-1)为系统控制矩阵

 

H(k)为k时刻观测矩阵

 

N(k)为k时刻观测噪声

则卡尔曼滤波的算法流程为：

 

预估计X(k)^= F(k,k-1)·X(k-1) 

 
计算预估计协方差矩阵
 

C(k)^=F(k,k-1)×C(k)×F(k,k-1)'+T(k,k-1)×Q(k)×T(k,k-1)'

Q(k) = U(k)×U(k)' 

 

计算卡尔曼增益矩阵
 

K(k) = C(k)^×H(k)'×[H(k)×C(k)^×H(k)'+R(k)]^(-1)

R(k) = N(k)×N(k)' 

 

更新估计
 

X(k)~=X(k)^+K(k)×[Y(k)-H(k)×X(k)^] 

 

计算更新后估计协防差矩阵
 

C(k)~ = [I-K(k)×H(k)]×C(k)^×[I-K(k)×H(k)]'+K(k)×R(k)×K(k)' 

 

 

X(k+1) = X(k)~

C(k+1) = C(k)~

重复以上步骤