Kalman滤波matlab算例

-3.3. Kalman滤波器、预报器1. 计算系统中，。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(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+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所示。图1 Kalman滤波分析：图中实线为实际值，虚线为Kalman滤波得到的估计值。可以看出，估计值在噪声的干扰下，基本与实际值一致，只在个别峰值处有一定偏离（趋势保持一致），达到了较好的估计效果。Kalman预报器仿真结果如图2。分析：可以看出，预报器对测量实际值提前2个时间的预报，存在一定的误差，没有滤波器的效果好。3.4 Bernoulli-Gaussian白噪声1. 计算程序1) 输入参数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 dlamda b(i)=1; else b(i)=0; endend4) 生成w(t),并作图w=b.*g;for