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設高度的測量誤差是均值為0、 方差為 1 的高斯白噪聲隨機序列，該物體的初始高度h0和速度V000010 。試求該物體高度2Eh01900m ,P0 var h0EV010m/s 0 V0( g 9.80m/s2)1

2、. 令 X(k) h(k) t=1 R( k) =1 Q(k)=0 v(k)根據離散時間卡爾曼濾波公式，則有：X(k 1) (k 1,k)X(k) U(k)Y(k 1) H(k 1)X(k 1) V(k 1)(k 1,k )1t10.5gt2U (k)gtH (k1 ) 1 0X(0|0) EX(0)190010P(0|0) varX(0)P02001X(k 1|k) (k 1,k)X(k|k) U(k)P(k 1| k) (k 1,k)P(k|k) T(k 1,k)K(k 1) P(k 1|k)HT(k 1)H(k 1)P(k 1|k)HT(k 1) R(k 1) 1X(k 1|k 1) X

3、(k 1| k) K(k 1)Y(k 1) H(k 1)X(k 1| k)P(k 1| k 1) I K(k 1)H(k 1)P(k 1|k)2. 實驗結果高度隨時間變化估計速度隨時間變化的最優估計高度協方差速度協方差從以上的結果，可以得到高度和速度的估計值，再通過所得到的高度協方差和速度協方差，可見用卡爾曼濾波法，雖然剛開始的初始高度協方差很大為100， 但通過 2 步之后減小到不超過1 ，逐漸接近于0，同樣的速度協方差剛開始的時候也比較大，為 2， 但是通過5 步之后迅速減小，到 10 步之后接近于0。3. 有關參數的影響（例如初始條件、噪聲統計特性對濾波結果的影響等）1）初始條件改變時，

4、改變初始高度值，和速度值Eh02300mEV030m/s向反叼刀左100 口80604020 I0 _'，0510152025由實驗結果分析可得度濾波值和速度濾波值在開始幾步接近初始值，協方差值基本不變。h0150 02）當初始協方差值改變時，改為P0 var 00 V 010實驗結果分析高度和速度濾波值基本不變，速度協方差和高度協方差開始要接近速度協方差和高度協方差的初始值。但是經過幾步之后，都趨于0。Ed01995md050示。如果Eh02005m,P0var h005EV01m/sV00000 ，且雷達測距和測角的測量噪聲2是高斯白噪聲隨機序列，均值為零、方差陣物體的高度和速度隨

5、時間變化的估計值。時間 s*10000.000500000000000.001000000000000.00150000000000斜距 km2.827416437818912.825198117297712.82066686966236R 0.040 ，試根據下列測量數據確定00.01俯仰角 rad*10000.000758504358760.000832822604780.000678082416390.002000000000000.002500000000000.003000000000000.003500000000000.004000000000000.00450000000000

6、0.005000000000000.005500000000000.006000000000000.006500000000000.007000000000000.007500000000000.008000000000000.008500000000000.009000000000000.009500000000000.010000000000000.010500000000000.011000000000000.011500000000000.012000000000000.012500000000000.013000000000000.013500000000000.0140000000

7、00000.014500000000000.015000000000000.015500000000000.016000000000000.016500000000000.017000000000000.017500000000000.018000000000000.018500000000000.019000000000000.019500000000000.020000000000002.814872331059012.806717865362442.797252689740892.786642734750392.773203650263132.759195354645512.743312

8、886281952.725388884828122.706649677123122.686324034064732.663865338522202.640935297073332.616211117273570.000765737677062.590381098507852.562987942728432.534983179507972.506475893722462.475710750163862.445606760009822.414036907720882.382522286116960.000536819165442.350165011823320.000658719607812.31

9、7909398371372.285976166564532.254184316814012.222593202195352.192373989694662.162901779972712.134417257937062.108110646907270.000375450331422.083221798231952.061481090267672.042198850940312.026102353143572.00463157388395 -0.000045361869642.000581432519130.000032462840682.012903268635790.000852790368

10、020.000729007684520.000800724818190.000750955762130.000657627253790.000811861485450.000797837270340.000730607129860.000632420065300.000636565244950.000806598456390.000677047400690.000549557590810.000584879139710.000556027473680.000335504125880.000560126884520.000566944919780.000593806310250.00068598

11、3443280.000609224713480.000570860189180.000413085357080.000473020262810.000309493099720.000405526249860.000172823192620.000207583279800.000371864645790.000180821634650.00023323830160解：1.題 2 示意圖d令 X (k) h(k) t=0.5v(k)擴展卡爾曼濾波公式，則有：根據離散時間0. 04R(k)0.01l(k)Q(k)=0 Y(k)(k)0狀態方程：X(k 1) (k 1,k)X(k) U(k) W(k)

12、 U (k)0. 5g2tgt1(k 1,k)1 t1h2(k) d2測量方程：Y(k 1)h(k) V(k)arctan輔助方程：H(k 1) hX(k 1),k 1 |X(k 1) x(k 1) x(k 1|k)h(k 1| k)2h (k 1|k) d2d22h (k 1|k) d2 h (k 1|k) d2 00 h(k 1|k)22h (k 1| k) d2X(k 1|k) (k 1,k)X(k|k) U(k)P(k 1| k) (k 1,k)P(k|k) T(k 1,k)濾波增益：K(k 1) P(k 1|k)HT(k 1)H(k 1)P(k 1|k)HT(k 1) R(k 1)

13、1濾波計算：X(k 1| k 1) X(k 1| k) K(k 1)Y(k 1) h(X(k 1| k), k 1)P(k 1| k 1) I K(k 1)H(k 1)P(k 1|k)19955濾波初值：X (0 | 0)2005P(0|0)5122. 實驗結果高度隨時間變化估計速度隨時間變化的估計高度協方差：速度協方差估計實驗結果分析：根據圖，可得高度和速度的估計值，通過擴展卡爾曼濾波法，高度協方差和速度協方差，剛開始的值比較大，但是迅速減小，在幾步之后逐漸趨近于0。3. 有關參數的影響（例如初始條件、噪聲統計特性對濾波結果的影響等）；19951）初始條件發生變化，改變高度和速度的初始值為X

14、 （0 | 0）23001010步之后，逐漸趨于平穩，高度協方分析：高度和速度在剛開始的時候波動比較大，不過經過差收斂變快，速度協方差基本不變2）初始噪聲改變，R(k) 1 1分析：高度和速度濾波值基本不變，速度協方差也基本不變，高度協方差剛開始的時候有波動，10 步之后趨于穩定。附源程序：第一題t=1;A=1 -t;0 1;g=9.8;U=-0.5*g*t2;g*t;C=1 0;R=1;I=1 0;0 1;X=zeros(2,1);K=zeros(2,1);P=zeros(2,2);P=150 0;0 10;X=1900;10y=1900 1994.5 1979.4 1955.4 1921.

15、4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.41172.4 1039.9 898.0 745.5 585.0 412.5 231.8 399;for i=2:21Pi=A*P*A'% 一步預測Xi=A*X+U;K=Pi*C'*(C*Pi*C'+R)(-1);% 增益X=Xi+K*(y(i)-C*Xi);% 濾波he(i)=X(1,1);ve(i)=X(2,1);P=(I-K*C)*Pi;ph(i)=P(1,1);pv(i)=P(2,2);endhe(1)=1900;ve(1)=10;ph(1)=150;

16、pv(1)=10;figure;t=1:21;plot(t,he(t),'r');title(' 高度濾波值')figure;t=1:21;plot(t,ve(t);title(' 速度濾波值')figure;t=1:21;plot(t,ph(t);title(' 高度協方差')figure;t=1:21;plot(t,pv(t);title(' 速度度協方差')第二題 d=1995;t=0.5;A=1 0 0;0 1 -t;0 0 1;%表示狀態方程中的faig=9.8;U=0;-0.5*g*t2;g*t;%表示

17、狀態方程中的uR=1.04 0;0 1.01;%表示測量噪聲I=1 0 0;0 1 0;0 0 1;P=5 0 0;0 5 0;0 0 2;X=1995;2005;1;% 濾波初值 y=2.8284;2.82741643781891;2.82741643781891;2.82066686966236;2.81487233105901;2.80671786536244;2.79725268974089;2.78664273475039;2.77320365026313;2.75919535464551;2.74331288628195;2.72538888482812;2.70664967712

18、312;2.68632403406473;2.66386533852220;2.64093529707333;2.61621111727357;2.59038109850785;2.56298794272843;2.53498317950797;2.50647589372246;2.47571075016386 ;2.44560676000982;2.41403690772088;2.38252228611696;2.35016501182332;2.31790939837137;2.28597616656453;2.25418431681401;2.22259320219535;2.1923

19、7398969466;2.16290177997271;2.13441725793706;2.10811064690727;2.08322179823195;2.06148109026767;2.04219885094031;2.02610235314357;2.01290326863579;2.00463157388395;2.00058143251913;y=y*1000;yy=0.000787898158189;0.00075850435876;0.00083282260478;0.00067808241639;0.00085279036802;0.00072900768452;0.00

20、080072481819;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;0.00056694491978;0.00059380631025;0.00053681916544;0.00065871960781;0.00068598344328;0.00060922471348;0.00057086018918;0.00041308535708;0.00047302026281;0.00030949309972;0.00040552624986;0.00037545033142;0.00017282319262;0