# 2D Target tracking using Kalman filter

12 Mar 2007 (Updated )

This Demo shows tracking target and prediction next position using kalman filter

kalman.m
clear,clc
% compute the background image
Imzero = zeros(240,320,3);
for i = 1:5
Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);

% Kalman filter initialization
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
P = 100*eye(4);
dt=1;
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
g = 6; % pixels^2/time step
Bu = [0,0,0,g]';
kfinit=0;
x=zeros(100,4);

% loop over all images
for i = 1 : 60
imshow(Im)
imshow(Im)
Imwork = double(Im);

%extract ball
if flag==0
continue
end

hold on
plot(cc(i)+c,cr(i)+r,'g.')
plot(cc(i)+c,cr(i)-r,'g.')
end
% Kalman update
i
if kfinit==0
xp = [MC/2,MR/2,0,0]'
else
xp=A*x(i-1,:)' + Bu
end
kfinit=1;
PP = A*P*A' + Q
K = PP*H'*inv(H*PP*H'+R)
x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
x(i,:)
[cc(i),cr(i)]
P = (eye(4)-K*H)*PP

hold on
plot(x(i,1)+c,x(i,2)+r,'r.')
plot(x(i,1)+c,x(i,2)-r,'r.')
end
pause(0.3)
end

% show positions
figure
plot(cc,'r*')
hold on
plot(cr,'g*')
%end

%estimate image noise (R) from stationary ball
posn = [cc(55:60)',cr(55:60)'];
mp = mean(posn);
diffp = posn - ones(6,1)*mp;
Rnew = (diffp'*diffp)/5;