已知一物體作自由落體運(yùn)動(dòng)對(duì)其高度進(jìn)行了20次測(cè)量

1、1已知一物體作自由落體運(yùn)動(dòng)，對(duì)其高度進(jìn)行了20次測(cè)量，測(cè)量值如下表:時(shí)間s1234567高度km1.99451.97941.95541.92141.87771.82501.7598時(shí)間s891011121314高度km1.68671.60361.50921.40761.29441.17241.0399時(shí)間s151617181920高度km0.89800.74550.58500.41250.23180.0399設(shè)高度的測(cè)量誤差是均值為 0、方差為1的高斯白噪聲隨機(jī)序列， 該物體的初始高度 h0和速度 V也是高斯分布的隨機(jī)變量，且Eh。= |1900mP0 = vaqho= 100 0 l試EV。

2、l0m/s0V。一 o 2一求該物體高度和速度隨時(shí)間變化的最優(yōu)估計(jì)。(g = 9.80m/s2)解：選取系統(tǒng)的狀態(tài)變量為 hv vk T， hk為物體在k時(shí)刻的高度，vk為物體在k時(shí)刻的瞬時(shí)速度。這里不考慮過程噪聲的存在。(由采樣周T=1s,根據(jù)離散化的方法得由牛頓第二運(yùn)動(dòng)定律可以寫出系統(tǒng)的狀態(tài)方程: 離散的狀態(tài)方程)X(k 1其中>(k 1,k)=v(k 1,k)-0.51N1 J建立如下的測(cè)量方程:ihk 1丫 (k+1) = 1 0|+Vk 卅W一 其中 H(k 1) = 1 0 1。應(yīng)用卡爾曼濾波算法可以得到物體的高度和速度隨時(shí)間變化的最優(yōu)估計(jì)，卡爾曼濾波算法 為：一步預(yù)測(cè):Xk

3、 + 1k)=(k + 1,k)X"(k k) + 甲(k + 1,k)U(k)預(yù)測(cè)方差:P(k +1k) =6(k+1,k)P(kk) T(k+1, k)濾波增益:K(k +1) =P(k +1k)HT(k +1)H(k+1)P(k +1k)H T(k+1) + R(k + 1)濾波計(jì)算：P)?(k 1k 1 )?(k 1k) K(k 1)Y(k 1) - H (k 1)>?(k 1k) 濾波方差：P(k 1k 1) =l -K(k 1)H(k 1)P(k 1k)由題設(shè)，濾波初始值為：AX(0)=Eh0_ 1900mEV0 _ 10m/sP0 =Varh0100 0bHo 2

4、測(cè)量誤差為：EV( k+1)=0, Var V(k+1)=1采用MATLAB軟件進(jìn)行計(jì)算，程序清單為:A=1 -1;0 1;B=-1/2;1;C=1 0;U=9.80; R=1;h仁1994.5 1979.4 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,le ngth(t);for i=1:20%步預(yù)測(cè)%一步預(yù)測(cè)誤差方差矩陣

5、%濾波增益%濾波值%保存高度濾波值%保存速度濾波值%濾波方差x=A*x+B*U;p=A*p*A： k=p*C'*i nv(R+C*p*C'); x=x+k*(h1(i)-C*x);h2(i)=x(1,:);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('濾波曲線','測(cè)量曲線') title('濾波曲線和測(cè)量曲線') figure

6、 (2),plot(t,v');lege nd(速度曲線') title('速度曲線') figure(3)plot(P1,'r');lege nd('高度方差') title('高度方差') figure(4)plot(P2,'r')lege nd(速度方差') title('速度方差')運(yùn)行程序，得到的物體高度和速度隨時(shí)間變化的最優(yōu)估計(jì)如下表所示:時(shí)間s測(cè)量高度m高度的最優(yōu)估計(jì)m速度的最優(yōu)估計(jì)m/s11994.51993.417.67621979.41977.323.23

7、831955.41953.730.5541921.41920.339.551877.7187748.96461825.01824.158.571759.81760.368.39181686.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.

8、3419231.8231.76186.142039.940.566195.95上表為物體隨時(shí)間變化的高度量測(cè)值和高度，速度的最優(yōu)估計(jì)值 按此計(jì)算結(jié)果繪制的曲線圖如下：濾液曲統(tǒng)和測(cè)量曲錢涯液曲蝮和測(cè)雖曲議20001* *7 I1111l一II_ |入*濾波柑找1B0D -、*初薑曲線|一I1EOD -*-1JQD -+-X1200 - * _1D0D -丸-90060D -羊-40D -200 -I - Ik0 IIIiiillII"FD 2 J B B 1012 U 161020結(jié)果分析：A此題中我們利用初值 X (0)和P(0)根據(jù)卡爾曼濾波算法遞推的求出了物體的高度和速 度的最優(yōu)

9、估計(jì)，從物體的高度的最優(yōu)估計(jì)曲線中我們可以看到，物體的高度最優(yōu)估計(jì)值和實(shí)際測(cè)量值幾乎相等，物體的速度的最優(yōu)估計(jì)也和自由落體的速度幾乎一致，只是在初始濾波時(shí)有一定的的偏差，這是由于濾波初值選取決定的。在許多實(shí)際問題中，往往不能精確知道 初值。很顯然此時(shí)計(jì)算得到的濾波值將不是最優(yōu)的。根據(jù)相應(yīng)的判定準(zhǔn)則，我們知道該系統(tǒng)是一致完全能和一直完全能觀的。即它的最優(yōu)線性最優(yōu)濾波系統(tǒng)是一直漸近穩(wěn)定的，所以濾波初值選取只影響濾波的初始階段，系統(tǒng)的濾波最終會(huì)趨向穩(wěn)定。這是有系統(tǒng)自身的特性決定的。因此初值任意選取并不會(huì)影響濾波的最后階段的結(jié)果。顯然，上述結(jié)果是在測(cè)量誤差矩陣R精確可知的情況下得到的。如果R不精確，那

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

11、波方差在初始時(shí)更大（接近于100），但是很快速度濾波方差就收斂到 0；而高度的濾波方差收斂速度 比速度濾波方差的要慢，但是最終也收斂到了某個(gè)穩(wěn)態(tài)值。改變參數(shù)看影響：將濾波初值、初值方差、測(cè)量噪聲方差分別變化，其相應(yīng)濾波方差如圖所示：濾浪初值增加時(shí)高度方差和遠(yuǎn)度方差爾波初值減少時(shí)高度方菱和速度方菱初值方差增加時(shí)高度方差和速度方差初值方差減少時(shí)高度方差和速度方差測(cè)宗噪聲增加B扌高度方差和速度方差測(cè)量噪聲械少時(shí)高度方差和速度方差所示。Ed。刁995mEh。=2005m，P0 = varhEV0 一i1m/s1 1v。一如果50聲是高斯白噪聲隨機(jī)序列，均值為零、方差陣試根據(jù)下列測(cè)量數(shù)據(jù)確0.01綜上所

12、示，濾波初值的改變對(duì)濾波方差影響不大（其實(shí)這一點(diǎn)在卡爾曼濾波算法中也可看出），而改變初值方差、測(cè)量噪聲方差對(duì)濾波方差有較大的影響。初值方差和測(cè)量噪聲方 差的增加都會(huì)使高度方差和速度方差變大，且收斂速度變慢；相應(yīng)地，他們的減少也都會(huì)使高度方差和速度方差變小，收斂速度變快。從統(tǒng)計(jì)學(xué)的角度講，方差代表數(shù)據(jù)的精度， 方差大則數(shù)據(jù)偏離真值副度就大，數(shù)據(jù)本身的可靠性就差，數(shù)據(jù)所帶來的信息就小，所帶來的誤差就大（根據(jù)誤差理論的誤差可傳遞性）2、同樣考慮自由落體運(yùn)動(dòng)的物體，用雷達(dá)（和物體落地點(diǎn)在同一水平面）進(jìn)行測(cè)量，如圖0 050 ，且雷達(dá)測(cè)距和測(cè)角的測(cè)量噪0 2定物體的高度和速度隨時(shí)間變化的估計(jì)值。時(shí)間s*

13、10000.000500000000000.001000000000000.001500000000000.002000000000000.002500000000000.00300000000000斜距km2.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.79725268974089 俯仰角rad*10000.000758504358760.000832822604780.000678082416390.000852790368020.000729007684520.000800

14、724818190.003500000000000.004000000000000.004500000000000.005000000000000.005500000000000.006000000000000.006500000000000.007000000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.01

15、2500000000000.013000000000000.013500000000000.014000000000000.014500000000000.015000000000000.015500000000000.016000000000000.016500000000000.017000000000000.017500000000000.018000000000000.018500000000000.019000000000000.019500000000000.020000000000002.786642734750392.773203650263132.75919535464551

16、2.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564532.2541843168

17、14012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143572.012903268635792.004631573883952.000581432519130.00075095576213 0.00065762725379 0.00081186148545 0.00079783727034 0.00073060712986 0.00063242006530 0.

18、00063656524495 0.00080659845639 0.00067704740069 0.00076573767706 0.00054955759081 0.00058487913971 0.00055602747368 0.00033550412588 0.00056012688452 0.00056694491978 0.00059380631025 0.00053681916544 0.00065871960781 0.00068598344328 0.00060922471348 0.00057086018918 0.00041308535708 0.00047302026

19、281 0.00030949309972 0.00040552624986 0.00037545033142 0.00017282319262 0.00020758327980 0.00037186464579 0.00018082163465 0.00023323830160 -0.000045361869640.00003246284068物體俯仰角、雷達(dá)do圖2示意圖解：選取系統(tǒng)的狀態(tài)變量為 dkhk VJ，其中dk為雷達(dá)距離目標(biāo)的水平距離，在物體的自由下落過程中可以認(rèn)為是常值,hk為物體在k時(shí)刻的高度，Vk為物體在k時(shí)刻的速度。這里不考慮過程噪聲的存在。由牛頓第二運(yùn)動(dòng)定律可以建立系統(tǒng)的

20、狀態(tài)方程為: 法得離散的狀態(tài)方程）（由采樣周T=0.5s，根據(jù)離散化的方測(cè)量方程如下:設(shè) hX(k),k=X(k 1)二W! _1hk +50 dk_0-0.5-0.1250.5100 1-01-0.5，普(k+1,k) =-0.125衛(wèi)01 _1 10.5 一01，g其中 ：（k 1,k）二曲+dk卅1hk+i、2=9.8m/ sY(k +1)=卜 | h41 卅larcta n()-dk4t _Jd2(k) +h2(k) arctg 型- d(k) 一d(k)Vk1h(k)k）=血決（k）.d2(k) h2(k). d2(k) h2(k)-h(k)d2(k) h2(k)d(k)d2(k)

21、h2(k)00擴(kuò)展卡爾曼濾應(yīng)用擴(kuò)展卡爾曼濾波算法可以得到物體的高度和速度隨時(shí)間變化的最優(yōu)估計(jì)， 波算法為：一步預(yù)測(cè)：X(k 1|k)二G(k 1,k)?(k|k) "k 1,k)U(k) 預(yù)測(cè)方差：P(k 1k)- ：:(k 1,k)P(kk)： J(k 1, k) 濾波增益：K k 1 二 P(k 1|k)HT(k 1)H(k 1)P(k 1k)HT(k 1) R(k 1)J 濾波計(jì)算：Xk +1|k +1) = >?(k +1|k) + K(k +1)(Y(k +1) h(Xk +1|k), k + 1) 濾波方差：P(k 1k 1) =l -K(k 1)H(k 1)P(k

22、 1|k)由題設(shè)，濾波初始值為：a一1995"5 0 0X(0) = 2005 P( 0>0 5 0Ji P 0 2測(cè)量誤差為：0.040 1EV( k+1)=0,方差 R= I0 0.01 一采用MATLAB軟件進(jìn)行計(jì)算，程序清單為: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.827416437818912.825198117297712.820666869662362.814872331059012.806717865362442.797252689740892.786642

23、734750392.773203650263132.759195354645512.743312886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273572.590381098507852.562987942728432.534983179507972.506475893722462.475710750163860.00075850435876;0.00083282260478;0.00067808241639;0.00085279036802;

24、0.00072900768452;0.00080072481819;0.00075095576213;0.00065762725379;0.00081186148545;0.00079783727034;0.00073060712986;0.00063242006530;0.00063656524495;0.00080659845639;0.00067704740069;0.00076573767706;0.00054955759081;0.00058487913971;0.00055602747368;0.00033550412588;0.00056012688452;2.445606760

25、009822.414036907720882.382522286116962.350165011823322.317909398371372.285976166564530.00056694491978;0.00059380631025;0.00053681916544;0.00065871960781;0.00068598344328;0.00060922471348;2.254184316814012.222593202195352.192373989694660.00057086018918;0.00041308535708;0.00047302026281;2.162901779972

26、712.134417257937062.108110646907272.083221798231952.061481090267672.042198850940312.026102353143570.00030949309972;0.00040552624986;0.00037545033142;0.00017282319262;0.00020758327980;0.00037186464579;0.00018082163465;2.012903268635792.004631573883952.000581432519130.00023323830160;-0.00004536186964;

27、0.00003246284068; %輸入測(cè)量數(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); end for i=1:length(t)x=A*x+B*U;%一步預(yù)測(cè)p=A*p*A'% 一步預(yù)測(cè)誤差方差矩陣H=x(1,1)/sqrt(x(1,1)A2+x(2,1)A2),x(2,1)/sqrt(x(1,1)A2+x(2,1)A2),0; -x(2,1)/(x(1,

28、1)A2+x(2,1)A2),x(1,1)/(x(1,1F2+x(2,1)A2),0 k=p*H'*inv(R+H*p*H');% 濾波增益x=x+k*(C(i,:)'-sqrt(x(1,1)A2+x(2,1)A2);ata n(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(' 濾波曲

29、線 ') figure(2),plot(t,hh, 'b');title('高度測(cè)量曲線') figure (3),plot(t,v');title('速度濾波曲線') figure (4) ,plot(t,p2');title('高度方差曲線') figure(5),plot(t,p3');title('速度方差曲線') disp(h)disp(v)運(yùn)行程序，得到的物體高度和速度隨時(shí)間變化的最優(yōu)估計(jì)如下表所示:時(shí)間S高度的最優(yōu)估計(jì)km速度的最優(yōu)估計(jì)m/s0.000500000000

30、002.00345.87350.001000000000002.00059.23080.001500000000001.994214.53130.002000000000001.985819.31810.002500000000001.974524.46420.003000000000001.960929.44280.003500000000001.945234.20750.004000000000001.926739.20850.004500000000001.905944.07740.005000000000001.882748.94110.005500000000001.857053.8

31、6490.006000000000001.828858.74180.006500000000001.798063.60800.007000000000001.765168.51780.007500000000001.729473.40170.008000000000001.691278.29440.008500000000001.650583.18990.009000000000001.607988.09120.009500000000001.562592.99180.010000000000001.513497.90460.010500000000001.4639102.79650.011000000000001.4103107.71270.011500000000001.3549112.61690.012000000000001.2965117.53550.012500000000001.2365122.43640.013000000000001.