擴展卡爾曼濾波（EKF）

首先進行文檔下載

仔細閱讀文檔，理解文檔中所述內容。

我對文檔的matlab代碼進行了簡單調整如下：

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %日 期： 	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';

將代碼直接粘貼到matlab中會產生錯誤，需要將代碼中的5個子函數分別生成(.m文件) 

然後將生成的.m文件粘貼到matlab的Current Directory中。

在matlab中運行程序結果如下：