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

1、擴(kuò)展卡爾曼濾波(EKF仿真演示(西工大 嚴(yán)恭敏，2012-2-4 )問(wèn)題描述如圖1所示，從空中水平拋射出的物體，初始水平速度vx(0)，初始位置坐標(biāo)(x(0),y(0);受重力g和阻尼力影響，阻尼力與速度平方成正比，水平和垂直阻尼系數(shù)分別為kx,ky ;還存在不確定的零均值白噪聲干擾力：ax和 坷。在坐標(biāo) 原點(diǎn)處有一觀(guān)測(cè)設(shè)備(不妨想象成雷達(dá))，可測(cè)得距離r (零均值白噪聲誤差：.r )、角度(零均值白噪聲誤差建模系統(tǒng)方程：f :量測(cè)方程：h: 選狀態(tài)向量x = X0 100 _0_2kxvx000 0100 002kyvy 1010x2 +y2Jx +2y1/y0-x/ y20_1+(x/y)

2、21+(x/y)2、汀系統(tǒng)Jacobian矩陣二ex量測(cè)Jacobian矩陣-二exX =Vx 2x= *xvx Paxy =Vy 2yy =kyVy _g +5ayr = Jx2 +y2 +3ra = ata n(x/ y) vx y vy T ,量測(cè)向量z三、 Matlab仿真fun ctio n test_ekfkx = .01; ky = .05; % 阻尼系數(shù)g = 9.8; % 重力 t = 10; %仿真時(shí)間Ts = 0.1; %采樣周期 len = fix(t/Ts); % 仿真步數(shù)% 真實(shí)軌跡模擬dax = 1.5; day = 1.5; % 系統(tǒng)噪聲X = zeros(le

3、n,4); X(1,:) = 0, 50, 500, 0; % 狀態(tài)模擬的初值for k=2:lenx = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);x = x + vx*Ts;vx = vx + (-kx*vxA2+dax*ra ndn (1,1)*Ts;y = y + vy*Ts;vy = vy + (ky*vyA2-g+day*randn(1)*Ts;X(k,:) = x, vx, y, vy;endfigure(1), hold off, plot(X(:,1),X(:,3),-b), grid on% figure(2)

4、, plot(X(:,2:2:4)% 構(gòu)造量測(cè)量mrad = 0.001;dr = 10; dafa = 10*mrad; % 量測(cè)噪聲for k=1:lenr = sqrt(X(k,1)A2+X(k,3)A2) + dr*randn(1,1);a = atan(X(k,1)/X(k,3) + dafa*randn(1,1);Z(k,:) = r, a;endfigure(1), hold on, plot(Z(:,1).*sin(Z(:,2), Z(:,1).*cos(Z(:,2),*)% ekf 濾波Qk = diag(0; dax; 0; day)A2;Rk = diag(dr; daf

5、a)A2;Xk = zeros(4,1);Pk = 100*eye(4);X_est = X;for k=1:lenFt = JacobianF(X(k,:), kx, ky, g);Hk = JacobianH(X(k,:);fX = fff(X(k,:), kx, ky, g, Ts);hfX = hhh(fX, Ts);Xk, Pk, Kk = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)-hfX); X_est(k,:) = Xk;endfigure(1), plot(X_est(:,1),X_est(:,3), +r) xlabel(X);

6、 ylabel(Y); title(ekf simulation); legend(real, measurement, ekf estimated);% 子程序 % function F = JacobianF(X, kx, ky, g) % 系統(tǒng)狀態(tài)雅可比函數(shù)vx = X(2); vy = X(4);F = zeros(4,4);F(1,2) = 1;F(2,2) = -2*kx*vx;F(3,4) = 1;F(4,4) = 2*ky*vy;function H = JacobianH(X) % 量測(cè)雅可比函數(shù)x = X(1); y = X(3);H = zeros(2,4);r = sq

7、rt(xA2+yA2);H(1,1) = 1/r; H(1,3) = 1/r; xy2 = 1+(x/y)A2;H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/yA2);fun ctio n fX = fff(X, kx, ky, g, Ts) %系統(tǒng)狀態(tài)非線(xiàn)性函數(shù)x = X(1); vx = X(2); y = X(3); vy = X(4);x1 = x + vx*Ts;vx1 = vx + (-kx*vxA2)*Ts;y1 = y + vy*Ts;vy1 = vy + (ky*vyA2-g)*Ts;fX = x1; vx1; y1; vy1;fun ctio n hfX = hhh(fX, Ts) %量測(cè)非線(xiàn)性函數(shù)x = fX(1); y = fX(3);r = sqrt(xA2+yA2);a = ata n( x/y);濾波函數(shù)hfX = r; a;fun ctio n Xk, Pk, Kk = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf Pkk_1 = Phikk_1*Pk_1*Phikk_1 +