卡爾曼濾波器程序解析(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