kalman濾波程序及其圖像

1、例子1:clear N=200; %隨機(jī)序列長(zhǎng)度 w(1)=0; %初始化操作 w=randn(1,N) %產(chǎn)生隨機(jī)序列 x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1); %產(chǎn)生隨機(jī)序列x（n） end subplot(311) t=1:N; plot(t,x,'b') %繪制原始輸入信號(hào)圖形x(n) title('原始輸入信號(hào)圖像') xlabel('x軸單位：t/s','color','b') ylabel('y軸單位：f/HZ','color&

2、#39;,'b') V=randn(1,N); q1=std(V); %計(jì)算隨機(jī)列序V的標(biāo)準(zhǔn)差 Rvv=q1.2; %將標(biāo)準(zhǔn)差q1的平方賦予Rvv q2=std(w); %計(jì)算隨機(jī)序列w的標(biāo)準(zhǔn)差 Rww=q2.2; %將標(biāo)準(zhǔn)差q2的平方賦予Rww c=0.2; y=c*x+V; %產(chǎn)生隨機(jī)序列Y subplot(312) t=1:N; plot(t,y,'g') %繪制加噪后輸入信號(hào)圖像 title('加噪后輸入信號(hào)圖像') xlabel('x軸單位：t/s','color','b') ylabel

3、('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); %輸出預(yù)測(cè)值估計(jì)公式 p(t)=p1(t)-c*b(t)*p1(t); end subplot(313) t=1:N; plot(t,s,'r') %繪制經(jīng)過(guò)卡爾曼濾波后圖像 title('經(jīng)過(guò)卡爾曼濾波后圖像

4、9;) xlabel('x軸單位：t/s','color','b') ylabel('y軸單位：f/HZ','color','b')出圖結(jié)果為: 附 matlab下，kalman濾波程序：%卡爾曼濾波clearN=800;w(1)=0;w=randn(1,N) %系統(tǒng)預(yù)測(cè)的隨機(jī)白噪聲x(1)=0;a=1;for k=2:N;x(k)=a*x(k-1)+w(k-1); %系統(tǒng)的預(yù)測(cè)值endV=randn(1,N); %測(cè)量值的隨機(jī)白噪聲q1=std(V);Rvv=q1.2;q2=std(x)

5、;Rxx=q2.2;q3=std(w);Rww=q3.2;c=0.2;Y=c*x+V; %測(cè)量值p(1)=0;s(1)=0;for t=2:N;p1(t)=a.2*p(t-1)+Rww; %前一時(shí)刻X的相關(guān)系數(shù)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); %經(jīng)過(guò)濾波后的信號(hào)p(t)=p1(t)-c*b(t)*p1(t);%t狀態(tài)下x(t|t)的相關(guān)系數(shù)endfigure(1)plot(x)title('系統(tǒng)的預(yù)測(cè)值')figure(2)plot(Y)title('測(cè)量值&

6、#39;)figure(3)plot(s)title('濾波后的信號(hào)')                運(yùn)行結(jié)果圖：來(lái)自: clearN=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);endV=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