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

1、<p>　　控制系統(tǒng)仿真課程設(shè)計</p><p><b>　?。?011級）</b></p><p>　　控制系統(tǒng)仿真課程設(shè)計</p><p><b>　　一、題目</b></p><p>　　基于Kalman濾波的信息融合算法設(shè)計</p><p>　　1) 學習并

2、掌握線性系統(tǒng)Kalman濾波的基本原理和基本公式；</p><p>　　2) 學習并掌握一種常用的融合算法；</p><p>　　3) 學習并利用Matlab軟件實現(xiàn)基本的Kalman濾波和信息融合算法的仿真。</p><p><b>　　二、主要要求</b></p><p>　　1) 具備基本的概率與數(shù)理統(tǒng)計知識；&l

3、t;/p><p>　　2) 熟悉并掌握基本的Matlab軟件編寫能力；</p><p>　　3) 學習并掌握正交投影定理和矩陣求逆定理；</p><p>　　4) 了解Kalman濾波的功能、來源和基本原理；</p><p>　　5) 掌握Kalman濾波的推導過程和基本運行公式；</p><p>　　6) 了解信息融合的

4、基本概念和方法；</p><p>　　7) 掌握一種典型的多傳感器信息融合算法：分布式局部估計值加權(quán)融合。</p><p><b>　　三、主要內(nèi)容</b></p><p>　　一）線性系統(tǒng)的Kalman濾波</p><p>　　考慮如下一類單傳感器線性動態(tài)估計系統(tǒng)</p><p><b&g

5、t;　　(1)</b></p><p><b>　　(2)</b></p><p>　　其中，是離散的時間變量；是系統(tǒng)的狀態(tài)向量，是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；是狀態(tài)的觀測向量，是相應(yīng)的觀測矩陣；和是零均值的高斯白噪聲過程，且滿足如下條件：</p><p>　　， (3)</p><p>　　初始狀態(tài)

6、為一隨機向量，且滿足</p><p><b>　　(4)</b></p><p>　　那么，線性系統(tǒng)的Kalman濾波基本公式如下：</p><p>　　計算狀態(tài)的一步預測值</p><p><b>　　(5)</b></p><p>　　計算一步預測誤差協(xié)方差陣</p

7、><p><b>　　(6)</b></p><p><b>　　計算增益陣</b></p><p><b>　　(7)</b></p><p><b>　　計算狀態(tài)估計值</b></p><p><b>　　(8)</

8、b></p><p><b>　　和估計誤差協(xié)方差陣</b></p><p><b>　　(9)</b></p><p>　　其中和為時刻的狀態(tài)估計以及相應(yīng)的估計誤差協(xié)方差陣。</p><p>　　那么，Kalman濾波仿真程序執(zhí)行方案如下：</p><p>　　確定初

9、始狀態(tài)、初始狀態(tài)估計和相應(yīng)的協(xié)方差矩陣；給定狀態(tài)轉(zhuǎn)移矩陣、過程噪聲方差、測量矩陣和測量噪聲方差（這些量均可認為是常量）</p><p><b>　　產(chǎn)生仿真信號數(shù)據(jù)</b></p><p>　　從開始循環(huán)（L為給定的仿真時刻長度）</p><p><b>　　當時</b></p><p>　　a1)

10、 利用和隨機函數(shù)產(chǎn)生一個高斯白噪聲；</p><p>　　a2) 根據(jù)式(1)有；</p><p>　　a3) 利用和隨機函數(shù)產(chǎn)生一個高斯白噪聲；</p><p>　　a4) 根據(jù)式(2)有。</p><p><b>　　當時</b></p><p>　　b1) 利用和隨機函數(shù)產(chǎn)生一個高斯白噪聲；

11、</p><p>　　b2) 根據(jù)式(1)有；</p><p>　　b3) 利用和隨機函數(shù)產(chǎn)生一個高斯白噪聲；</p><p>　　b4) 根據(jù)式(2)有。</p><p>　　開始Kalman濾波估計</p><p>　　從開始循環(huán)（L為給定的仿真時刻長度）</p><p><b>

12、　　當時</b></p><p>　　a1) 根據(jù)式(5)和式(6)有</p><p><b>　　, </b></p><p>　　a2) 利用式(7)-(9)計算估計和相應(yīng)的估計誤差協(xié)方差矩陣。</p><p><b>　　當時</b></p><p>　　

13、b1) 根據(jù)式(5)和式(6)計算和；</p><p>　　b2) 利用式(7)-(9)計算估計和相應(yīng)的估計誤差協(xié)方差矩陣。</p><p>　　問題：給定相應(yīng)參數(shù)（也鼓勵采用其他參數(shù)），進行Kalman濾波估計算法程序的編寫，并進行繪圖和分析</p><p><b>　　標量情形：</b></p><p><b&

14、gt;　　，，，，，，</b></p><p>　?。?）請利用Matlab軟件進行Kalman濾波估計仿真程序編寫；</p><p><b>　　%初始化</b></p><p><b>　　clear;</b></p><p><b>　　A=1;</b><

15、;/p><p><b>　　P0=100;</b></p><p><b>　　X0=10;</b></p><p><b>　　X0_est=1;</b></p><p><b>　　H=1;</b></p><p><b>

16、;　　Q=0.1;</b></p><p><b>　　R=8;</b></p><p>　　%產(chǎn)生仿真信號和數(shù)據(jù)</p><p>　　for k=1:100</p><p>　　W(k)=sqrt(Q)*randn(1); %產(chǎn)生高斯白噪聲W</p><p>　　V(k)=sqrt(

17、R)*randn(1); %產(chǎn)生高斯白噪聲V</p><p><b>　　if k==1</b></p><p>　　X(k)=A*X0+W(k);</p><p>　　Z(k)=H*X(k)+V(k);</p><p><b>　　else</b></p><p>　　X(

18、k)=A*X(k-1)+W(k); %狀態(tài)值</p><p>　　Z(k)=H*X(k)+V(k); %觀測向量</p><p><b>　　end</b></p><p><b>　　end</b></p><p><b>　　%Kalman濾波</b></p>

19、;<p>　　for k=1:100</p><p><b>　　if k==1</b></p><p>　　X_yc(k)=A*X0_est; </p><p>　　P_yc(k)=A*P0*A'+Q </p><p><b>　　else</b></p>

20、<p>　　X_yc(k)=A*X(k-1); %狀態(tài)預測值</p><p>　　P_yc(k)=A*P_gj(k-1)*A'+Q; %協(xié)方差預測值</p><p><b>　　end</b></p><p>　　K(k)=P_yc(k)*H'*inv(H*P_yc(k)*H'+R); %增益矩陣&l

21、t;/p><p>　　X_gj(k)=X_yc(k)+K(k)*(Z(k)-H*X_yc(k)); %狀態(tài)估計值</p><p>　　P_gj(k)=(eye(1)-K(k)*H)*P_yc(k); %協(xié)方差估計值</p><p><b>　　end</b></p><p>　　%繪制狀態(tài)估計與狀態(tài)預測值的曲線圖<

22、/p><p><b>　　figure(1)</b></p><p><b>　　hold on</b></p><p>　　plot(X_gj,'r');</p><p><b>　　hold on</b></p><p>　　plot(X

23、_yc,'-ob');</p><p><b>　　hold on</b></p><p>　　plot(X,'-*g');</p><p>　　legend('狀態(tài)估計值','狀態(tài)預測值','狀態(tài)')</p><p>　　title(

24、9;狀態(tài)估計與狀態(tài)預測值的曲線圖')</p><p>　　xlabel('仿真次數(shù)')</p><p>　　ylabel('數(shù)值')</p><p>　　%繪制預測誤差協(xié)方差和估計誤差協(xié)方差曲線</p><p><b>　　figure(2)</b></p><

25、p><b>　　hold on</b></p><p>　　plot(P_gj,'r');</p><p><b>　　hold on</b></p><p>　　plot(P_yc,'-ob');</p><p>　　legend('估計誤差協(xié)方差&#

26、39;,'預測誤差協(xié)方')</p><p>　　title('估計誤差協(xié)方差與預測誤差協(xié)方的曲線圖')</p><p>　　xlabel('仿真次數(shù)')</p><p>　　ylabel('數(shù)值')</p><p>　?。?）繪出狀態(tài)預測值和狀態(tài)估計值的曲線圖；</p>

27、;<p>　?。?）繪出預測誤差協(xié)方差和估計誤差協(xié)方差的曲線圖；</p><p>　?。?）對仿真結(jié)果進行分析。</p><p>　　預測值和估計值都能夠在一定程度上反應(yīng)真實值，但是估計值比觀測值更接近真實值。</p><p>　　狀態(tài)估計值：表明估計值是在預測值的基礎(chǔ)上進行優(yōu)化后得到結(jié)果，所以估計值更準確一些。</p><p>

28、;<b>　　矢量情形：</b></p><p><b>　　，，，，</b></p><p><b>　　，，</b></p><p>　?。?）請利用Matlab軟件進行Kalman濾波估計仿真程序編寫；</p><p><b>　　%程序初始值</b>

29、;</p><p><b>　　clear;</b></p><p>　　A=[1 1;0 1];</p><p>　　P0=[100 10;10 100];</p><p>　　x0=[1;0.1];</p><p>　　X_0=[10;1];</p><p>　　H=[

30、1 0;0 1];</p><p>　　Q=[0.1 0;0 0.1];</p><p>　　R=[5 0;0 5];</p><p><b>　　%產(chǎn)生仿真信號數(shù)據(jù)</b></p><p>　　for k=1:50</p><p>　　W(:,k)=sqrt(Q)*randn(2,1); &l

31、t;/p><p>　　V(:,k)=sqrt(R)*randn(2,1); </p><p><b>　　if k==1</b></p><p>　　X(:,k)=A*x0+W(:,k);</p><p>　　Z(:,k)=H*X(:,k)+V(:,k);</p><p><b>　　el

32、se </b></p><p>　　X(:,k)=A*X(:,k-1)+W(:,k);</p><p>　　Z(:,k)=H*X(:,k)+V(:,k);</p><p><b>　　end</b></p><p><b>　　end</b></p><p>&l

33、t;b>　　%Kalman濾波</b></p><p>　　for k=1:50</p><p><b>　　if k==1</b></p><p>　　X_yc(:,k)=A*X_0;</p><p>　　P_yc(:,:,k)=A*P0*A'+Q;</p><p>　

34、　T_yc(k)=trace(P_yc(:,:,k));</p><p><b>　　else</b></p><p>　　X_yc(:,k)=A*X_gj(:,k-1);</p><p>　　Z_yc(:,k)=H*X_yc(:,k);</p><p>　　P_yc(:,:,k)=A*P_gj(:,:,k-1)*A&#

35、39;+Q;</p><p>　　T_yc(k)=trace(P_yc(:,:,k));</p><p><b>　　end</b></p><p>　　K(:,:,k)=P_yc(:,:,k)*H'*inv(H*P_yc(:,:,k)*H'+R);</p><p>　　X_gj(:,k)=X_yc(:,

36、k)+K(:,:,k)*(Z(:,k)-H*X_yc(:,k));</p><p>　　P_gj(:,:,k)=(eye(2)-K(:,:,k)*H)*P_yc(:,:,k);</p><p>　　T_gj(k)=trace(P_gj(:,:,k));</p><p><b>　　end</b></p><p>　　%繪

37、制狀態(tài)預測值和狀態(tài)估計值分量一曲線</p><p>　　figure(1) </p><p><b>　　k=1:50;</b></p><p>　　plot(k,X_gj(1,k),'-or')</p><p><b>　　hold on</b></p><p&

38、gt;　　plot(k,X_yc(1,k),'k')</p><p><b>　　hold on</b></p><p>　　plot(k,X(1,k),'-*b')</p><p><b>　　hold off</b></p><p>　　legend('分

39、量一估計','分量一預測','分量—狀態(tài)')</p><p>　　xlabel('仿真次數(shù)')</p><p>　　ylabel('數(shù)值')</p><p>　　%繪制狀態(tài)預測值和狀態(tài)估計值分量二曲線</p><p><b>　　figure(2)</b&

40、gt;</p><p>　　plot(k,X_gj(2,k),'-ok')</p><p><b>　　hold on</b></p><p>　　plot(k,X_yc(2,k),'r')</p><p><b>　　hold on</b></p>&

41、lt;p>　　plot(k,X(2,k),'-*b')</p><p><b>　　hold off</b></p><p>　　legend('分量二估計','分量二預測','分量二狀態(tài)')</p><p>　　xlabel('仿真次數(shù)')</p>

42、;<p>　　ylabel('數(shù)值')</p><p>　　%繪制預測誤差協(xié)方差陣跡和估計誤差協(xié)方差跡的曲線</p><p><b>　　figure(3)</b></p><p>　　plot(k,T_gj(k),'-ok',k,T_yc(k),'r')</p>&l

43、t;p>　　legend('估計','預測')</p><p>　　xlabel('仿真次數(shù)')</p><p>　　ylabel('數(shù)值')</p><p>　?。?）繪出狀態(tài)預測值和狀態(tài)估計值的曲線圖（每個狀態(tài)包括兩個分量）；</p><p><b>　　圖1

44、.1</b></p><p><b>　　圖2.1</b></p><p>　?。?）繪出預測誤差協(xié)方差陣跡(Trace)和估計誤差協(xié)方差陣跡的曲線圖；</p><p><b>　　圖3.1</b></p><p>　　（4）對仿真結(jié)果進行分析。</p><p>

45、　　分量的估計值比分量的觀測值更接近真實值。整個時也是估計值更準確。</p><p>　　針對矢量情形，自行選取三組不同的參數(shù)進行Kalman濾波的仿真，并進行相應(yīng)仿真結(jié)果的比較分析。</p><p>　　改變Q變大（Q=10）</p><p>　　改變R增大（R=10）</p><p>　　改變H變小（H=0.3）</p>&

46、lt;p>　　當R的值變小時，預測值的陣跡會變得下墜更快，預測值本身的震蕩會減小，對真實值的偏離會變小。當Q的值增大時，估計值也會更加偏離真實值。當H變小時，預測值與真實值偏差變大，估計值與真實值的偏差也會變大。</p><p>　　二）基于線性Kalman濾波信息融合算法</p><p>　　考慮如下一類多傳感器線性動態(tài)估計系統(tǒng)</p><p><b&

47、gt;　　(10)</b></p><p>　　， (11)</p><p>　　其中，是離散的時間變量，為傳感器的數(shù)目；是系統(tǒng)的狀態(tài)向量，是系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣；是狀態(tài)的觀測向量，是相應(yīng)的觀測矩陣；和是零均值的高斯白噪聲過程，且滿足如下條件：</p><p>　　， (12)</p><

48、p>　　初始狀態(tài)為一隨機向量，且滿足</p><p><b>　　(13)</b></p><p>　　那么，對于每一個傳感器觀測均可執(zhí)行一)當中基于單個觀測的Kalman濾波估計，可得到個局部估計和相應(yīng)的估計誤差協(xié)方差矩陣。從而，可利用分布式加權(quán)融合技術(shù)將上述個局部Kalman濾波估計進行融合，即：</p><p><b>　

49、　(14)</b></p><p>　　此時，和為融合后的狀態(tài)估計和相應(yīng)的融合估計誤差協(xié)方差矩陣。</p><p>　　問題：給定相應(yīng)參數(shù)（也鼓勵采用其他參數(shù)），進行上述分布式融合算法的仿真</p><p><b>　　給定如下參數(shù)：</b></p><p><b>　　，，，，</b>

50、</p><p><b>　　，，</b></p><p>　?。?）請利用Matlab軟件進行分布式融合估計算法仿真程序編寫；</p><p><b>　　%初始化</b></p><p><b>　　clear;</b></p><p><b&

51、gt;　　clc;</b></p><p>　　A=[1 1;0 1];</p><p>　　P0=[100 10;10 100];</p><p>　　x0=[1;0.1];</p><p>　　X_0=[10;1];</p><p>　　H=[1 0;0 1];</p><p>

52、　　Q=[0.1 0;0 0.1];</p><p>　　R=[4 0;0 4];</p><p><b>　　%觀測器一真實數(shù)據(jù)</b></p><p>　　for k=1:50</p><p>　　W(:,k)=sqrt(Q)*randn(2,1);</p><p>　　V1(:,k)=sqr

53、t(R)*randn(2,1);</p><p><b>　　if k==1</b></p><p>　　X1(:,k)=A*x0+W(:,k);</p><p>　　Z1(:,k)=H*X1(:,k)+V1(:,k);</p><p><b>　　else </b></p><

54、p>　　X1(:,k)=A*X1(:,k-1)+W(:,k);</p><p>　　Z1(:,k)=H*X1(:,k)+V1(:,k);</p><p><b>　　end</b></p><p><b>　　end</b></p><p>　　%預測數(shù)據(jù)和估計數(shù)據(jù)1</p>&

55、lt;p>　　for k=1:50</p><p><b>　　if k==1</b></p><p>　　X_yc1(:,k)=A*X_0;</p><p>　　Z_yc1(:,k)=H*X_yc1(:,k);</p><p>　　P_yc1(:,:,k)=A*P0*A'+Q;</p>&l

56、t;p>　　T_yc1(k)=trace(P_yc1(:,:,k));</p><p><b>　　else</b></p><p>　　X_yc1(:,k)=A*X_gj1(:,k-1);</p><p>　　Z_yc1(:,k)=H*X_yc1(:,k);</p><p>　　P_yc1(:,:,k)=A*P_

57、gj1(:,:,k-1)*A'+Q;</p><p>　　T_yc1(k)=trace(P_yc1(:,:,k));</p><p><b>　　end</b></p><p>　　K1(:,:,k)=P_yc1(:,:,k)*H'/(H*P_yc1(:,:,k)*H'+R);</p><p>　

58、　X_gj1(:,k)=X_yc1(:,k)+K1(:,:,k)*(Z1(:,k)-Z_yc1(:,k));</p><p>　　P_gj1(:,:,k)=(eye(2)-K1(:,:,k)*H)*P_yc1(:,:,k);</p><p>　　T_gj1(k)=trace(P_gj1(:,:,k));</p><p><b>　　end</b>

59、;</p><p><b>　　%觀測器2真實數(shù)據(jù)</b></p><p>　　for k=1:50</p><p>　　V2(:,k)=sqrt(R)*randn(2,1);</p><p><b>　　if k==1</b></p><p>　　X2(:,k)=A*x0+

60、W(:,k);</p><p>　　Z2(:,k)=H*X2(:,k)+V2(:,k);</p><p><b>　　else </b></p><p>　　X2(:,k)=A*X2(:,k-1)+W(:,k);</p><p>　　Z2(:,k)=H*X2(:,k)+V2(:,k);</p><p&

61、gt;<b>　　end</b></p><p><b>　　end</b></p><p>　　%預測數(shù)據(jù)和估計數(shù)據(jù)2</p><p>　　for k=1:50</p><p><b>　　if k==1</b></p><p>　　X_yc2(:,k

62、)=A*X_0;</p><p>　　Z_yc2(:,k)=H*X_yc2(:,k);</p><p>　　P_yc2(:,:,k)=A*P0*A'+Q;</p><p>　　T_yc2(k)=trace(P_yc2(:,:,k));</p><p><b>　　else</b></p><p

63、>　　X_yc2(:,k)=A*X_gj2(:,k-1);</p><p>　　Z_yc2(:,k)=H*X_yc2(:,k);</p><p>　　P_yc2(:,:,k)=A*P_gj2(:,:,k-1)*A'+Q;</p><p>　　T_yc2(k)=trace(P_yc2(:,:,k));</p><p><b&

64、gt;　　end</b></p><p>　　K2(:,:,k)=P_yc2(:,:,k)*H'/(H*P_yc2(:,:,k)*H'+R);</p><p>　　X_gj2(:,k)=X_yc2(:,k)+K2(:,:,k)*(Z2(:,k)-Z_yc2(:,k));</p><p>　　P_gj2(:,:,k)=(eye(2)-K2(

65、:,:,k)*H)*P_yc2(:,:,k);</p><p>　　T_gj2(k)=trace(P_gj2(:,:,k));</p><p><b>　　end</b></p><p><b>　　% 融合</b></p><p>　　for k=1:50</p><p>

66、　　P_rh(:,:,k)=inv(P_gj2(:,:,k))+inv(P_gj1(:,:,k));</p><p>　　P_rhgj(:,:,k)=inv(P_rh(:,:,k));</p><p>　　T_rhgj(k)=trace(P_rhgj(:,:,k)); X_rhgj(:,k)=P_rhgj(:,:,k)*inv(P_gj2(:,:,k))*X_gj2(:,k)+P_r

67、hgj(:,:,k)*inv(P_gj1(:,:,k))*X_gj1(:,k);</p><p><b>　　end</b></p><p>　　%繪制融合狀態(tài)估計局部1局部2分量一曲線</p><p><b>　　figure(1)</b></p><p><b>　　t=1:50;&l

68、t;/b></p><p>　　plot(t,X_rhgj(1,t),'-ok',t,X_gj1(1,t),'-*r',t,X_gj2(1,t),'-b')</p><p>　　legend('融合狀態(tài)分量一','局部1分量一','局部2分量一')</p><p>

69、　　xlabel('仿真次數(shù)')</p><p>　　ylabel('數(shù)值')</p><p>　　%繪制融合狀態(tài)估計局部1局部2分量一曲線</p><p><b>　　figure(2)</b></p><p><b>　　t=1:50;</b></p>

70、<p>　　plot(t,X_rhgj(2,t),'-or',t,X_gj1(2,t),'-*k',t,X_gj2(2,t),'-b')</p><p>　　legend('融合狀態(tài)分量二','局部1分量二','局部2分量二')</p><p>　　xlabel('仿真次數(shù)

71、')</p><p>　　ylabel('數(shù)值')</p><p>　　%繪制融合估計誤差協(xié)方差和每個局部估計誤差協(xié)方差曲線</p><p><b>　　figure(3)</b></p><p><b>　　k=1:50;</b></p><p>　

72、　plot(T_gj1(1,k),'-ob');</p><p><b>　　hold on</b></p><p>　　plot(T_gj2(1,k),'-r');</p><p><b>　　hold on</b></p><p>　　plot(T_rhgj(1,

73、k),'-*k');</p><p><b>　　hold off</b></p><p>　　legend('估計一陣跡','估計二陣跡','估計融合陣跡')</p><p>　　xlabel('仿真次數(shù)')</p><p>　　ylabe

74、l('數(shù)值')</p><p>　?。?）繪出融合狀態(tài)估計和每個局部估計狀態(tài)估計值的曲線圖</p><p>　?。?）繪出融合估計誤差協(xié)方差矩陣跡和每個局部估計誤差協(xié)方差陣跡曲線圖；</p><p>　?。?）對上述仿真結(jié)果進行分析。</p><p>　　融合之后的估計值與真實值的誤差比兩個分量都要小。這是因為通過多個設(shè)備對同

75、一狀態(tài)的加權(quán)估計，得到的結(jié)果可以進一步減小與真實值的誤差。</p><p><b>　　四、實踐總結(jié)</b></p><p>　　經(jīng)過這兩個星期的短學期，我對kalman濾波有了一定的了解，通過編程對Matlab軟件更加熟悉。體驗到了跟以往不同的學習方式，提高了自己的自學能力。從中學到了不少以前沒有深入過的知識，讓我獲益匪淺。 </p><p&g