控制系統(tǒng)仿真課程設(shè)計(jì)--基于kalman濾波的信息融合算法設(shè)計(jì)(存儲(chǔ)版)
【正文】 endend %預(yù)測(cè)數(shù)據(jù)和估計(jì)數(shù)據(jù)2for k=1:50 if k==1 X_yc2(:,k)=A*X_0。+Q。end% 融合for k=1:50 P_rh(:,:,k)=inv(P_gj2(:,:,k))+inv(P_gj1(:,:,k))。*r39。局部2分量一39。,t,X_gj1(2,t),39。,39。ob39。hold offlegend(39。)ylabel(39。 五、致謝非常感謝葛老師及研究生學(xué)長(zhǎng)在我上機(jī)過(guò)程為我解答疑問(wèn)，對(duì)我的錯(cuò)誤提出指正。體驗(yàn)到了跟以往不同的學(xué)習(xí)方式，提高了自己的自學(xué)能力。)xlabel(39。*k39。)%繪制融合估計(jì)誤差協(xié)方差和每個(gè)局部估計(jì)誤差協(xié)方差曲線figure(3)k=1:50。,39。plot(t,X_rhgj(2,t),39。局部1分量一39。ok39。 P_gj2(:,:,k)=(eye(2)K2(:,:,k)*H)*P_yc2(:,:,k)。 Z_yc2(:,k)=H*X_yc2(:,k)。 else X2(:,k)=A*X2(:,k1)+W(:,k)。/(H*P_yc1(:,:,k)*H39。+Q。 V1(:,k)=sqrt(R)*randn(2,1)。1]。clc。整個(gè)時(shí)也是估計(jì)值更準(zhǔn)確。,39。)ylabel(39。)hold offlegend(39。)ylabel(39。)hold offlegend(39。 T_gj(k)=trace(P_gj(:,:,k))。 P_yc(:,:,k)=A*P_gj(:,:,k1)*A39。 else X(:,k)=A*X(:,k1)+W(:,k)。Q=[ 0。P0=[100 10。)ylabel(39。legend(39。)ylabel(39。,39。hold onplot(X_yc,39。 %協(xié)方差預(yù)測(cè)值 end K(k)=P_yc(k)*H39。 Z(k)=H*X(k)+V(k)。X0=10。三、主要內(nèi)容一）線性系統(tǒng)的Kalman濾波考慮如下一類單傳感器線性動(dòng)態(tài)估計(jì)系統(tǒng) (1) (2)其中，是離散的時(shí)間變量；是系統(tǒng)的狀態(tài)向量，是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；是狀態(tài)的觀測(cè)向量，是相應(yīng)的觀測(cè)矩陣；和是零均值的高斯白噪聲過(guò)程，且滿足如下條件：， (3)初始狀態(tài)為一隨機(jī)向量，且滿足 (4)那么，線性系統(tǒng)的Kalman濾波基本公式如下：① 計(jì)算狀態(tài)的一步預(yù)測(cè)值(5)② 計(jì)算一步預(yù)測(cè)誤差協(xié)方差陣(6)③ 計(jì)算增益陣 (7)④ 計(jì)算狀態(tài)估計(jì)值 (8)和估計(jì)誤差協(xié)方差陣 (9)其中和為時(shí)刻的狀態(tài)估計(jì)以及相應(yīng)的估計(jì)誤差協(xié)方差陣。iii) 開(kāi)始Kalman濾波估計(jì)從開(kāi)始循環(huán)（L為給定的仿真時(shí)刻長(zhǎng)度）a) 當(dāng)時(shí)a1) 根據(jù)式(5)和式(6)有, a2) 利用式(7)(9)計(jì)算估計(jì)和相應(yīng)的估計(jì)誤差協(xié)方差矩陣。Q=。 %觀測(cè)向量 endend%Kalman濾波for k=1:100 if k==1 X_yc(k)=A*X0_
試題試卷相關(guān)推薦
鄂ICP備17016276號(hào)-1