隨機系統(tǒng)濾波與控制大作業(yè)

1、 隨機系統(tǒng)濾波與控制大作業(yè)姓名：史貝貝學號：3107040016班級：碩736專業(yè)：檢測技術(shù)與自動化裝置時間s1234567高度km1.99451.97941.95541.92141.87771.82501.7598時間s891011121314高度km1.68671.60361.50921.40761.29441.17241.0399時間s151617181920高度km0.89800.74550.58500.41250.23180.03991.已知一物體作自由落體運動，對其高度進行了20次測量，測量值如下表：設(shè)高度的測量誤差是均值為0、方差為1的高斯白噪聲隨機序列，試求該物體高度和速度隨時

2、間變化的最優(yōu)估計。(g=9.80m/s2)解：(1).理論分析設(shè)t時刻的位移、速度、加速度和加加速度分別為h,v,a,j。對運動物體的跟蹤kkkkk者來說丿；是隨機量此處取為白噪聲。由運動學方程可得：aT2 HYPERLINK l bookmark2 o Current Document =h+vT+k-1k-12v=v+aTkk-1n-1a=a+jTkk-1k-1觀測方程二h+Nkk其中T=1s,EN=0,DN,Nt=6,Ej=0,Dj,jT=8。（題目中所給高度kklklkklkl數(shù)據(jù)單位為Km,應(yīng)先將其化為單位m）.（h）kvka丿k取狀態(tài)向量xk二則狀態(tài)方程為x二兀+口kk-1k-1觀

3、測方程為z=h+N=Hx+Nkkkkkk廠1tT2/2、0、其中二01T,r=01JTvT丿可見，這是隨機線性定常系統(tǒng)的濾波問題。應(yīng)用Kalman基本濾波方程式，有,H=（100）。kx二x+KzHxkk1kkk1P二PT+FTtk,k1k1P二IKHPkkk,k1K二PHtkk1000、1443.5初始條件的選取：P二0100,x(0)二10、001,、9.8,x(0)中速度是運動學方程得出。如果系統(tǒng)是穩(wěn)定的，那么初始條件的選取對結(jié)果的影響很小。從以后的實驗結(jié)果可以看出。濾波方差最終穩(wěn)定在某一數(shù)值上，系統(tǒng)是穩(wěn)定的。(2).計算結(jié)果與結(jié)果分析1).計算得物體高度隨時間變化的最優(yōu)估計可見用卡爾曼

4、濾波法得到的高度濾波值在初始時與理想值有一定誤差，在遞推步數(shù)k增大時，誤差值越來越小2).計算得物體速度隨時間變化的最優(yōu)估計可見用卡爾曼濾波法得到的速度濾波值在初始時與理想值有一定誤差，在遞推步數(shù)k增大時，誤差值越來越小3).高度協(xié)方差可見用卡爾曼濾波法得到的高度協(xié)方差在初始時較大，為100,在遞推步數(shù)kv3時迅速減小，在k3時不超過2。4).速度協(xié)方差可見用卡爾曼濾波法得到的速度協(xié)方差在初始時較大為2,在遞推步數(shù)kv5時迅速減小,在k5時，不超過0.2，k10時接近于0。(3).源程序%卡爾曼濾波算法在估計自由落體軌跡中的應(yīng)用%輸入觀測值，此處對高度H做出觀測Z=1994.51979.419

5、55.41921.41877.71825.01759.81686.71603.61509.21407.61294.41172.41039.9898.0745.5585.0412.5231.839.9;%初始狀態(tài)向量X=1993.5;-10;-9.8;%初始方差矩陣D=1000;0100;001;%觀測間隔T=1;%狀態(tài)方程系數(shù)矩陣fea=lTTA2/2;01T;001;tao=0;0;T;H=100;%噪聲方差r=1;%觀測噪聲q=1;%過程噪聲%調(diào)用子函數(shù)KALMANtempX，varH，varV，varA=KALMAN1(Z，X，D，fea，tao，H，r，q);%處理結(jié)果n=length

6、(Z);i=1:n;figure(1)plot(i，Z，i，tempX(1，:)，r*-)title(對高度進行濾波)legend(濾波前,濾波后)figure(2)plot(i,tempX(2,:)title(速度估計)figure(3)plot(i,tempX(3,:)title(加速度估計)figure(4)plot(i,varH,i,varV,r+-,i,varA,-.)title(高度、速度、加速度方差估計)legend(高度,速度,加速度)%子程序%卡爾曼濾波算法functiontempX,varH,varV,varA=KALMAN1(Z,X,D,fea,tao,H,r,q)%子程

7、序返回值：3個狀態(tài)變量中間值，3個狀態(tài)變量濾波方差變化n=length(Z);%計算觀測值長度I=eye(3);%下面定義的變量用于存儲計算過程濾波方差變化varH=zeros(1,n);%高度方差varV=zeros(1,n);%速度方差varA=zeros(1,n);%加速度方差%變量tempX用于存儲狀態(tài)變化tempX=zeros(3,n);forj=1:n;tempX(:,j)=X;varH(1,j)=D(1,1);varV(1,j)=D(2,2);varA(1,j)=D(3,3);tempD=fea*D*fea+q*tao*tao;K=tempD*H*inv(H*tempD*H+r)

8、;X=fea*X+K*(Z(1,j)-H*fea*X);D=(I-K*H)*tempD;end2.同樣考慮自由落體運動的物體，用雷達（和物體落地點在同一水平面）進行測量，如圖所示。如果雷達測距和測角的測量噪聲是高斯白噪聲隨機序列，均值為零、方差陣0.040R=0001,試根據(jù)下列測量數(shù)據(jù)確定物體的高度和速度隨時間變化的估計值。時間s*10000.000500000000000.001000000000000.001500000000000.002000000000000.002500000000000.003000000000000.003500000000000.00400000000000

9、0.004500000000000.005000000000000.005500000000000.006000000000000.006500000000000.007000000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.012500000000000.013000000000000.0135000000

10、00000.014000000000000.014500000000000.015000000000000.015500000000000.016000000000000.016500000000000.017000000000000.017500000000000.018000000000000.018500000000000.01900000000000斜距km2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642734750392.77

11、3203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.31790939837137

12、2.285976166564532.254184316814012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.01290326863579俯仰角rad*10000.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800724818190

13、.000750955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000765737677060.000549557590810.000584879139710.000556027473680.000335504125880.000560126884520.000566944919780.000593806310250.000536819165440.00065871960

14、7810.000685983443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000375450331420.000172823192620.000207583279800.000371864645790.000180821634650.000233238301600.019500000000002.00463157388395-0.00004536186964題2示意圖dv(t)二dv(t)二gdh(t)=v(t)dtdt000r0即:加

15、dt001X(t)+0000_g_0001解：(1).理論分析d應(yīng)該選取高度h(t)，速度v(t),d為狀態(tài)變量x(t)，即:X(t)二h(t)v(t)根據(jù)自由落體系統(tǒng)的機理，選取向上為正方向，則有:根據(jù)連續(xù)時間描述離散化的相關(guān)知識，F(xiàn)=00-10000C=0。則有:_g_)TOC o 1-5 h z100Q(t,t)=I+F(tt)=01(tt001100故：0(k+1,k)=01T001屮(k+1,k)=”r(k+1),tc(t)dT二故系統(tǒng)的狀態(tài)方程為：100_0_X(k+1)二01TX(k)+0.5gt2001_gt_kT又因為給定的量測值是斜距，俯仰角，故系統(tǒng)的量測方程為：一0.5g

16、t2gtY(k+1)二L(k+1)9(k+1)、h2(k+1)+d2h(k+1)arctand+V(k+1)輔助方程：H(k+1)XX(；+n+1|dh(k+1Ik)2：h(k+1Ik)+d2；a2h(k+1Ik)+d2-h(k+1Ik)dA2h(k+1Ik)+d2A2h(k+1Ik)+d2A00AX(k+1)=X(k+1lk)則系統(tǒng)量測方程變?yōu)椋篩(k+1)二H(k+1)X(k+1)+V(k+1)0.04題目要求給出該物體高度和速度隨時間變化的的最優(yōu)估計值，并且給定了斜距，俯仰角測量值，故可以用卡爾曼濾波解決問題。0.0419955A濾波初值：X(0I0)二2005P(0Ig512T=0.5

17、R(k)二Q(k)=OY(k)二0.01AA一步預(yù)測：f(k+11k)=Q(k+1,k)f(kIk)+U(k)P(k+1Ik)=e(k+1,k)P(kIk)Qt(k+1,k)濾波增益：K(k+1)二P(k+1Ik)Ht(k+1)H(k+1)P(k+1Ik)Ht(k+1)+R(k+1)-iAAA濾波計算：X(k+1Ik+1)=X(k+1Ik)+K(k+1)Y(k+1)h(X(k+1Ik),k+1)P(k+1Ik+1)二I一K(k+1)H(k+1)P(k+1Ik)(2).計算結(jié)果與結(jié)果分析1).高度濾波值2).速度濾波值可見用擴展卡爾曼濾波法得到的速度濾波值與理想值在初始幾步有一些誤差，再k5時誤

18、差很小。3).高度協(xié)方差4).速度協(xié)方差用擴展卡爾曼濾波法得到的速度協(xié)方差初始值為2,在kv5時以指數(shù)形式迅速減小，在k5時不超過0.1。(3).源程序%卡爾曼濾波算法在估計自由落體軌跡中的應(yīng)用%直接測量值為斜距和俯仰角，要求對高度H和速度V做出預(yù)測formatlong;%為斜距，t為俯仰角r=2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642734750392.773203650263132.759195354645512.743312

19、886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.254184316814012.22

20、2593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.012903268635792.004631573883952.00058143251913*1000;t=0.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800724818190.000750

21、955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000765737677060.000549557590810.000584879139710.000556027473680.000335504125880.000560126884520.000566944919780.000593806310250.000536819165440.000658719607810.00

22、0685983443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000375450331420.000172823192620.000207583279800.000371864645790.000180821634650.000233238301600.000045361869640.00003246284068*1000;%人為高度，dO為水平距離；兩者作為觀測值使用n=length(r);h=;dO=;fori=1:nh(i)=r(i)*sin(t(i);dO(i)=r(i)*cos(t(i);endZ=dO;h;%初始狀態(tài)向量X=2O16.O;2OOO.O;-1;%初始方差矩陣D=5OO;O5O;OO1O;%觀測間隔T=O.5;%狀態(tài)方程系數(shù)矩陣fea=1OO;O1T;OO1;H=1OO;O1O;%調(diào)用子函數(shù)KALMANtempX,vardO,varH,varV=KALMAN1(Z,r,t,X,D,fea,H);%處理結(jié)果n=length(r);i=1:n;figure(1)plot(i,tempX(1,:)title(水平