Kalman Filter算法详解

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

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

step1 ：时间更新（预测predict）

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

step2 ：状态更新（校正correct）

计算误差增益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越大，表示预测值对估计值的影响越小，即更相信测量值;
计算最优估计（后验估计）:X(k|k) = X(k-1|k-1) + K_k[Z(k)-HX(k|k-1)]，其中Z(k)表示测量值,由观测方程Z(k)=HX(k)可得,这就是我们要到最优估计输出;
更新最有估计误差矩阵（后验估计协方差）: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('加速度计输出（度）')