Kalman Filter in matlab

Question

0 个投票

Hi everyone! I am having a video and I have to locate the position of ball using Kalman equations. Suppose the initial position(xi,yi)is known. for the first frame current position is (xi,yi).Please guide me what would be the current position(currx,curry;as in the code below)in the next frames.The piece of code is:

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=0;
for k=1:nframes
if kfinite==0 % kalman initialization
Au=[xi;yi;0;0];
currx=Au(1);
curry=Au(2);
    else
        u=[x1;y1;0;0];
        Au=F*x(k-1,:)'+u; % Prediction 1st equation
        currx=xi
        curry=yi
end
        vx= currx-Au(1);
        vy=curry-Au(2);
        xactual=[currx;curry;vx;vy];
        xactualnext=F*xactual;
        ydesirednext=Hi*xactualnext;
        pp=F*Pe*F' + Q;   %Prediction 2nd equation
        K=pp*Hi'*inv(Hi*pp*Hi' +R);  %%%correction 3rd equation
        Anu=Au + K*(ydesirednext-Hi*Au);  %%correction 4th equation
        Pe=(eye(4)-K*Hi)*pp;  %%correction 5th equation
        x1=Anu(1);
        x2=Anu(2);
        kfinite=1;
    end