# Kalman Filter in matlab

2 views (last 30 days)
azan on 5 Nov 2012
Answered: John Petersen on 23 Jul 2014
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

John Petersen on 23 Jul 2014
You are using +u in the prediction equation but not in the "actual" equation. Could this be your error?