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

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ī)序列，該物體的初始高度和速度也是高斯分布的隨機(jī)變量，且。試求該物體高度和速度隨時(shí)間變化的最優(yōu)估計(jì)。（）解： 1. 令 t=1 r（k

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

3、和速度濾波值在開始幾步接近初始值，協(xié)方差值基本不變。2）當(dāng)初始協(xié)方差值改變時(shí)，改為實(shí)驗(yàn)結(jié)果分析高度和速度濾波值基本不變，速度協(xié)方差和高度協(xié)方差開始要接近速度協(xié)方差和高度協(xié)方差的初始值。但是經(jīng)過幾步之后，都趨于0。二 同樣考慮自由落體運(yùn)動(dòng)的物體，用雷達(dá)（和物體落地點(diǎn)在同一水平面）進(jìn)行測(cè)量，如圖所示。如果，且雷達(dá)測(cè)距和測(cè)角的測(cè)量噪聲是高斯白噪聲隨機(jī)序列，均值為零、方差陣，試根據(jù)下列測(cè)量數(shù)據(jù)確定物體的高度和速度隨時(shí)間變化的估計(jì)值。時(shí)間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ù)離散時(shí)間擴(kuò)展卡爾曼濾波公式，則有：狀態(tài)方程： 測(cè)量方程： 輔助方程：一步預(yù)測(cè)： 濾波增益：濾波計(jì)算：濾波初值： 2. 實(shí)驗(yàn)結(jié)果高度隨時(shí)間變化估計(jì)速度隨時(shí)間變化的

11、估計(jì)高度協(xié)方差：速度協(xié)方差估計(jì)實(shí)驗(yàn)結(jié)果分析：根據(jù)圖，可得高度和速度的估計(jì)值，通過擴(kuò)展卡爾曼濾波法，高度協(xié)方差和速度協(xié)方差，剛開始的值比較大，但是迅速減小，在幾步之后逐漸趨近于0。3. 有關(guān)參數(shù)的影響（例如初始條件、噪聲統(tǒng)計(jì)特性對(duì)濾波結(jié)果的影響等）；1）初始條件發(fā)生變化，改變高度和速度的初始值為分析：高度和速度在剛開始的時(shí)候波動(dòng)比較大，不過經(jīng)過10步之后，逐漸趨于平穩(wěn)，高度協(xié)方差收斂變快，速度協(xié)方差基本不變2）初始噪聲改變，分析：高度和速度濾波值基本不變，速度協(xié)方差也基本不變，高度協(xié)方差剛開始的時(shí)候有波動(dòng)，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ù)測(cè)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; %表示測(cè)量噪聲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