Kalman滤波matlab算例

3.3.Kalman滤波器、预报器1.计算系统中，0.60.41==110.21.12H，，。1)取初值为：x^(0|0)=[00]',P(0|0)=[00;00];2)clearall;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.21.1];11)gama=[12]';12)H=[11];13)x(:,1)=[00]';y(1)=H*x(:,1)+v(1);14)fori=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)fori=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+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(:,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(2,t),'r:');2.仿真结果与分析kalman滤波仿真结果如图1所示。图1Kalman滤波分析：图中实线为实际值，虚线为Kalman滤波得到的估计值。可以看出，估计值在噪声的干扰下，基本与实际值一致，只在个别峰值处有一定偏离（趋势保持一致），达到了较好的估计效果。Kalman预报器仿真结果如图2。分析：可以看出，预报器对测量实际值提前2个时间的预报，存在一定的误差，没有滤波器的效果好。3.4Bernoulli-Gaussian白噪声1.计算程序1)输入参数functionBGnoise()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)fori=1:bushud=rand;ifdlamdab(i)=1;elseb(i)=0;endend4)生成w(t),并作图w=b.*g;fort=1:bushuline([t,t],[0,w(t)]);end2.计算结果Bernoulli-Guassian白噪声如下图所示：题目3.5为平滑，不用做