卡爾曼濾波兩例題含matlab程序

1、一. 已知一物體作自由落體運動，對其高度進(jìn)行了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)估計。（）解： 1. 令 t=1 r（k

2、）=1 q(k)=0 根據(jù)離散時間卡爾曼濾波公式，則有： 濾波初值： 一步預(yù)測： 濾波增益：濾波計算： 2. 實驗結(jié)果高度隨時間變化估計速度隨時間變化的最優(yōu)估計高度協(xié)方差速度協(xié)方差從以上的結(jié)果，可以得到高度和速度的估計值，再通過所得到的高度協(xié)方差和速度協(xié)方差，可見用卡爾曼濾波法，雖然剛開始的初始高度協(xié)方差很大為100，但通過2步之后減小到不超過1，逐漸接近于0， 同樣的速度協(xié)方差剛開始的時候也比較大，為2，但是通過5步之后迅速減小，到10步之后接近于0。3. 有關(guān)參數(shù)的影響（例如初始條件、噪聲統(tǒng)計特性對濾波結(jié)果的影響等）；1）初始條件改變時，改變初始高度值，和速度值 由實驗結(jié)果分析可得度濾波值

3、和速度濾波值在開始幾步接近初始值，協(xié)方差值基本不變。2）當(dāng)初始協(xié)方差值改變時，改為實驗結(jié)果分析高度和速度濾波值基本不變，速度協(xié)方差和高度協(xié)方差開始要接近速度協(xié)方差和高度協(xié)方差的初始值。但是經(jīng)過幾步之后，都趨于0。二 同樣考慮自由落體運動的物體，用雷達(dá)（和物體落地點在同一水平面）進(jìn)行測量，如圖所示。如果，且雷達(dá)測距和測角的測量噪聲是高斯白噪聲隨機序列，均值為零、方差陣，試根據(jù)下列測量數(shù)據(jù)確定物體的高度和速度隨時間變化的估計值。時間s*1000斜距km俯仰角rad*1000 0.00050000000000 2.82741643781891 0.00075850435876 0.001000000

4、00000 2.82519811729771 0.00083282260478 0.00150000000000 2.82066686966236 0.00067808241639 0.00200000000000 2.81487233105901 0.00085279036802 0.00250000000000 2.80671786536244 0.00072900768452 0.00300000000000 2.79725268974089 0.00080072481819 0.00350000000000 2.78664273475039 0.00075095576213 0.004

5、00000000000 2.77320365026313 0.00065762725379 0.00450000000000 2.75919535464551 0.00081186148545 0.00500000000000 2.74331288628195 0.00079783727034 0.00550000000000 2.72538888482812 0.00073060712986 0.00600000000000 2.70664967712312 0.00063242006530 0.00650000000000 2.68632403406473 0.00063656524495

6、 0.00700000000000 2.66386533852220 0.00080659845639 0.00750000000000 2.64093529707333 0.00067704740069 0.00800000000000 2.61621111727357 0.00076573767706 0.00850000000000 2.59038109850785 0.00054955759081 0.00900000000000 2.56298794272843 0.00058487913971 0.00950000000000 2.53498317950797 0.00055602

7、747368 0.01000000000000 2.50647589372246 0.00033550412588 0.01050000000000 2.47571075016386 0.00056012688452 0.01100000000000 2.44560676000982 0.00056694491978 0.01150000000000 2.41403690772088 0.00059380631025 0.01200000000000 2.38252228611696 0.00053681916544 0.01250000000000 2.35016501182332 0.00

8、065871960781 0.01300000000000 2.31790939837137 0.00068598344328 0.01350000000000 2.28597616656453 0.00060922471348 0.01400000000000 2.25418431681401 0.00057086018918 0.01450000000000 2.22259320219535 0.00041308535708 0.01500000000000 2.19237398969466 0.00047302026281 0.01550000000000 2.1629017799727

9、1 0.00030949309972 0.01600000000000 2.13441725793706 0.00040552624986 0.01650000000000 2.10811064690727 0.00037545033142 0.01700000000000 2.08322179823195 0.00017282319262 0.01750000000000 2.06148109026767 0.00020758327980 0.01800000000000 2.04219885094031 0.00037186464579 0.01850000000000 2.0261023

10、5314357 0.00018082163465 0.01900000000000 2.01290326863579 0.00023323830160 0.01950000000000 2.00463157388395 -0.00004536186964 0.02000000000000 2.00058143251913 0.00003246284068雷達(dá)物體vhd0斜距俯仰角題2 示意圖解： 1. 令 t=0.5 q(k)=0 根據(jù)離散時間擴(kuò)展卡爾曼濾波公式，則有：狀態(tài)方程： 測量方程： 輔助方程：一步預(yù)測： 濾波增益：濾波計算：濾波初值： 2. 實驗結(jié)果高度隨時間變化估計速度隨時間變化的

11、估計高度協(xié)方差：速度協(xié)方差估計實驗結(jié)果分析：根據(jù)圖，可得高度和速度的估計值，通過擴(kuò)展卡爾曼濾波法，高度協(xié)方差和速度協(xié)方差，剛開始的值比較大，但是迅速減小，在幾步之后逐漸趨近于0。3. 有關(guān)參數(shù)的影響（例如初始條件、噪聲統(tǒng)計特性對濾波結(jié)果的影響等）；1）初始條件發(fā)生變化，改變高度和速度的初始值為分析：高度和速度在剛開始的時候波動比較大，不過經(jīng)過10步之后，逐漸趨于平穩(wěn)，高度協(xié)方差收斂變快，速度協(xié)方差基本不變2）初始噪聲改變，分析：高度和速度濾波值基本不變，速度協(xié)方差也基本不變，高度協(xié)方差剛開始的時候有波動，10步之后趨于穩(wěn)定。附源程序：第一題t=1;a=1 -t;0 1;g=9.8;u=-0.5

12、*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.4 1877.7 1825.0 1759.8 1686.7 1603.6 1509.2 1407.6 1294.4 1172.4 1039.9 898.0 745.5 585.0 412.5 231.8 399;for i=2:21pi=a*p*a;%一步預(yù)測xi=a*x+u;k=pi*c*(c*pi*c+r)(-1);%增益x=xi+k*(y(i)

13、-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;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(高度協(xié)方差)figure;t=1:21;plot(t,pv(t);title(速度度協(xié)方差)第二題d=1995;t=0.5;a=1 0 0

14、;0 1 -t;0 0 1; %表示狀態(tài)方程中的faig=9.8;u=0;-0.5*g*t2;g*t; %表示狀態(tài)方程中的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

15、.75919535464551; 2.74331288628195; 2.72538888482812; 2.70664967712312; 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.3501650118

16、2332; 2.31790939837137; 2.28597616656453; 2.25418431681401; 2.22259320219535; 2.19237398969466; 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.00078789815

17、8189; 0.00075850435876; 0.00083282260478; 0.00067808241639; 0.00085279036802; 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.0005

18、4955759081; 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.00020758327980; 0.00037