《扩展卡尔曼滤波(EKF)仿真演示》由会员分享，可在线阅读，更多相关《扩展卡尔曼滤波(EKF)仿真演示（5页珍藏版）》请在金锄头文库上搜索。
1、扩展卡尔曼滤波（EKF ）仿真演示（西工大 严恭敏，2012-2-4）一、 问题描述如图1所示，从空中水平抛射出的物体，初始水平速度 0（x v ，初始位置坐标（ 0（, 0（y x ）；受重力 g 和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻 尼系数分别为y x k k ,；还存在不确定的零均值白噪声干扰力x a 8和y a 6。在坐 标原点处有一观测设备（不妨想象成雷达），可测得距离r （零均值白噪声误差r 8）、角度a （零均值白噪声误差8a）。图1 雷达观测示意图二、 建模系统方程：丨丨+-=+-=y y y yy x x x x x a g v k v v y a v k v
2、v x 8822: f 量测方程:(+=+=8aa8 /tan(a :22y x ry x r h 选状态向量T y x v y v x =x ,量测向量T ar =z系统Jacobian矩阵I丨1II11 IIIILT -=ddy y xx v k v k 2000010000200010x f 量测 Jacobian 矩阵 I III1IIIILT +-+=dd0 /(I/O/(1/10112222222y x y x y x yy x y x x h 三、Matlab 仿真function test_ekfkx = .01; ky = .05; %阻尼系数 g = 9.8; %重力 t
3、= 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; vy = X(k-1,4; x = x + vx*Ts;vx = vx + (-kx*vxA2+dax*randn(l,l*Ts; y = y + vy*Ts;vy = vy + (ky*vyA2-g+day*randn(1*
4、Ts; X(k,: = x, vx, y, vy; endfigure(l, hold off, plot(X(:,l,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,1A2+X(k,3A2 + 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
5、(Z(:,2, Z(:,1.*cos(Z(:,2,* % ekf 滤波Qk = diag(0; dax; 0; dayA2; Rk = diag(dr; dafaA2; 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,:-hf
6、X; 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(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 =
7、 JacobianH(X % 量测雅可比函数 x = X(1; y = X(3; H = zeros(2,4; r = sqrt(xA2+yA2;H(l,l = 1/r; H(l,3 = 1/r; xy2 = l+(x/yA2;H(2,1 = 1/xy2*1/y; H(2,3 = 1/xy2*x*(-1/yA2;function fX = fff(X, kx, ky, g, Ts %系统状态非线性函数 x = X(l; vx = X(2; y = X(3; vy = X(4; xl = x + vx*Ts;vxl = vx + (-kx*vxA2*Ts; yl = y + vy*Ts;vyl
8、= vy + (ky*vyA2-g*Ts; fX = xl; vxl; yl; vyl;function hfX = hhh(fX, Ts % 量测非线性函数 x = fX(l; y = fX(3; r = sqrt(xA2+yA2;a = atan(x/y; hfX = r; a;function Xk, Pk, Kk = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX % ekf 滤 波函数 Pkk_1 = Phikk_1*Pk_1*Phikk_1 + Qk;Pxz = Pkk_l*Hk; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz-l; Xk = fXk_1 + Kk*Zk_hfX;
