擴充套件卡爾曼濾波(EKF)
阿新 • • 發佈:2019-02-05
首先進行文件下載
仔細閱讀文件,理解文件中所述內容。
我對文件的matlab程式碼進行了簡單調整如下:
將程式碼直接貼上到matlab中會產生錯誤,需要將程式碼中的5個子函式分別生成(.m檔案)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %日 期: 2015.10.12 %程式功能: 使用擴充套件卡爾曼濾波器(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(k-1,2); y = X(k-1,3); vy = X(k-1,4); x = x + vx*Ts; vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts; y = y + vy*Ts; vy = vy + (ky*vy^2-g+day*randn(1))*Ts; X(k,:) = [x, vx, y, vy]; end figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 構造量測量 mrad = 0.001; dr = 10; dafa = 10*mrad; % 量測噪聲 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*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(x^2+y^2); 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/y^2); function 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*vx^2)*Ts; y1 = y + vy*Ts; vy1 = vy + (ky*vy^2-g)*Ts; fX = [x1; vx1; y1; vy1]; function hfX = hhh(fX, Ts) % 量測非線性函式 x = fX(1); y = fX(3); r = sqrt(x^2+y^2); 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_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1; Xk = fXk_1 + Kk*Zk_hfX; Pk = Pkk_1 - Kk*Pzz*Kk';
然後將生成的.m檔案貼上到matlab的Current Directory中。
在matlab中執行程式結果如下: