1、精選優(yōu)質(zhì)文檔-傾情為你奉上1、已知一物體作自由落體運動，對其高度進行了20次測量，測量值如下表： 時間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.0399設(shè)高度的測量誤差是均值為0、方差為1的高斯白噪聲隨機序列，該物體的初始高度和速度也是高斯分布的隨機變量，且。試求該物體高度和速度隨時間變化的最優(yōu)估計。（）解：

2、選取系統(tǒng)的狀態(tài)變量為，為物體在k時刻的高度，為物體在k時刻的瞬時速度。這里不考慮過程噪聲的存在。由牛頓第二運動定律可以寫出系統(tǒng)的狀態(tài)方程：（由采樣周T=1s，根據(jù)離散化的方法得離散的狀態(tài)方程） 其中 ，建立如下的測量方程：其中。應(yīng)用卡爾曼濾波算法可以得到物體的高度和速度隨時間變化的最優(yōu)估計，卡爾曼濾波算法為：一步預(yù)測：預(yù)測方差：濾波增益：濾波計算：P濾波方差：由題設(shè)，濾波初始值為：= 測量誤差為：EV（k+1）=0 , Var V(k+1)=1采用MATLAB軟件進行計算，程序清單為：A=1 -1;0 1;B=-1/2;1;C=1 0;U=9.80; R=1;h1=1994.5 1979.4

3、1955.4 1921.4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.4 1172.4 1039.9 898 745.5 585 412.5 231.8 39.9;x=1900 10'p=100 0;0 2;t=1:20;he=zeros(1,length(t);for i=1:20 x=A*x+B*U; %一步預(yù)測 p=A*p*A' %一步預(yù)測誤差方差矩陣 k=p*C'*inv(R+C*p*C'); %濾波增益 x=x+k*(h1(i)-C*x); %濾波值 h2(i)=x(1,:); %保存高

4、度濾波值 v(i)=x(2,:); %保存速度濾波值 p=(eye(2)-k*C)*p; %濾波方差 P1(i)=p(1,1) P2(i)=p(2,2)endfigure(1),plot(t,h1','r',t,h2','*');legend('濾波曲線','測量曲線')title('濾波曲線和測量曲線')figure(2),plot(t,v');legend('速度曲線')title('速度曲線')figure(3) plot(P1,'r')

5、;legend('高度方差')title('高度方差')figure(4) plot(P2,'r')legend('速度方差')title('速度方差')運行程序，得到的物體高度和速度隨時間變化的最優(yōu)估計如下表所示：時間s測量高度m高度的最優(yōu)估計m速度的最優(yōu)估計m/s11994.51993.417.67621979.41977.323.23831955.41953.730.5541921.41920.339.551877.7187748.96461825.01824.158.571759.81760.368.391

6、81686.71686.978.21491603.61603.788.024101509.21510.297.909111407.61407.5107.7121294.41294.7117.52131172.41172.3127.32141039.91040.1137.1215898.0898.03146.9216745.5746.05156.7417585.0584.54166.5318412.5412.98176.3419231.8231.76186.142039.940.566195.95上表為物體隨時間變化的高度量測值和高度，速度的最優(yōu)估計值按此計算結(jié)果繪制的曲線圖如下：結(jié)果分析：此題

7、中我們利用初值和P(0)根據(jù)卡爾曼濾波算法遞推的求出了物體的高度和速度的最優(yōu)估計，從物體的高度的最優(yōu)估計曲線中我們可以看到，物體的高度最優(yōu)估計值和實際測量值幾乎相等，物體的速度的最優(yōu)估計也和自由落體的速度幾乎一致，只是在初始濾波時有一定的的偏差，這是由于濾波初值選取決定的。在許多實際問題中，往往不能精確知道初值。很顯然此時計算得到的濾波值將不是最優(yōu)的。根據(jù)相應(yīng)的判定準則，我們知道該系統(tǒng)是一致完全能和一直完全能觀的。即它的最優(yōu)線性最優(yōu)濾波系統(tǒng)是一直漸近穩(wěn)定的，所以濾波初值選取只影響濾波的初始階段，系統(tǒng)的濾波最終會趨向穩(wěn)定。這是有系統(tǒng)自身的特性決定的。因此初值任意選取并不會影響濾波的最后階段的結(jié)果

8、。顯然，上述結(jié)果是在測量誤差矩陣R精確可知的情況下得到的。如果R不精確，那么由上述算法給出的將不是X(k)的最優(yōu)估計。并且此時，濾波還可能發(fā)散。這時我們可以采用噪聲不精確（未知）系統(tǒng)的自適應(yīng)濾波：可以先選取一個適當?shù)牧繙y噪聲矩陣并將它固定下來，然后按照動態(tài)噪音不精確的情況設(shè)計自適應(yīng)濾波。本題中假設(shè)測量誤差為均值為0，方差為1的高斯白色噪聲隨機序列，并且初始高度和速度相互獨立。顯然這種假設(shè)是比較理想的狀況。如果測量誤差為有色噪聲，此時我們可以通過適當?shù)淖儞Q把系統(tǒng)方程中的有色噪聲轉(zhuǎn)變成白色噪聲的情形處理，從而得到一個白色噪聲作用下的離散線性系統(tǒng)，有此系統(tǒng)的濾波算法導(dǎo)出原系統(tǒng)的濾波算法。由上圖可以看

9、出，速度的濾波方差在最初的時間內(nèi)很大（接近于2），高度濾波方差在初始時更大（接近于100），但是很快速度濾波方差就收斂到0；而高度的濾波方差收斂速度比速度濾波方差的要慢，但是最終也收斂到了某個穩(wěn)態(tài)值。改變參數(shù)看影響：將濾波初值、初值方差、測量噪聲方差分別變化，其相應(yīng)濾波方差如圖所示：綜上所示，濾波初值的改變對濾波方差影響不大（其實這一點在卡爾曼濾波算法中也可看出），而改變初值方差、測量噪聲方差對濾波方差有較大的影響。初值方差和測量噪聲方差的增加都會使高度方差和速度方差變大，且收斂速度變慢；相應(yīng)地，他們的減少也都會使高度方差和速度方差變小，收斂速度變快。從統(tǒng)計學(xué)的角度講，方差代表數(shù)據(jù)的精度，方差

10、大則數(shù)據(jù)偏離真值副度就大，數(shù)據(jù)本身的可靠性就差，數(shù)據(jù)所帶來的信息就小，所帶來的誤差就大（根據(jù)誤差理論的誤差可傳遞性）。2、同樣考慮自由落體運動的物體，用雷達（和物體落地點在同一水平面）進行測量，如圖所示。如果，且雷達測距和測角的測量噪聲是高斯白噪聲隨機序列，均值為零、方差陣，試根據(jù)下列測量數(shù)據(jù)確定物體的高度和速度隨時間變化的估計值。時間s*1000斜距km俯仰角rad*1000 0.000 2.891 0.876 0.000 2.771 0.478 0.000 2.236 0.639 0.000 2.901 0.802 0.000 2.244 0.452 0.000 2.089 0.819 0

11、.000 2.039 0.213 0.000 2.313 0.379 0.000 2.551 0.545 0.000 2.195 0.034 0.000 2.812 0.986 0.000 2.312 0.530 0.000 2.473 0.495 0.000 2.220 0.639 0.000 2.333 0.069 0.000 2.357 0.706 0.000 2.785 0.081 0.000 2.843 0.971 0.000 2.797 0.368 0.000 2.246 0.588 0.000 2.386 0.452 0.000 2.982 0.978 0.000 2.088 0

12、.025 0.000 2.696 0.544 0.000 2.332 0.781 0.000 2.137 0.328 0.000 2.453 0.348 0.000 2.401 0.918 0.000 2.535 0.708 0.000 2.466 0.281 0.000 2.271 0.972 0.000 2.706 0.986 0.000 2.727 0.142 0.000 2.195 0.262 0.000 2.767 0.980 0.000 2.031 0.579 0.000 2.357 0.465 0.000 2.579 0.160 0.000 2.395 -0.964 0.000

13、2.913 0.068雷達物體Vhd0斜距俯仰角圖2示意圖解：選取系統(tǒng)的狀態(tài)變量為，其中為雷達距離目標的水平距離，在物體的自由下落過程中可以認為是常值，為物體在k時刻的高度，為物體在k時刻的速度。這里不考慮過程噪聲的存在。由牛頓第二運動定律可以建立系統(tǒng)的狀態(tài)方程為：（由采樣周T=0.5s，根據(jù)離散化的方法得離散的狀態(tài)方程） 其中 ， ，測量方程如下：設(shè)hX(k),k=H(k)= 應(yīng)用擴展卡爾曼濾波算法可以得到物體的高度和速度隨時間變化的最優(yōu)估計，擴展卡爾曼濾波算法為：一步預(yù)測：預(yù)測方差：濾波增益：濾波計算：濾波方差：由題設(shè)，濾波初始值為： 測量誤差為：EV（k+1）=0 , 方差采用MATLA

14、B軟件進行計算，程序清單為：A=1 0 0;0 1 -0.5;0 0 1;B=0;-0.125;0.5;U=9.80; R=0.04 0;0 0.01;C=1000* 2.891 0.876; 2.771 0.478; 2.236 0.639; 2.901 0.802; 2.244 0.452; 2.089 0.819; 2.039 0.213; 2.313 0.379; 2.551 0.545; 2.195 0.034; 2.812 0.986; 2.312 0.530; 2.473 0.495; 2.220 0.639; 2.333 0.069; 2.357 0.706; 2.785 0.

15、081; 2.843 0.971; 2.797 0.368; 2.246 0.588; 2.386 0.452; 2.982 0.978; 2.088 0.025; 2.696 0.544; 2.332 0.781; 2.137 0.328; 2.453 0.348; 2.401 0.918; 2.535 0.708; 2.466 0.281; 2.271 0.972; 2.706 0.986; 2.727 0.142; 2.195 0.262; 2.767 0.980; 2.031 0.579; 2.357 0.465; 2.579 0.160; 2.395 -0.964; 2.913 0.

16、068; %輸入測量數(shù)據(jù)x=1995 2005 1'p=5 0 0;0 5 0;0 0 2;t=0.5:0.5:20;h=zeros(1,length(t);v=zeros(1,length(t);for i=1:length(t)hh(i)=C(i,1)*sin(C(i,2);endfor i=1:length(t) x=A*x+B*U; %一步預(yù)測 p=A*p*A' %一步預(yù)測誤差方差矩陣H=x(1,1)/sqrt(x(1,1)2+x(2,1)2),x(2,1)/sqrt(x(1,1)2+x(2,1)2),0; -x(2,1)/(x(1,1)2+x(2,1)2),x(1,1

17、)/(x(1,1)2+x(2,1)2),0 k=p*H'*inv(R+H*p*H'); %濾波增益 x=x+k*(C(i,:)'-sqrt(x(1,1)2+x(2,1)2);atan(x(2,1)/x(1,1); %濾波值 h(i)=x(2,:); %保存高度濾波值 v(i)=x(3,:); %保存速度濾波值 p=(eye(3)-k*H)*p; %濾波方差 p1(i)=p(1,1); p2(i)=p(2,2); p3(i)=p(3,3);endfigure(1)plot(t,h,'r');title('濾波曲線')figure(2),pl

18、ot(t,hh, 'b');title('高度測量曲線')figure(3),plot(t,v');title('速度濾波曲線')figure(4),plot(t,p2');title('高度方差曲線')figure(5),plot(t,p3');title('速度方差曲線')disp(h)disp(v)運行程序，得到的物體高度和速度隨時間變化的最優(yōu)估計如下表所示：時間s高度的最優(yōu)估計km速度的最優(yōu)估計m/s0.0002.00345.87350.0002.00059.23080.0001.9

19、94214.53130.0001.985819.31810.0001.974524.46420.0001.960929.44280.0001.945234.20750.0001.926739.20850.0001.905944.07740.0001.882748.94110.0001.857053.86490.0001.828858.74180.0001.798063.60800.0001.765168.51780.0001.729473.40170.0001.691278.29440.0001.650583.18990.0001.607988.09120.0001.562592.99180.

20、0001.513497.90460.0001.4639102.79650.0001.4103107.71270.0001.3549112.61690.0001.2965117.53550.0001.2365122.43640.0001.1740127.33630.0001.1086132.24990.0001.0407137.16490.0000.9709142.06370.0000.8981146.98170.0000.8228151.89680.0000.7458156.79270.0000.6660161.69810.0000.5843166.58550.0000.4996171.49050.0000.4125176.39560