kalman滤波程序及其图像VIP专享

例子1: clear N=200; %随机序列长度 w(1)=0; %初始化操作 w=randn(1,N) %产生随机序列 x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1); %产生随机序列x（n） end subplot(311) t=1:N; plot(t,x,'b') %绘制原始输入信号图形x(n) title('原始输入信号图像') xlabel('x 轴单位：t/s','color','b') ylabel('y 轴单位：f/HZ','color','b') V=randn(1,N); q1=std(V); %计算随机列序V 的标准差 Rvv=q1.^2; %将标准差q1 的平方赋予Rvv q2=std(w); %计算随机序列w 的标准差 Rww=q2.^2; %将标准差q2 的平方赋予Rww c=0.2; y=c*x+V; %产生随机序列 Y subplot(312) t=1:N; plot(t,y,'g') %绘制加噪后输入信号图像 title('加噪后输入信号图像') xlabel('x 轴单位：t/s','color','b') ylabel('y 轴单位：f/HZ','color','b') p(1)=0; %初始化操作 s(1)=0; %初始化操作 for t=2:N; p1(t)=a.^2*p(t-1)+Rww; b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); s(t)=a*s(t-1)+b(t)*(y(t)-a*c*s(t-1)); %输出预测值估计公式 p(t)=p1(t)-c*b(t)*p1(t); end subplot(313) t=1:N; plot(t,s,'r') %绘制经过卡尔曼滤波后图像 title('经过卡尔曼滤波后图像') xlabel('x 轴单位：t/s','color','b') ylabel('y 轴单位：f/HZ','color','b') 出图结果为: 附 matlab下，kalman滤波程序： %卡尔曼滤波 clear N=800; w(1)=0; w=randn(1,N) %系统预测的随机白噪声 x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1); %系统的预测值 end V=randn(1,N); %测量值的随机白噪声 q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2; Y=c*x+V; %测量值 p(1)=0; s(1)=0; for t=2:N; p1(t)=a.^2*p(t-1)+Rww; %前一时刻 X的相关系数 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %卡尔曼增益 s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); %经过滤波后的信号 p(t)=p1(t)-c*b(t)*p1(t);%t状态下 x(t|t)的相关系数 end figure(1) plot(x) title('系统的预测值') figure(2) plot(Y) title('测量值') figure(3) plot(s) title('滤波后的信号') 运行结果图： 来自: http://hi.baidu .com/tangsu 2009/blog/item/f61ba9f10f3f31a6a40f5234.html clear N=200; w(1)=0; w=randn(1,N) x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1); end V=randn(1,N); q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2; Y=c*x+V; p(1)=0; s(1)=0; for t=2:N; p1(t)=a.^2*p(t-1)+Rww; b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1)); p(t)=p1(t)-c*b(t)*p1(t); end t=1:N; plot(t,s,'r',t,Y,'g',t,x,'b'); 02 04 06 08 01 0 01 2 01 4 01 6 01 8 02 0 0-5051 0