1、扩展卡尔曼滤波（EKF）仿真演示一、 问题描述如图 1 所示，从空中水平抛射出的物体，初始水平速度 ，初始位置坐)0(xv标（ ） ；受重力 和阻尼力影响，阻尼力与速度平方成正比，水平和垂)0(,yxg直阻尼系数分别为 ；还存在不确定的零均值白噪声干扰力 和 。在坐yxk, xay标原点处有一观测设备（不妨想象成雷达） ，可测得距离 （零均值白噪声误差r） 、角度 （零均值白噪声误差 ） 。rxy )0(xvgr)0,( 图 1 雷达观测示意图二、 建模系统方程： yyxxagvkv22:f量测方程： )/tn(a:xrrh选状态向量 ,量测向量TyxvTrz系统 Jacobian 矩阵 yx
2、vk201f量测 Jacobian 矩阵 0)/(1)/(122yxyxh三、 Matlab 仿真function test_ekfkx = .01; ky = .05; % 阻尼系数g = 9.8; % 重力t = 10; % 仿真时间Ts = 0.1; % 采样周期len = fix(t/Ts); % 仿真步数% 真实轨迹模拟dax = 1.5; day = 1.5; % 系统噪声X = zeros(len,4); X(1,:) = 0, 50, 500, 0; % 状态模拟的初值for k=2:lenx = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); v
3、y = X(k-1,4); x = x + vx*Ts;vx = vx + (-kx*vx2+dax*randn(1,1)*Ts;y = y + vy*Ts;vy = vy + (ky*vy2-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), plot(X(:,2:2:4)% 构造量测量mrad = 0.001;dr = 10; dafa = 10*mrad; % 量测噪声for k=1:lenr = sqrt(X(k,1)2+X
4、(k,3)2) + 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)2;Rk = diag(dr; dafa)2;Xk = zeros(4,1);Pk = 100*eye(4);X_est = X;for k=1:lenFt = JacobianF(X(k,:), kx, ky, g);Hk = Jaco
5、bianH(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); ylabel(Y); title(ekf simulation);legend(real, measurement, ekf estimated);%子程序%function F = JacobianF(
6、X, kx, ky, g) % 系统状态雅可比函数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) % 量测雅可比函数x = X(1); y = X(3);H = zeros(2,4);r = sqrt(x2+y2);H(1,1) = 1/r; H(1,3) = 1/r;xy2 = 1+(x/y)2;H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y2);function
7、fX = fff(X, kx, ky, g, Ts) % 系统状态非线性函数x = X(1); vx = X(2); y = X(3); vy = X(4); x1 = x + vx*Ts;vx1 = vx + (-kx*vx2)*Ts;y1 = y + vy*Ts;vy1 = vy + (ky*vy2-g)*Ts;fX = x1; vx1; y1; vy1;function hfX = hhh(fX, Ts) % 量测非线性函数x = fX(1); y = fX(3);r = sqrt(x2+y2);a = atan(x/y);hfX = r; a;function Xk, Pk, Kk =
8、 ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数Pkk_1 = Phikk_1*Pk_1*Phikk_1 + Qk; Pxz = Pkk_1*Hk; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz-1;Xk = fXk_1 + Kk*Zk_hfX;Pk = Pkk_1 - Kk*Pzz*Kk;0 20 40 60 80 100 120 140 160 180 200360380400420440460480500520 ekf simulationXYrealmeasurementekf estimated图 2 仿真结果
