無跡卡爾曼濾波算法.docx

《無跡卡爾曼濾波算法.docx》由會員上傳分享，免費在線閱讀，更多相關(guān)內(nèi)容在教育資源-天天文庫。

1、%該文件用于編寫無跡卡爾曼濾波算法及其測試%注解：主要子程序包括：軌跡發(fā)生器、系統(tǒng)方程%????測量方程、UKF濾波器%作者：Jiangfeng%日期：2012.4.16%---------------------------------------functionUKFmain%------------------清屏----------------closeall;clearall;clc;tic;globalQfn;????????????%定義全局變量%------------------初始化--

2、------------stater0=[220;1;55;-0.5];????%標準系統(tǒng)初值state0=[200;1.3;50;-0.3];????%測量狀態(tài)初值%--------系統(tǒng)濾波初始化??p=[0.005000;00.00500;??000.0050;0000.005];??%狀態(tài)誤差協(xié)方差初值????????????????????n=4;T=3;Qf=[T^2/20;0T;T^2/20;0T];%--------------------------------------stater=st

3、ater0;state=state0;xc=state;staterout=[];stateout=[];xcout=[];errorout=[];tout=[];t0=1;h=1;tf=1000;??????%仿真時間設(shè)置%---------------濾波算法----------------fort=t0:h:tf??[state,stater,yc]=track(state,stater);%軌跡發(fā)生器：標準軌跡和輸出??[xc,p]=UKFfiter(@systemfun,@measurefun,x

4、c,yc,p);??error=xc-stater;??????????%濾波處理后的誤差??staterout=[staterout,stater];??stateout=[stateout,state];??errorout=[errorout,error];??xcout=[xcout,xc];????tout=[tout,t];end?%---------------狀態(tài)信息圖像---------------figure;plot(tout,xcout(1,:),'r',tout,staterout

5、(1,:),'g',...????tout,stateout(1,:),'black');legend('濾波后','真實值','無濾波');gridon;xlabel('時間t（s）');ylabel('系統(tǒng)狀態(tài)A');title('無跡卡爾曼濾波');figure;plot(tout,xcout(2,:),'r',tout,staterout(2,:),'g',...????tout,stateout(2,:),'black');gridon;legend('濾波后','真實值','無濾波');xlab

6、el('時間t（s）');ylabel('系統(tǒng)狀態(tài)B');title('無跡卡爾曼濾波');figure;plot(tout,xcout(3,:),'r',tout,staterout(3,:),'g',...????tout,stateout(3,:),'black');gridon;legend('濾波后','真實值','無濾波');xlabel('時間t（s）');ylabel('系統(tǒng)狀態(tài)C');title('無跡卡爾曼濾波');figure;plot(tout,xcout(4,:),'r',tout

7、,staterout(4,:),'g',...????tout,stateout(4,:),'black');gridon;legend('濾波后','真實值','無濾波');xlabel('時間t（s）');ylabel('系統(tǒng)狀態(tài)D');title('無跡卡爾曼濾波');figure;plot(tout,errorout(1,:),'r',tout,errorout(2,:),'g',...????tout,errorout(3,:),'black',tout,errorout(4,:),'b');gr

8、idon;legend('A','B','C','D');xlabel('時間t（s）');ylabel('濾波后的狀態(tài)誤差');title('無跡卡爾曼濾波誤差');%---------------------------------------------toc;??%計算仿真程序運行時間endfunction[state,stater,yout]=track(state0,stater0)%--------