Testtrackkalman.m
clear all
NoOfImg = 8;
Xmsaved = zeros(2, NoOfImg); Xhsaved = zeros(2, NoOfImg);
for k = 1:NoOfImg [xm, ym] = GetBallPos(k); [xh, yh] = TrackKalman(xm, ym);
hold on
plot(xm, ym, 'r*')
plot(xh, yh, 'bs')
pause(1)
Xmsaved(:, k) = [xm ym]';
Xhsaved(:, k) = [xh yh]';
end
figure hold on plot(Xmsaved(1,:), Xmsaved(2,:), '*') plot(Xhsaved(1,:), Xhsaved(2,:), 's')
Trackkalman.m
if true
end
function [xh, yh] = TrackKalman(xm, ym)
persistent A H Q R
persistent x P
persistent firstRun
if isempty(firstRun) dt = 1;
A = [ 1 dt 0 0
0 1 0 0
0 0 1 dt
0 0 0 1 ];
H = [ 1 0 0 0
0 0 1 0 ];
Q = 1.0*eye(4);
R = [ 50 0
0 50 ];
x = [0, 0, 0, 0]';
P = 100*eye(4);
firstRun = 1;
end
xp = A*x; Pp = A*P*A' + Q;
K = Pp*H'*inv(H*Pp*H' + R);
z = [xm ym]'; x = xp + K*(z - H*xp); P = Pp - K*H*Pp;
xh = x(1); yh = x(3);
function [xc, yc] = GetBallPos1(index) % % persistent imgBg persistent firstRun
if isempty(firstRun) imgBg = imread('Process1/bg1.tif',1);
xc = 0; yc = 0;
imgWork = imread(['Process1/', int2str(index), '.tif']); imshow(imgWork)
fore = imabsdiff(imgWork, imgBg); fore = (fore(:,:,1) > 10) | (fore(:,:,2) > 10) | (fore(:,:,3) > 10);
L = logical(fore); stats = regionprops(L, 'area', 'centroid'); area_vector = [stats.Area]; [tmp, idx] = max(area_vector);
centroid = stats(idx(1)).Centroid;
xc = centroid(1) + 15*randn; yc = centroid(2) + 15*randn;