KalmanFilter.m

%% Kalman Filter demo of Ball Tracking
% by Bassam Jalgha 16/6/2013
% Blue  '+' is the centroid of the detected ball
% Green '+' is the position of the modelled ball
% Red   '+' is the Kalman filtered position of the centroid

clc
clear all
close all

vid = VideoReader('ball2.mp4');

nFrames = vid.NumberOfFrames;
vidWidth = vid.Height;
vidHeight= vid.Width;



minx = 92;
maxx = 440;
miny = 554;
maxy = 1198;

vidWidth  =  maxx-minx;
vidHeight =  maxy-miny;

% Preallocate movie structure.
mov(1:nFrames-1) = ...
    struct('cdata', zeros(vidHeight,vidWidth,1, 'uint8'),...
    'colormap', []);
detectedFrame = zeros(vidHeight,vidWidth, nFrames, 'uint8');

% Read one frame at a time.
I = rgb2gray(read(vid, 1));
I = imrotate(I,-90);
background = imcrop(I ,[minx miny maxx maxy]); % for background image removal


h= figure(1)

Centroid= zeros(nFrames,2); %y and x
collectedX = 0;
collectedY = 0;

g = 9.8; % gravity in m/s^2
conv = 1600; %pixel/m --> I got that from the handle 

deltaT = 1/vid.FrameRate;


initialSpeed = 650; % trial and error

X(:,1)      = [18;initialSpeed];
Xhat(:,1)   = [18;initialSpeed];
Xmodel(:,1) = [18;initialSpeed];

A = [ 0 1 ; 0 0 ]; % state space model: Xdot = Ax + Bu with X= [y,ydot]
B = [0 ; g*conv];
F = A*deltaT + eye(2); % transforming from contineous state space into discrete x[k] = F*x[k-1] + Gu;
G = B*deltaT;

H = [1 0];
Q = [80 0 ;0 1]; % the covariance of the process noise
R = 400; % the covariance of the observation noise
P(:,:,1) = [100 0 ;0 100];
Phat(:,:,1) = P(:,:,1) ;



m = 1;
K(:,1) = [1;0];

for k = 16 : nFrames-15
    %pause(0.5);
    
    %tansform into grayscale
    I = rgb2gray(read(vid, k));
    I = imrotate(I,-90);
    % mov(m).cdata = I;
    mov(m).cdata = imcrop(I ,[minx miny maxx maxy]);
    BackgroundReduced = mov(m).cdata - background;% background subtraction
    count = 0;
    for y=1:vidHeight
        for x=1:vidWidth
            if(BackgroundReduced(y,x)>10) %Threshold level
                detectedFrame(y,x,m) = 255;
                count = count + 1; % number of pixels detected
                collectedX(count) = x;
                collectedY(count) = y;
            end
        end
    end
    if(count > 500)
        Centroid(m,1) = mean(collectedY);
        Centroid(m,2) = mean(collectedX);
    else % no "ball" pixels detected
        Centroid(m,1) = 0;
        Centroid(m,2) = 0;
    end

    imshow(mov(m).cdata)
    hold on
    plot(Centroid(m,2),Centroid(m,1),'+');
    plot(Centroid(1,2),X(1,m),'r+')
    plot(Centroid(1,2),Xmodel(1,m),'g+')
    hold off
    drawnow
    
    m = m + 1;
    
    % Model Only
    Xmodel(:,m) = F * Xmodel(:,m-1) + G;
    
    %% Kalman Filter:
    
    %prediction
    Xhat(:,m) = F * X(:,m-1) + G;
    Phat(:,:,m) = F * P(:,:,m-1) + Q;
    
    %Correction
    S = H*Phat(:,:,m)*H'+R;
    K(:,m) = Phat(:,:,m)*H'*inv(S);
    
    if(Centroid(m-1,1))
        e = Centroid(m-1,1) - H*Xhat(:,m);
    else
        e = 0;
        K(:,m)  = [0;0];
    end
    
    X(:,m)   = Xhat(:,m)+ (K(:,m)*e);
    P(:,:,m) = (eye(2)-K(:,m)*H)*Phat(:,:,m);
    
    %name = strcat('KalmanFilteredFrames/frame_',num2str(m-1));
    %saveas(h,name,'png'); %name is a string
end