Kalman Filter in matlab

4 views (last 30 days)
azan
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

Answers (1)

John Petersen
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?

Tags

Products

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!