Kalman濾波matlab算例

1、3.3. Kalman濾波器、預報器1. 計算系統(tǒng)中，。1) 取初值為：x(0|0)=0 0',P(0|0)=0 0;0 0;2) clear all;3) clc;4) bushu=300;5) Q=0.64;6) R=0.36;7) randn('seed',1);w=sqrt(Q)*randn(1,bushu);8) randn('seed',2);v=sqrt(R)*randn(1,bushu);9) phi=0.6 -0.4;10) 0.2 1.1;11) gama=1 2'12) H=1 1;13) x(:,1)=0 0'y(

2、1)=H*x(:,1)+v(1);14) for i=2:bushu15) x(:,i)=phi*x(:,i-1)+gama*w(i-1);16) y(i)=H*x(:,i)+v(i);17) end18) 19) n=2;20) xjian(:,1)=zeros(2,1);p(:,:)=zeros(2);21) xjian_2(:,1:2)=zeros(2);22) for i=1:bushu-123) %*Kalman濾波器* 24) pp(:,n*(i-1)+1:n*i)=phi*p(:,n*(i-1)+1:n*i)*phi'+gama*Q*gama'25) kf(:,i

3、+1)=pp(:,n*(i-1)+1:n*i)*H'*inv(H*pp(:,n*(i-1)+1:n*i)*H'+R);26) p(:,n*i+1:n*(i+1)=eye(n)-kf(:,i+1)*H*pp(:,n*(i-1)+1:n*i);27) xxjian(:,i+1)=phi*xjian(:,i);28) epsilon(:,i+1)=y(i+1)-H*xxjian(:,i+1);29) xjian(:,i+1)=xxjian(:,i+1)+kf(:,i+1)*epsilon(:,i+1);30) %*預報器*31) xjian_2(:,i+2)=phi*xxjian(:

4、,i+1);32) end33) t=1:bushu;34) subplot(2,1,1);plot(t,x(1,t),'b',t,xjian(1,t),'r:');35) subplot(2,1,2);plot(t,x(2,t),'b',t,xjian(2,t),'r:');36) subplot(2,1,1);plot(t,x(1,t),'b',t,xjian_2(1,t),'r:');37) subplot(2,1,2);plot(t,x(2,t),'b',t,xjian_2(

5、2,t),'r:');2. 仿真結(jié)果與分析kalman濾波仿真結(jié)果如圖1所示。圖1 Kalman濾波分析：圖中實線為實際值，虛線為Kalman濾波得到的估計值?？梢钥闯?，估計值在噪聲的干擾下，基本與實際值一致，只在個別峰值處有一定偏離（趨勢保持一致），達到了較好的估計效果。Kalman預報器仿真結(jié)果如圖2。分析：可以看出，預報器對測量實際值提前2個時間的預報，存在一定的誤差，沒有濾波器的效果好。3.4 Bernoulli-Gaussian白噪聲1. 計算程序1) 輸入?yún)?shù)function BGnoise()bushu=500;disp('ÊäÈëlamda')lamda=input('lamda=');% lamda=0.1;sigma2_g=2.25;randn('seed',1);2) 生成g(t)g=sqrt(sigma2_g)*randn(1,bushu);3) 生成b(t)for i=1:bushu d=rand; if d<lamda b(i)=1; else b(i)=0; endend4) 生成w(t)