控制系統(tǒng)仿真課程設計

控制系統(tǒng)仿真課程設計（2010級）題目控制系統(tǒng)仿真課程設計學院專業(yè)班級學號學生姓名指導教師完成日期控制系統(tǒng)仿真課程設計、題目基于Kalman濾波地信息融合算法設計學習并掌握線性系統(tǒng)Kalman濾波地基本原理和基本公式；學習并掌握一種常用地融合算法；學習并利用Matlab軟件實現基本地Kalman濾波和信息融合算法地仿真.二、主要要求具備基本地概率與數理統(tǒng)計知識；熟悉并掌握基本地Matlab軟件編寫能力；學習并掌握正交投影定理和矩陣求逆定理；了解Kalman濾波地功能、來源和基本原理；掌握Kalman濾波地推導過程和基本運行公式；了解信息融合地基本概念和方法；掌握一種典型地多傳感器信息融合算法：分布式局部估計值加權融合三、主要內容一)線性系統(tǒng)地Kalman濾波考慮如下一類單傳感器線性動態(tài)估計系統(tǒng)TOC\o"1-5"\h\zx(k)=魚(k,k-1)x(k-1)+w(k,k-1)(1)z(k)=H(k)x(k)+v(k)(2)其中，k>0是離散地時間變量；x(k-1)GRnx1是系統(tǒng)地狀態(tài)向量，①(k,k-1)GRnxn是系統(tǒng)地狀態(tài)轉移矩陣；Z(k)GRpx1是狀態(tài)x(k)地觀測向量，H(k)GRp鄧是相應地觀測矩陣；w(k,k-1)和v(k)是零均值地高斯白噪聲過程，且滿足如下條件：'E^w(k,k-1)}=0IE如k,k-1)w(j,j-5LQ(k,k-1)5<Ejv(k)}=0"，k,j>0(3)E\(k)v(j)J=R(f)5kj、EW(k,k-1)v(j)tJ=00J初始狀態(tài)x(0)為一隨機向量，且滿足

‘E§(0)}=‘E§(0)}=x

E1x(0)-x0]X(0)-x}}=P計算狀態(tài)地一步預測值X(kIk-1)=魚(k,k-1)x(k-11k-1)(5)計算一步預測誤差協(xié)方差陣P(kIk-1)二璽(k,k-1)P(k-1Ik-1)①(k,k-1)t+Q(k,k-1)⑹計算增益陣TOC\o"1-5"\h\zK(k)=P(kIk-1)Ht(k)H(k)P(kIk-1)Ht(k)+R(k)--1(7)計算狀態(tài)估計值x(kIk)=X(kIk-1)+K(k)lz(k)-H(k)x(kIk-1)](8)和估計誤差協(xié)方差陣P(kIk)=ll-K(k)H(k)^P(kIk-1)(9)其中X(k-1Ik-1)和P(k-1Ik-1)為k-1時刻地狀態(tài)估計以及相應地估計誤差協(xié)方差陣.那么，Kalman濾波仿真程序執(zhí)行方案如下：確定初始狀態(tài)x(0)、初始狀態(tài)估計X和相應地協(xié)方差矩陣P；給定狀態(tài)轉移矩陣00也(k,k-1)、過程噪聲方差Q(k,k-1)、測量矩陣H(k)和測量噪聲方差R(k)(這些量均可認為是常量)產生仿真信號數據從k=1:L開始循環(huán)(L為給定地仿真時刻長度)當k=1時al)利用Q(k,k-1)和隨機函數產生一個高斯白噪聲w(1,0)；a2)根據式(1)有x(1)=Q(1,0)x(0)+w(1,0)；a3)利用R(k)和隨機函數產生一個高斯白噪聲v(1)；a4)根據式⑵有z(1)=H(1)x(1)+v⑴.當k=2,3,,L時bl)利用Q(k,k-1)和隨機函數產生一個高斯白噪聲w(k,k-1)；b2)根據式(1)有x(k)=①(k,k-1)x(k-1)+w(k,k-1)；b3)利用R(k)和隨機函數產生一個高斯白噪聲v(k)；b4)根據式⑵有z(k)=H(k)x(k)+v(k).開始Kalman濾波估計從k=1:L開始循環(huán)（L為給定地仿真時刻長度）a）當k=1時al)根據式(5)和式(6)有x(110)=①(1,0)x,P(110)=①(1,0)PQ(1,0)t+Q(1,0)

00a2)利用式(7)-(9)計算估計X(111)和相應地估計誤差協(xié)方差矩陣P(111).b)當k=2,3,,L時bl)根據式(5)和式(6)計算X(kIk-1)和P(kIk-1)；b2)利用式(7)-(9)計算估計X(kIk)和相應地估計誤差協(xié)方差矩陣P(kIk).問題：給定相應參數(也鼓勵采用其他參數)，進行Kalman濾波估計算法程序地編寫，并進行繪圖和分析1)標量情形：x(0)=1，X0=10，P0=100，①(k,k-1)=1，Q(k,k-1)=0.1，H(k)=1，R(k)=0.1(1)請利用Matlab軟件進行Kalman濾波估計仿真程序編寫；%mykf.m%producesystemclear。A=1。P0=100。X0=10。C=1。Q=0.1。R=10。%realstatesandmeasurestatesfork=1:150W(k)=sqrt(Q)*randn(1,1)。V(k)=sqrt(R)*randn(1,1)。ifk==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%predictstatesandestimatestatesfork=1:150ifk==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)-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%createfigurefiguret=1:150。plot(t,Z(1,t),'-og')holdonplot(t,X_gj(1,t),'r')holdonholdofflegend（'觀測','估計','狀態(tài)'）xlabel（'仿真次數'）ylabel（'數值'）figureplot（abs（Z-X）,'-og'）。holdonplot（abs（X_gj-X））。holdofflegend（'預測與真實之差'，'估計與真實之差'）xlabel（'仿真次數'）ylabel（'數值'）(2)繪出狀態(tài)預測值和狀態(tài)估計值地曲線圖;4045505560657075808590仿真次數(3)繪出預測誤差協(xié)方差和估計誤差協(xié)方差地曲線圖；4.53.5值3數2.521.51~~預測與真實之差危估計與真實之差30405060708090仿真次數(4)對仿真結果進行分析.預測值和估計值都能夠在一定程度上反應真實值，但是估計值比觀測值更接近真實值狀態(tài)估計值：X(kIk)=X(kIk-1)+K(k)lz(k)-H(k)x(kIk-1)]表明估計值是在預測值地基礎上進行優(yōu)化后得到結果，所以估計值更準確一些.2)矢量情形：x(0)=[1;0.1]，xo=[10;1]，P0=[100,10;10,100]，中(k,k-1)=[1,1;0,1]，Q(k,k-1)=[0.1,0;0,0.1]，H(k)=[1,0;0,1]，R(k)=[0.1,0;0,0.1](1)請利用Matlab軟件進行Kalman濾波估計仿真程序編寫；%mykf.m%producesystemclear。A=[11。01]。P0=[10010。10100]。x0=[1。0.1]。X_0=[10。1]。C=[10。01]。Q=[0.10。00.1]。R=[100。010]。%realstatesandmeasurestatesfork=1:150W(:,k)=sqrt(Q)*randn(2,1)。V(:,k)=sqrt(R)*randn(2,1)。ifk==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%predictstatesandestimatestatesfork=1:150ifk==1X_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))。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))。elseX_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(:，:,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%createfigurefiguret=1:150。plot(t,X(1,t),'-or')holdonplot(t,X_gj(1,t),'g')plot(t,Z(1,t),'--')holdofflegend（'分量一狀態(tài)','分量一估計','分量一預測'）xlabel（'仿真次數'）ylabel（'數值'）figureplot(t,X(2,t),'-or',t,X_gj(2,t),'g')holdonplot(t,Z(2,t),'--')holdofflegend('分量二狀態(tài)','分量二估計','分量二預測')xlabel('仿真次數')ylabel('數值')figureplot(t,abs(Z(2,t)-X(2,t)),'-or')holdonplot(t,abs(X_gj(2,t)-X(2,t)),'g')holdofflegend('預測與真實之差'，'估計與真實之差')xlabel('仿真次數')ylabel('數值')figureplot(t,T_gj(t),'g',t,T_yc(t),'-or')legend('估計','預測')xlabel('仿真次數')ylabel('數值')繪出狀態(tài)預測值和狀態(tài)估計值地曲線圖(每個狀態(tài)包括兩個分量)；圖1.1分量一狀態(tài)分量一估計分量一預測151050-5-10-15-20-25分量一狀態(tài)分量一估計分量一預測151050-5-10-15-20-2510分量二狀態(tài)分量二估計分量二預測1101201305101520253010分量二狀態(tài)分量二估計分量二預測110120130仿真次數圖2.160708090100仿真次數

繪出預測誤差協(xié)方差陣跡(Trace)和估計誤差協(xié)方差陣跡地曲線圖;圖3.112010080值數60402010205060估計」預測仿真0次數40對仿真結果進行分析.12010080值數60402010205060估計」預測分量地估計值比分量地觀測值更接近真實值.整個時也是估計值更準確.針對矢量情形，自行選取三組不同地參數進行Kalman濾波地仿真，并進行相應仿真結果地比較分析.改變Q變大(Q=4)醇Y（草鯽099*017能0￡9COS9L0LS賒甌二喜好一K判二喜好翠姓二喜好-11醇Y（草鯽仿真次數改變R變?。≧=4）125120115110105100859095100105110115120125130仿真次數估計

預測43210-1-2-32030405060708090估計

預測仿真次數40353025值數201510581012141618仿真次數改變H變小（H=0.99）醇Y（草鯽001-06080Z09090*9-*-賒甌二喜好——:O■K判二喜好十||■翠姓二喜好——j]]1]]10L藤醇Y（草鯽9*017S￡0￡9COS9L賒甌一喜好一K判一喜好翠姓一喜好-1-|0L9C-02-g-o當R地值變小時，預測值地陣跡會變得下墜更快，預測值本身地震蕩會減小，對真實值地偏離會變小.當Q地值增大時，估計值也會更加偏離真實值.當H變小時，預測值與真實值偏差變大，估計值與真實值地偏差也會變大.)基于線性Kalman濾波信息融合算法考慮如下一類多傳感器線性動態(tài)估計系統(tǒng)x(k)=魚(k,k-1)x(k-1)+w(k,k-1)(10)z.(k)=H(k)x(k)+v.(k)，i=1,2,,N(11)其中，k>0是離散地時間變量，N為傳感器地數目；X(k-1)GRnx1是系統(tǒng)地狀態(tài)向量，G(k,k-1)GRnxn是系統(tǒng)地狀態(tài)轉移矩陣；z.(k)GRp’xl是狀態(tài)x(k)地觀測向量，H(k)GRpxn是相應地觀測矩陣；w(k,k-1)和v(k)是零均值地高斯白噪聲過程，且滿i足如下條件：‘勺&(k,k-1)}=0]EW(k,k-1)w(j,j-1)r/=Q(k,k-1)5<砂.(k)}=0，k,j〉0(12)E\(k)v(j)t/=R(k)5i.k,jEW(k,k-1)v(j)t/=0i初始狀態(tài)x(0)為一隨機向量，且滿足

‘E‘E§(0)}=x

E1x(0)-x°]X(0)-xIXp那么，對于每一個傳感器觀測zj(k)均可執(zhí)行一)當中基于單個觀測地Kalman濾波估計，可得到N個局部估計x,(kIk)和相應地估計誤差協(xié)方差矩陣P(kIk)(i=1,2,,N).從而，可利用分布式加權融合技術將上述N個局部Kalman濾波估計進行融合，即：x(kIk)=￡P(kIk)P-1(kIk)x.(kIk)(14)i=1(14)p-i(kIk)=1Lp-i(kIk)ii=1此時，x(kIk)和P(kIk)為融合后地狀態(tài)估計和相應地融合估計誤差協(xié)方差矩陣.問題：給定相應參數(也鼓勵采用其他參數)，進行上述分布式融合算法地仿真給定如下參數(i=1,2；N=2):x(0)=[1;0.1]，x0=[10;1]，P0=[100,10;10,100]①(k,k-1)=[1,1;0,1]，Q(k,k-1)=[0.1,0;0,0.1]，H.(k)=[1,0;0,1]，R.(k)=[0.1,0;0,0.1]請利用Matlab軟件進行分布式融合估計算法仿真程序編寫；%mykf.m%producesystemclear。clcoA=[11o01]oP0=[10010o10100]oX0=[1o0.1]oX_0=[10o1]oC=[10o01]oQ=[0.10o00.1]oR=[40o04]o%guanceqi1realstatesandmeasurestatesfork=1:150W(:,k)=sqrt(Q)*randn(2,1)oV1(:,k)=sqrt(R)*randn(2,1)oifk==1X1(:,k)=A*x0+W(:,k)oZ1(:,k)=C*X1(:,k)+V1(:,k)oelseX1(:,k)=A*X1(:,k-1)+W(:,k)。Z1(:,k)=C*X1(:,k)+V1(:,k)。endend%predictstatesandestimatestatesfork=1:150ifk==1X_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(:,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))。elseX_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'+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%guanceqi2realstatesandmeasurestatesfork=1:150V2(:,k)=sqrt(R)*randn(2,1)。ifk==1X2(:,k)=A*x0+W(:,k)。Z2(:,k)=C*X2(:,k)+V2(:,k)。elseX2(:,k)=A*X2(:,k-1)+W(:,k)。Z2(:,k)=C*X2(:,k)+V2(:,k)。endend%predictstatesandestimatestatesfork=1:150ifk==1X_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_yc2(:,k))。P_gj2(:,:,k)=(eye(2)-K2(:,:,k)*C)*P_yc2(:,:,k)。T_gj2(k)=trace(P_gj2(:,:,k))。elseX_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(:,:,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%ronghefork=1:150P_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(:，:,k)*inv(P_gj1(:,:,k))*X_gj1(:,k)。end%createfigurefiguret=1:150。plot(t,X2(1,t),'r比Z2(1,t),'g')holdonplot(t,X_gj2(1,t),'-ok',t,X_rhgj(1,t),'--')holdofflegend('觀測器2狀態(tài)一二預測分量一','估計分量一),','融合分量一')xlab