Kalman Filter算法详解

关键词：卡尔曼滤波 线性最优估计 迭代 预测值 估计值 协方差矩阵 状态转移方程 观测方程 最小均方估计 预测+矫正

 　　经典的卡尔曼滤波是一个迭代的线性的最优状态估计器。利用最小均方误差原理，可以保证状态值的估计是最优的。它只需要知道上次的状态估计值和当前的测量值，就可以预测到当前的最优估计值，适用于实际系统,共需5个方程既可以得到完成一次迭代。卡尔曼滤波器在singnal processing navigation control etc 应用广泛。

step1 ：时间更新（预测predict）

1. 根据状态转移方程得到k时刻的预测值（先验估计值）:  X"(k) = AX(k-1|k-1) + Bu(k); 
2. 预测值（先验估计值）的协方差：P(k|k-1) = AP(k-1|k-1)A^T+Q;

step2 ：状态更新（校正correct）

1. 计算误差增益Kalman gain;K_k = P(k|k-1)H^T[R+HP(k|k-1)H^T]^-1，  其中[R+HP(k|k-1)HT]表示新息的协方差，Kalman gain越大，表示预测值对估计值的影响越小，即更相信测量值;
2. 计算最优估计（后验估计）:X(k|k) = X(k-1|k-1) + K_k[Z(k)-HX(k|k-1)]，其中Z(k)表示测量值,由观测方程Z(k)=HX(k)可得,这就是我们要到最优估计输出;
3. 更新最有估计误差矩阵（后验估计协方差）:P(k|k) = [I-K_kH]P(k|k-1)

下面是Kalman filter 仿真程序 using matlab

clear all
close all
clc
A=1;
H=1;
x(1)=0;%初始状态
p(1)=1;%后验协方差估计
w1=randn(1,10000);%加速度计白噪声
t=[0.01:0.01:100];
z=sin(10*t)+w1;%加速度计测量值
R=20;%测量协方差矩阵
Q=1;%过程状态协方差矩阵
for k=1:9999 %卡尔曼滤波器滤波过程
    p_(k)=A*p(k)*A'+Q;
    K(k)=p_(k)*H'*inv(H*p_(k)*H'+R);
    x(k+1)=A*x(k)+K(k)*(z(k)-H*A*x(k));
    p(k+1)=(1-K(k)*H)*p_(k);
end

figure(1),clf
plot(t,z,'b')
grid on
axis([3,5,-6,6])
title('仿真加速度计的输出')
xlabel('时间（秒）'),ylabel('仿真加速度计输出（度）')

figure(2),clf
plot(t,x,'b',t,sin(10*t),'r')
grid on
axis([3,5,-6,6])
title('加速度计估计')
legend('加速度计估计值','加速度计真值')
xlabel('时间（秒）'),ylabel('加速度计输出（度）')

测量值如下（含有测量造成v(k)）

 

滤波效果如下,图中蓝色曲线为估计值

参考资料：

[1]https://en.wikipedia.org/wiki/Kalman_filter#Predict

[2]http://blog.csdn.net/xiahouzuoxin/article/details/39582483