扩展卡尔曼滤波(EKF)仿真演示.doc

(完整版)扩展卡尔曼滤波(EKF)仿真演示扩展卡尔曼滤波(EKF)仿真演示(西工大 严恭敏,20122-4）一、 问题描述如图1所示，从空中水平抛射出的物体，初始水平速度，初始位置坐标（);受重力和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为;还存在不确定的零均值白噪声干扰力和。在坐标原点处有一观测设备（不妨想象成雷达)，可测得距离(零均值白噪声误差)、角度(零均值白噪声误差）。图1 雷达观测示意图二、 建模系统方程：量测方程：选状态向量,量测向量系统Jacobian矩阵量测Jacobian矩阵三、 Matlab仿真function test_ekf kx = 。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：len x = X（k-1,1）; vx = X(k1，2）; y = X（k1，3)； vy = X(k1,4)； x = x + vx*Ts; vx = vx + （kxvx2+daxrandn(1,1)）Ts； y = y + vyTs; vy = vy + （kyvy2g+dayrandn(1)Ts; X(k,:) = x, vx， y， vy； end figure（1）， hold off, plot(X(:,1）,X(:，3），b）， grid on% figure(2), plot(X（：，2：2：4)） 构造量测量 mrad = 0。001; dr = 10； dafa = 10mrad; % 量测噪声 for k=1：len r = sqrt(X(k,1)2+X(k,3)2） + dr*randn（1，1)； a = atan（X(k，1)/X（k，3)） + dafa*randn(1,1); Z（k,:) = r， a； end figure（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:len Ft = 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; end figure（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*kxvx; F（3，4） = 1; F(4,4） = 2*kyvy; 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/xy21/y； H(2,3） = 1/xy2*x（1/y2); function fX = fff(X， kx, ky， g， Ts) % 系统状态非线性函数 x = X(1); vx = X(2）； y = X（3）； vy = X（4)； x1 = x + vxTs； vx1 = vx + (-kx*vx2)Ts； y1 = y + vyTs; vy1 = vy + （kyvy2-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 = ekf（Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) ekf 滤波函数 Pkk_1 = Phikk_1Pk_1Phikk_1 + Qk； Pxz = Pkk_1*Hk； Pzz = Hk*Pxz + Rk； Kk = Pxz*Pzz1； Xk = fXk_1 + Kk*Zk_hfX；Pk = Pkk_1 - KkPzzKk;图2 仿真结果