[记录二]学习扩展卡尔曼滤波器

下面是一平抛物体的matlab应用EKF的程序（复制过来的，可以运行，原理上似乎有点问题）：

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(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
%     
    figure(2), plot(X(:,2:2:4))

% 构造量测量
    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(2,2) = -2*kx*vx;
    F(3,4) = 1;
    F(4,4) = 2*ky*vy;
    F(1, 2) = 1;
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 滤波函数
    %时间更新方程（2.15）
    Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk; 
    %状态更新方程（2.16）
    Pxz = Pkk_1*Hk';    Pzz = Hk*Pxz + Rk;     Kk = Pxz*Pzz^-1;
    %状态更新方程（2.17）
    Xk = fXk_1 + Kk*Zk_hfX;
    %状态更新方程（2.18）
    Pk = Pkk_1 - Kk*Pzz*Kk';

----分割线2017-05-22

我分析一一下

系统的数学模型如下：

X(n-1) = [x,vx,y,vy] '= [x(n-1)+vx*Ts;  vx+(-kx*vx^2)*Ts;   y(n-1)+vy*Ts   vy+(-ky*vy^2)*Ts;] + w(n);

Z(n-1) = [r,a] =[ sqrt(X(k,1)^2+X(k,3)^2);   atan(X(k,1)/X(k,3)) ;] + v(n);

问题一：

我很好奇 Z为什么是这样的？？？这样似乎和X的意义没有统一；

问题二：

JacobianF求雅可比矩阵是不是有问题？？

问题三：

[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
这个语句中第一个参数如果联系上下文，这个参数应该直接就是F的雅可比举证，为什么会是eye(4)+Ft*Ts？看不明！
这样
    %状态更新方程（2.17）
    Xk = fXk_1 + Kk*Zk_hfX;
这一句应该才是F的更新。。。。。
问题四：
    Qk = diag([0; dax; 0; day])^2;
    Rk = diag([dr; dafa])^2;
原谅我《概率论与数理统计》没学好。有大神告诉我么？



----分割线，mark 2017-5-23