基于Kalman濾波的信息融合算法設(shè)計(jì)_控制系統(tǒng)仿真課程設(shè)計(jì)

1、控制系統(tǒng)仿真課程設(shè)計(jì)題目控制系統(tǒng)仿真課程設(shè)計(jì)學(xué)院專業(yè)班級(jí)學(xué)號(hào)學(xué)生姓名指導(dǎo)教師完成日期控制系統(tǒng)仿真課程設(shè)計(jì)一、題目基于Kalman濾波的信息融合算法設(shè)計(jì)1) 學(xué)習(xí)并掌握線性系統(tǒng)Kalman濾波的根本原理和根本公式；2) 學(xué)習(xí)并掌握一種常用的融合算法；3) 學(xué)習(xí)并利用Matlab軟件實(shí)現(xiàn)根本的Kalman濾波和信息融合算法的仿真。二、主要要求1) 具備根本的概率與數(shù)理統(tǒng)計(jì)知識(shí)；2) 熟悉并掌握根本的Matlab軟件編寫能力；3) 學(xué)習(xí)并掌握正交投影定理和矩陣求逆定理；4) 了解Kalman濾波的功能、來源和根本原理；5) 掌握Kalman濾波的推導(dǎo)過程和根本運(yùn)行公式；6) 了解信息融合的根本概念和

2、方法；7) 掌握一種典型的多傳感器信息融合算法：分布式局部估計(jì)值加權(quán)融合。三、主要內(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è)矩陣；和是零均值的高斯白噪聲過程，且滿足如下條件：， (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é)方差陣。那么，Kalman

3、濾波仿真程序執(zhí)行方案如下：i) 確定初始狀態(tài)、初始狀態(tài)估計(jì)和相應(yīng)的協(xié)方差矩陣；給定狀態(tài)轉(zhuǎn)移矩陣、過程噪聲方差、測(cè)量矩陣和測(cè)量噪聲方差這些量均可認(rèn)為是常量ii) 產(chǎn)生仿真信號(hào)數(shù)據(jù)從開始循環(huán)L為給定的仿真時(shí)刻長(zhǎng)度a) 當(dāng)時(shí)a1) 利用和隨機(jī)函數(shù)產(chǎn)生一個(gè)高斯白噪聲；a2) 根據(jù)式(1)有；a3) 利用和隨機(jī)函數(shù)產(chǎn)生一個(gè)高斯白噪聲；a4) 根據(jù)式(2)有。b) 當(dāng)時(shí)b1) 利用和隨機(jī)函數(shù)產(chǎn)生一個(gè)高斯白噪聲；b2) 根據(jù)式(1)有；b3) 利用和隨機(jī)函數(shù)產(chǎn)生一個(gè)高斯白噪聲；b4) 根據(jù)式(2)有。iii) 開始Kalman濾波估計(jì)從開始循環(huán)L為給定的仿真時(shí)刻長(zhǎng)度a) 當(dāng)時(shí)a1) 根據(jù)式(5)和式(6)有

4、, a2) 利用式(7)-(9)計(jì)算估計(jì)和相應(yīng)的估計(jì)誤差協(xié)方差矩陣。b) 當(dāng)時(shí)b1) 根據(jù)式(5)和式(6)計(jì)算和；b2) 利用式(7)-(9)計(jì)算估計(jì)和相應(yīng)的估計(jì)誤差協(xié)方差矩陣。問題：給定相應(yīng)參數(shù)也鼓勵(lì)采用其他參數(shù)，進(jìn)行Kalman濾波估計(jì)算法程序的編寫，并進(jìn)行繪圖和分析1) 標(biāo)量情形：，1請(qǐng)利用Matlab軟件進(jìn)行Kalman濾波估計(jì)仿真程序編寫；%produce systemclear;A=1;P0=100;X0=10;C=1;Q=0.1;R=10;%real states and measure statesfor k=1:150W(k)=sqrt(Q)*randn(1,1);V(k)

5、=sqrt(R)*randn(1,1);if k=1X(k)=A*X0+W(k);Z(k)=C*X(k)+V(k);elseX(k)=A*X(k-1)+W(k);Z(k)=C*X(k)+V(k);endend%predict states and estimate statesfor k=1:150if k=1X_yc(k)=A*X0;Z_yc(k)=C*X_yc(k);P_yc(k)=A*P0*A'+Q;K(k)=P_yc(k)*C'/(C*P_yc(k)*C'+R);X_gj(k)=X_yc(k)+K(k)*(Z(k)-Z_yc(k);P_gj(k)=(eye(1)

6、-K(k)*C)*P_yc(k);elseX_yc(k)=A*X(k-1);Z_yc(k)=C*X_yc(k);P_yc(k)=A*P_yc(k-1)*A'+Q;K(k)=P_yc(k)*C'/(C*P_yc(k)*C'+R);X_gj(k)=X_yc(k)+K(k)*(Z(k)-Z_yc(k);P_gj(k)=(eye(1)-K(k)*C)*P_yc(k);endend%create figurefiguret=1:150;plot(t,Z(1,t),'-og')hold onplot(t,X_gj(1,t),'r')hold onpl

7、ot(t,X(1,t),'b')hold offlegend('觀測(cè)','估計(jì)','狀態(tài)')xlabel('仿真次數(shù)')ylabel('數(shù)值')figureplot(abs(Z-X),'-og');hold onplot(abs(X_gj-X);hold offlegend('預(yù)測(cè)與真實(shí)之差','估計(jì)與真實(shí)之差')xlabel('仿真次數(shù)')ylabel('數(shù)值')2繪出狀態(tài)預(yù)測(cè)值和狀態(tài)估計(jì)值的曲線圖；3繪出預(yù)測(cè)誤差協(xié)方

8、差和估計(jì)誤差協(xié)方差的曲線圖；4對(duì)仿真結(jié)果進(jìn)行分析。預(yù)測(cè)值和估計(jì)值都能夠在一定程度上反響真實(shí)值，但是估計(jì)值比觀測(cè)值更接近真實(shí)值。狀態(tài)估計(jì)值：說明估計(jì)值是在預(yù)測(cè)值的根底上進(jìn)行優(yōu)化后得到結(jié)果，所以估計(jì)值更準(zhǔn)確一些。2) 矢量情形：，1請(qǐng)利用Matlab軟件進(jìn)行Kalman濾波估計(jì)仿真程序編寫；%produce systemclear;A=1 1;0 1;P0=100 10;10 100;x0=1;0.1;X_0=10;1;C=1 0;0 1;Q=0.1 0;0 0.1;R=10 0;0 10;%real states and measure statesfor k=1:150 W(:,k)=sqrt

9、(Q)*randn(2,1); V(:,k)=sqrt(R)*randn(2,1); if k=1 X(:,k)=A*x0+W(:,k); Z(:,k)=C*X(:,k)+V(:,k); else X(:,k)=A*X(:,k-1)+W(:,k); Z(:,k)=C*X(:,k)+V(:,k); endend%predict states and estimate statesfor k=1:150 if k=1 X_yc(:,k)=A*X_0; Z_yc(:,k)=C*X_yc(:,k); P_yc(:,:,k)=A*P0*A'+Q; T_yc(k)=trace(P_yc(:,:,k

10、); K(:,:,k)=P_yc(:,:,k)*C'/(C*P_yc(:,:,k)*C'+R); X_gj(:,k)=X_yc(:,k)+K(:,:,k)*(Z(:,k)-Z_yc(:,k); P_gj(:,:,k)=(eye(2)-K(:,:,k)*C)*P_yc(:,:,k); T_gj(k)=trace(P_gj(:,:,k); else X_yc(:,k)=A*X_gj(:,k-1); Z_yc(:,k)=C*X_yc(:,k); P_yc(:,:,k)=A*P_gj(:,:,k-1)*A'+Q; T_yc(k)=trace(P_yc(:,:,k); K(:,:

11、,k)=P_yc(:,:,k)*C'/(C*P_yc(:,:,k)*C'+R); X_gj(:,k)=X_yc(:,k)+K(:,:,k)*(Z(:,k)-Z_yc(:,k); P_gj(:,:,k)=(eye(2)-K(:,:,k)*C)*P_yc(:,:,k); T_gj(k)=trace(P_gj(:,:,k); endend%create figurefigure t=1:150;plot(t,X(1,t),'-or')hold onplot(t,X_gj(1,t),'g')plot(t,Z(1,t),'-')hold o

12、fflegend('分量一狀態(tài)','分量一估計(jì)','分量一預(yù)測(cè)')xlabel('仿真次數(shù)')ylabel('數(shù)值')figure plot(t,X(2,t),'-or',t,X_gj(2,t),'g')hold onplot(t,Z(2,t),'-')hold offlegend('分量二狀態(tài)','分量二估計(jì)','分量二預(yù)測(cè)')xlabel('仿真次數(shù)')ylabel('數(shù)值')figu

13、replot(t,abs(Z(2,t)-X(2,t),'-or')hold onplot(t,abs(X_gj(2,t)-X(2,t),'g')hold offlegend('預(yù)測(cè)與真實(shí)之差','估計(jì)與真實(shí)之差')xlabel('仿真次數(shù)')ylabel('數(shù)值')figureplot(t,T_gj(t),'g',t,T_yc(t),'-or')legend('估計(jì)','預(yù)測(cè)')xlabel('仿真次數(shù)')ylabel(

14、'數(shù)值')2繪出狀態(tài)預(yù)測(cè)值和狀態(tài)估計(jì)值的曲線圖每個(gè)狀態(tài)包括兩個(gè)分量；圖圖3繪出預(yù)測(cè)誤差協(xié)方差陣跡(Trace)和估計(jì)誤差協(xié)方差陣跡的曲線圖；圖4對(duì)仿真結(jié)果進(jìn)行分析。分量的估計(jì)值比分量的觀測(cè)值更接近真實(shí)值。整個(gè)時(shí)也是估計(jì)值更準(zhǔn)確。針對(duì)矢量情形，自行選取三組不同的參數(shù)進(jìn)行Kalman濾波的仿真，并進(jìn)行相應(yīng)仿真結(jié)果的比擬分析。改變Q變大Q=4改變R變小R=4改變H變小當(dāng)R的值變小時(shí)，預(yù)測(cè)值的陣跡會(huì)變得下墜更快，預(yù)測(cè)值本身的震蕩會(huì)減小，對(duì)真實(shí)值的偏離會(huì)變小。當(dāng)Q的值增大時(shí)，估計(jì)值也會(huì)更加偏離真實(shí)值。當(dāng)H變小時(shí)，預(yù)測(cè)值與真實(shí)值偏差變大，估計(jì)值與真實(shí)值的偏差也會(huì)變大。二基于線性Kalman

15、濾波信息融合算法考慮如下一類多傳感器線性動(dòng)態(tài)估計(jì)系統(tǒng) (10)， (11)其中，是離散的時(shí)間變量，為傳感器的數(shù)目；是系統(tǒng)的狀態(tài)向量，是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；是狀態(tài)的觀測(cè)向量，是相應(yīng)的觀測(cè)矩陣；和是零均值的高斯白噪聲過程，且滿足如下條件：， (12)初始狀態(tài)為一隨機(jī)向量，且滿足 (13)那么，對(duì)于每一個(gè)傳感器觀測(cè)均可執(zhí)行一)當(dāng)中基于單個(gè)觀測(cè)的Kalman濾波估計(jì)，可得到個(gè)局部估計(jì)和相應(yīng)的估計(jì)誤差協(xié)方差矩陣。從而，可利用分布式加權(quán)融合技術(shù)將上述個(gè)局部Kalman濾波估計(jì)進(jìn)行融合，即： (14)此時(shí)，和為融合后的狀態(tài)估計(jì)和相應(yīng)的融合估計(jì)誤差協(xié)方差矩陣。問題：給定相應(yīng)參數(shù)也鼓勵(lì)采用其他參數(shù)，進(jìn)行上述分布

16、式融合算法的仿真給定如下參數(shù)：，1請(qǐng)利用Matlab軟件進(jìn)行分布式融合估計(jì)算法仿真程序編寫；%produce systemclear;clc;A=1 1;0 1;P0=100 10;10 100;x0=1;0.1;X_0=10;1;C=1 0;0 1;Q=0.1 0;0 0.1;R=4 0;0 4;%guan ce qi 1 real states and measure statesfor k=1:150 W(:,k)=sqrt(Q)*randn(2,1); V1(:,k)=sqrt(R)*randn(2,1); if k=1 X1(:,k)=A*x0+W(:,k); Z1(:,k)=C*X

17、1(:,k)+V1(:,k); else X1(:,k)=A*X1(:,k-1)+W(:,k); Z1(:,k)=C*X1(:,k)+V1(:,k); endend%predict states and estimate statesfor k=1:150 if k=1 X_yc1(:,k)=A*X_0; Z_yc1(:,k)=C*X_yc1(:,k); P_yc1(:,:,k)=A*P0*A'+Q; T_yc1(k)=trace(P_yc1(:,:,k); K1(:,:,k)=P_yc1(:,:,k)*C'/(C*P_yc1(:,:,k)*C'+R); X_gj1(:

18、,k)=X_yc1(:,k)+K1(:,:,k)*(Z1(:,k)-Z_yc1(:,k); P_gj1(:,:,k)=(eye(2)-K1(:,:,k)*C)*P_yc1(:,:,k); T_gj1(k)=trace(P_gj1(:,:,k); else X_yc1(:,k)=A*X_gj1(:,k-1); Z_yc1(:,k)=C*X_yc1(:,k); P_yc1(:,:,k)=A*P_gj1(:,:,k-1)*A'+Q; T_yc1(k)=trace(P_yc1(:,:,k); K1(:,:,k)=P_yc1(:,:,k)*C'/(C*P_yc1(:,:,k)*C'

19、;+R); X_gj1(:,k)=X_yc1(:,k)+K1(:,:,k)*(Z1(:,k)-Z_yc1(:,k); P_gj1(:,:,k)=(eye(2)-K1(:,:,k)*C)*P_yc1(:,:,k); T_gj1(k)=trace(P_gj1(:,:,k); endend%guan ce qi 2 real states and measure statesfor k=1:150 V2(:,k)=sqrt(R)*randn(2,1); if k=1 X2(:,k)=A*x0+W(:,k); Z2(:,k)=C*X2(:,k)+V2(:,k); else X2(:,k)=A*X2(:

20、,k-1)+W(:,k); Z2(:,k)=C*X2(:,k)+V2(:,k); endend%predict states and estimate statesfor k=1:150 if k=1 X_yc2(:,k)=A*X_0; Z_yc2(:,k)=C*X_yc2(:,k); P_yc2(:,:,k)=A*P0*A'+Q; T_yc2(k)=trace(P_yc2(:,:,k); K2(:,:,k)=P_yc2(:,:,k)*C'/(C*P_yc2(:,:,k)*C'+R); X_gj2(:,k)=X_yc2(:,k)+K2(:,:,k)*(Z2(:,k)-Z

21、_yc2(:,k); P_gj2(:,:,k)=(eye(2)-K2(:,:,k)*C)*P_yc2(:,:,k); T_gj2(k)=trace(P_gj2(:,:,k); else X_yc2(:,k)=A*X_gj2(:,k-1); Z_yc2(:,k)=C*X_yc2(:,k); P_yc2(:,:,k)=A*P_gj2(:,:,k-1)*A'+Q; T_yc2(k)=trace(P_yc2(:,:,k); K2(:,:,k)=P_yc2(:,:,k)*C'/(C*P_yc2(:,:,k)*C'+R); X_gj2(:,k)=X_yc2(:,k)+K2(:,:,

22、k)*(Z2(:,k)-Z_yc2(:,k); P_gj2(:,:,k)=(eye(2)-K2(:,:,k)*C)*P_yc2(:,:,k); T_gj2(k)=trace(P_gj2(:,:,k); endend% rong he for k=1:150 P_rh(:,:,k)=inv(P_gj2(:,:,k)+inv(P_gj1(:,:,k); P_rhgj(:,:,k)=inv(P_rh(:,:,k); T_rhgj(k)=trace(P_rhgj(:,:,k); X_rhgj(:,k)=P_rhgj(:,:,k)*inv(P_gj2(:,:,k)*X_gj2(:,k)+P_rhgj(:

23、,:,k)*inv(P_gj1(:,:,k)*X_gj1(:,k);end%create figurefiguret=1:150;plot(t,X2(1,t),'r',t,Z2(1,t),'g')hold onplot(t,X_gj2(1,t),'-ok',t,X_rhgj(1,t),'-')hold offlegend(' 觀測(cè)器2狀態(tài)一',',預(yù)測(cè)分量一',' 估計(jì)分量一),',' 融合分量一')xlabel('仿真次數(shù)')ylabel('數(shù)值')figureplot(t,X2(2,t),'r',t,Z2(2,t),'g')hold onplot(t,X_gj2(2,t),'-ok',t,X_rhgj(2,t),'-')hold offlegend(' 觀測(cè)器2狀態(tài)二',',預(yù)測(cè)分