卡尔曼滤波器程序解析(1)

  离散卡尔曼滤波器程序解析(1)

先看一下介绍卡尔曼滤波器的一个比较好的 文档<-点击文档进行下载。

例一，下面是一个用卡尔曼滤波器估计温度的matlab程序。

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %日 期： 	2015.9.22
 %程序功能：	使用卡尔曼滤波器估计温度
 %程序变量描述 ：X(k)	系统状态变量
 %		A	系统矩阵
 %		w	噪声
 %		Z(k)	观测值
 %		H	观测矩阵
 %		P(k)	协方差
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  clear 
  N=200;
  A=1;
  X(1)=1;			%最优估计 初始值 
  w=0;
  P(1)=10;              	%协方差 初始值                	
  Q = 0.000001;       		%温度模拟Q为1e-6         	 
  H=1;                 		%温度模拟H为1
  R=0.1;              		%测量过程协方差 温度模拟R为1e-1
  for k=1:N;
  Z(k)=25+sqrt(0.1)*randn(1);	%模拟平均温度25度 方差（协方差）为0.1的温度输入 测量方程
  end
  for k=2:N; 
  X(k)=A*X(k-1)+w;              %(1)先验估计 求当前时刻的估计值  
  P(k)=A.^2*P(k-1)+Q;           %(2)协方差估计 求当前时刻的估计值的偏差，A为系统参数，没有控制量，所以没有参数B，Q为噪声 
  K(k)=H*P(k)/(H.^2*P(k)+R);  	%(3)求Kg,K(k)为Kg，即Kalman增量 
  X(k)=A*X(k)+K(k)*(Z(k)-A*H*X(k));%(4)后验估计 求k时刻的最优值，即当前时刻的最优值 
  P(k)=P(k)-H*K(k)*P(k);        %(5)后验协方差 求当前最状态最优值的偏差，
  end

  figure(1);
  plot(Z,'g--');hold on;
  plot(X,'r--');hold on;

运行结果如下：

例二：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%日 期： 	2015.10.08
 %程序功能：	使用卡尔曼滤波器估计水位
 %程序变量描述 ：X(k)	系统状态变量
 %		A	系统矩阵
 %		w	噪声
 %		Z(k)	观测值
 %		H	观测矩阵
 %		P(k)	协方差
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear  
N=800; 
A=1;
X(1)=3;
w=0; 
Q=0.01; 
P(1)=5;    
H=1;    
R=2;   
for k=2:N;    
 Z(k)=5+sqrt(0.1)*randn(1);  
 X(k)=A*X(k-1)+w;			 % (1)估计值     
 P(k)=A.^2*P(k-1)+Q;         		 % (2)P  
 K(k)=H*P(k)/(H.^2*P(k)+R);  		 % (3)增益  R是方差  
X(k)=A*X(k)+K(k)*(Z(k)-A*H*X(k)); 	 % (4)最优   
P(k)=P(k)-H*K(k)*P(k);        		 % (5)P 
end 

 figure(1);
k=1:N;  
plot(k,X,'r',k,Z,'g',k,5,'k');  
axis([0 N 0 9])   
  
figure;  
h1 = stem(k -1.5, P, 'b');  
hold on;  
set(h1, 'MarkerFaceColor', 'white');  
hold off  
legend([h1(1)], 'A priori');  
title('A priori');  
ylabel('Covariance');  
  
figure;  
h1 = stem(k - 1.25, K, 'b');  
legend([h1(1)], 'Kalman gain');  
title('Kalman gain');  
ylabel('Kalman gain k');  
set(gca, 'XLim', xlim);  

运行结果如下：

图1

图2

图3