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

 

 　　经典的卡尔曼滤波是一个迭代的线性的最优状态估计器。利用最小均方误差原理，可以保证状态值的估计是最优的。它只需要知道上次的状态估计值和当前的测量值，就可以预测到当前的最优估计值，适用于实际系统,共需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)AT+Q;

step2 ：状态更新（校正correct）

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

版权声明：本文为wll-zju原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://www.cnblogs.com/wll-zju/p/4673228.html