擴(kuò)展卡爾曼濾波ekf仿真演示

1、擴(kuò)展卡爾曼濾波（擴(kuò)展卡爾曼濾波（EKFEKF）仿真演示）仿真演示一、一、問題描述問題描述如圖1所示，從空中水平拋射出的物體，初始水平速度，初始位置坐)0(xv標(biāo)（）；受重力和阻尼力影響，阻尼力與速度平方成正比，水平和垂)0()0(yxg直阻尼系數(shù)分別為；還存在不確定的零均值白噪聲干擾力和。在坐yxkkxa?ya?標(biāo)原點(diǎn)處有一觀測設(shè)備（不妨想象成雷達(dá)），可測得距離（零均值白噪聲誤差r）、角度（零均值白噪聲誤差）。r????xy)0(xvg

2、r?)00(圖1雷達(dá)觀測示意圖二、二、建模建模系統(tǒng)方程：???????????????yyyyyxxxxxagvkvvyavkvvx??22:????f量測方程：??????????????)tan(a:22yxryxrh選狀態(tài)向量量測向量??Tyxvyvx?x??T?r?z系統(tǒng)Jacobian矩陣??????????????????yyxxvkvk2000010000200010xf量測Jacobian矩陣??????????????

3、??????0)(10)(1101012222222yxyxyxyyxyxxh三、三、MatlabMatlab仿真仿真functiontest_ekfkx=.01ky=.05%阻尼系數(shù)g=9.8%重力t=10%仿真時間Ts=0.1%采樣周期len=fix(tTs)%仿真步數(shù)%真實(shí)軌跡模擬functionfX=fff(XkxkygTs)%系統(tǒng)狀態(tài)非線性函數(shù)x=X(1)vx=X(2)y=X(3)vy=X(4)x1=xvxTsvx1=vx(k

4、xvx^2)Tsy1=yvyTsvy1=vy(kyvy^2g)TsfX=[x1vx1y1vy1]functionhfX=hhh(fXTs)%量測非線性函數(shù)x=fX(1)y=fX(3)r=sqrt(x^2y^2)a=atan(xy)hfX=[ra]function[XkPkKk]=ekf(Phikk_1QkfXk_1Pk_1HkRkZk_hfX)%ekf濾波函數(shù)Pkk_1=Phikk_1Pk_1Phikk_1QkPxz=Pkk_1HkPz