Matlab中的卡尔曼滤波器

Question

我有一个视频，我必须使用卡尔曼方程找到一个球的位置。假设球在第一帧(xi,yi)中的初始位置是已知的。

请引导我当前位置(currx,curry);的内容，如下面代码中的代码所示。

到目前为止我所拥有的：

R  = [0.2845 0.0045;0.0045 0.0455];
Hi = [1 0 0 0; 0 1 0 0];
Q  = 0.01*eye(4);
Pe = 100*eye(4);
F  = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
x  = zeros(569,4);

kfinite = false;
for k = 1:nframes

    % Kalman initialization
    if ~kfinite
        Au    = [xi yi 0 0].';
        currx = xi;
        curry = yi;

    % Prediction 1st equation
    else            
        u  = [x1 y1 0 0].';
        Au = F*x(k-1,:).' + u; 
        currx = xi;
        curry = yi;

    end

    vx = currx-Au(1);
    vy = curry-Au(2);

    xactual      = [currx curry vx vy].';
    xactualnext  = F*xactual;
    ydesirednext = Hi*xactualnext;

    % Prediction 2nd equation
    pp = F*Pe*F.' + Q;   

    % correction 3rd equation
    K = pp*Hi.' / (Hi*pp*Hi.' + R); 

    % correction 4th equation
    Anu = Au + K*(ydesirednext - Hi*Au); 

    % correction 5th equation
    Pe = (eye(4) - K*Hi)*pp;  
    x1 = Anu(1);
    x2 = Anu(2);

    kfinite = true;

end