卡爾曼濾波器設(shè)計(jì)

1、卡爾曼濾波器設(shè)計(jì)一、卡爾曼濾波器基本設(shè)計(jì)思路二、設(shè)計(jì)要求觀測(cè)質(zhì)點(diǎn)在原點(diǎn)附近沿直線作振蕩運(yùn)動(dòng)，由于觀測(cè)的隨機(jī)誤差的原因，使得測(cè)量數(shù)據(jù)不能正確反映質(zhì)點(diǎn)的運(yùn)動(dòng)。設(shè)計(jì)濾波器對(duì)測(cè)量數(shù)據(jù)進(jìn)行處理，以消除隨機(jī)白噪聲的影響。三、程序設(shè)計(jì)%-產(chǎn)生正弦波信號(hào)-%t=0:0.1:20-0.1;T=0.1;y=5*sin(pi*t*0.5;figure;plot(t,y,or;axis(0,20,-8,8;xlabel('時(shí)間/s'ylabel('質(zhì)點(diǎn)位移/mm'title('理想質(zhì)點(diǎn)偏離中心位移圖'%-添加噪聲-%a=randn(1,200;Z=y+a;figure;

2、plot(t,Z,or;axis(0,20,-8,8;xlabel('時(shí)間/s 'ylabel('質(zhì)點(diǎn)位移/mm'title(測(cè)量所得質(zhì)點(diǎn)偏離中心位移圖'%-建立系統(tǒng)模型-%X=zeros(2,200;X(:,1=0,1;A=1,T;0,1; B=1/2*(T2 T'H=1,0;C=0,0;0,1;Q=(0.252; R=(0.252; %-卡爾曼算法-%for n=1:200Kg=C*H'/(H*C*H'+R;X(:,n=X(:,n+Kg*(Z(:,n-H*X(:,n;X(:,n+1=A*X(:,n;C=(eye(2,2-Kg*

3、H*C;C=A*C*A'+B*Q*B'end%-%figure t=0:0.1:20;Y=X(1,:;plot(t,Y,'or'grid;axis(0,20,-8,8;xlabel('時(shí)間/s 'ylabel('質(zhì)點(diǎn)位移/mm 'title(卡爾曼濾波后輸出質(zhì)點(diǎn)最優(yōu)化偏離中心位移'四、程序運(yùn)行結(jié)果分析1.、理想質(zhì)點(diǎn)偏離中心位移如下圖1所示。圖1 理想質(zhì)點(diǎn)偏離中心位移圖2.、模擬測(cè)量結(jié)果如下圖2所示。圖2 模擬質(zhì)點(diǎn)偏離中心位移測(cè)量結(jié)果3、卡爾曼濾波后結(jié)果如下圖3所示。圖3 卡爾曼濾波后結(jié)果4.、結(jié)果分析圖1中正弦波模擬了理想質(zhì)點(diǎn)偏離中心的位移圖，圖2在圖1的基礎(chǔ)上添加了隨機(jī)白噪聲，用以模擬測(cè)量得到的結(jié)果。圖3是經(jīng)過卡爾曼濾波后輸出的結(jié)果，可以看到圖3中數(shù)據(jù)