%% define our meta-variables (i.e.

how long and often we will sample)

clear all
close all
clc
%% Initialization
redThresh = 0.20; % Threshold for red detection
vidDevice = imaq.VideoDevice('winvideo', 1, 'YUY2_320x240', ... % Acquire input
video stream
'ROI', [1 1 320 240], ...
'ReturnedColorSpace', 'rgb');
vidInfo = imaqhwinfo(vidDevice); % Acquire input video property
hblob = vision.BlobAnalysis('AreaOutputPort', false, ... % Set blob analysis han
dling
'CentroidOutputPort', true, ...
'BoundingBoxOutputPort', true', ...
'MinimumBlobArea', 200, ...
'MaximumBlobArea', 1000, ...
'MaximumCount', 1);
hshapeinsBox = vision.ShapeInserter('BorderColorSource', 'Input port', ... % Set
box handling
'Fill', true, ...
'FillColorSource', 'Input port', ...
'Opacity', 0.3);
htextinsRed = vision.TextInserter('Text', 'Red : %2d', ... % Set text for numb
er of blobs
'Location', [5 2], ...
'Color', [1 0 0], ... // red color
'Font', 'Courier New', ...
'FontSize', 14);
htextinsCent = vision.TextInserter('Text', '+
X:%4d, Y:%4d', ... % set text
for centroid
'LocationSource', 'Input port', ...
'Color', [1 0 0], ... // red color
'Font', 'Courier New', ...
'FontSize', 14);
hVideoIn = vision.VideoPlayer('Name', 'Final Video', ... % Output video player
'Position', [100 100 vidInfo.MaxWidth+20 vidInfo
.MaxHeight+30]);
nFrame = 0; % Frame number initialization
%% Processing Loop
while(nFrame < 200)
rgbFrame = step(vidDevice); % Acquire single frame
rgbFrame = flipdim(rgbFrame,2); % obtain the mirror image for displaying
diffFrameRed = imsubtract(rgbFrame(:,:,1), rgb2gray(rgbFrame)); % Get red co
mponent of the image
diffFrameRed = medfilt2(diffFrameRed, [3 3]); % Filter out the noise by usin
g median filter
binFrameRed = im2bw(diffFrameRed, redThresh); % Convert the image into binar
y image with the red objects as white
[centroidRed, bboxRed] = step(hblob, binFrameRed); % Get the centroids and b
ounding boxes of the red blobs
centroidRed = uint16(centroidRed); % Convert the centroids into Integer for
further steps
rgbFrame(1:50,1:90,:) = 0; % put a black region on the output stream

vidIn = step(hshapeinsBox, rgbFrame, bboxRed, single([1 0 0])); % Instert th

e red box
for object = 1:1:length(bboxRed(:,1)) % Write the corresponding centroids fo
r red
centXRed = centroidRed(object,1); centYRed = centroidRed(object,2);
vidIn = step(htextinsCent, vidIn, [centXRed centYRed], [centXRed-6 centY
Red-9]);
end
vidIn = step(htextinsRed, vidIn, uint8(length(bboxRed(:,1)))); % Count the n
umber of red blobs
step(hVideoIn, vidIn); % Output video stream
nFrame = nFrame+1;
end
%% Clearing Memory
release(hVideoIn); % Release all memory and buffer used
release(vidDevice);
set(0,'DefaultFigureWindowStyle','normal')
duration = 10 %how long the ball flies
dt = .1; %The tracker looks for the ball,
%but we'll assume he's just repeatedly sampling over time at a fixed interval
%% Define update equations (Coefficent matrices): A physics based model for wher
e we expect the ball to be [state transition (state + velocity)] + [input contro
l (acceleration)]
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1] ; % state transition matrix: expecte
d flight of the ball (state prediction)
B = [0 0 0 0 ; 0 dt^2/2 0 0; 0 0 0 0 ;0 0 0 dt;]; %input control matrix: expect
ed effect of the input accceleration on the state.
C = [1 0 0 0; 0 1 0 0]; % measurement matrix: the expected measurement given the
predicted state (likelihood)
%since we are only measuring position, we set the velocity variable to zero.
%% define main variables
u = -9.81; % define acceleration magnitude
Q= [centXRed 0 0 0; 0 0 0 0; 0 centYRed 0 0; 0 0 0 0]; %initized state--it ha
s two components: [position; velocity] of the ball
Q_estimate = Q; %x_estimate of initial location estimation of where the ball is
(what we are updating)
BallAccel_noise_mag = 0.05; %process noise: the variability in how fast the ball
is speeding up (stdv of acceleration: meters/sec^2)
Tracker_noise_mag = 10; %measurement noise: tracker (stdv of location, in meter
s)
Ez = [Tracker_noise_mag^2 0; 0 Tracker_noise_mag^2];% Ez convert the measurement
noise (stdv) into covariance matri
% Ex = [dt^4/4 0 dt^3/2 0; ...
%
0 dt^4/4 0 dt^3/2; ...
%
dt^3/2 0 dt^2 0; ...
%
0 dt^3/2 0 dt^2].*BallAccel_noise_mag^2;
Ex = BallAccel_noise_mag^2 * [0 0 0 0;0 dt^4/4 0 dt^3/2;0 0 0 0;0 dt^3/2 0 dt^2]
; % Ex convert the process noise (stdv) into covariance matrix
P = Ex; % estimate of initial ball position variance (covariance matrix)
%% initize result variables
% Initialize for speed
Q_loc = []; % ACTUAL ball flight path
vel = []; % ACTUAL ball velocity
Q_loc_meas = []; % ball path that the tracker sees

Q_loc_x = []; % ACTUAL ball flight path

vel_x = []; % ACTUAL ball velocity
Q_loc_meas_x = []; % ball path that the tracker sees
%% simulate what the tracker sees over time
figure(1);clf
figure(4);clf
for t = 0 : dt: duration
% Generate the ball flight
BallAccel_noise = BallAccel_noise_mag * [0 0 0 0; 0 ((dt^2/2)*randn) 0 0; 0
0 0 0; 0 0 0 (dt*randn)];
Q= double(A) * double(Q) + double(B) * u + BallAccel_noise;
% Generate what the tracker sees
TrackerVision_noise = Tracker_noise_mag * randn;
y = C * Q + TrackerVision_noise;
Q_loc = [Q_loc; Q(2)];
Q_loc_meas = [Q_loc_meas; y(2)];
vel = [vel; Q(4)];
Q_loc_x = [Q_loc_x; Q(1)];
Q_loc_meas_x = [Q_loc_meas_x; y(1)];
vel_x = [vel_x; Q(3)];
%iteratively plot what the tracker sees
figure(1)
plot(0:dt:t, Q_loc, '-r.')
plot(0:dt:t, Q_loc_meas, '-k.')
axis([0 10 -30 80])
hold on
figure(4)
plot(0:dt:t, Q_loc_x, '-r.')
plot(0:dt:t, Q_loc_meas_x, '-k.')
axis([0 10 -30 80])
hold on
%pause
end
%plot theoretical path of tracker that doesn't use kalman filter ()
figure(1)
plot(0:dt:t,Q_loc_meas, '-g.')
figure(4)
plot(0:dt:t,Q_loc_meas_x, '-g.')
%plot velocity, just to show that it's constantly increasing, due to
%constant acceleration
%figure(2);
%plot(0:dt:t, vel, '-b.')
%% Do kalman filtering
%initize estimation variables
Q_loc_estimate = []; % ball position estimate
vel_estimate = []; % ball velocity estimate

Q_loc_estimate_x = []; % ball position estimate

vel_estimate_x = []; % ball velocity estimate
Q= [0; 0; 0; 0];% re-initized state
Q_x= [0; 0; 0; 0];
Q_estimate = Q;
Q_estimate_x = Q;
P_estimate = P;
P_estimate_x = P;
P_mag_estimate = [];
P_mag_estimate_x = [];
predic_state = [];
predic_state_x = [];
predic_var = [];
predic_var_x = [];
for t = 1:length(Q_loc)
% Predict next state of the ball with the last state and predicted motion.
Q_estimate = A * Q_estimate + B * u;
predic_state = [predic_state; Q_estimate(2)];
%predict next covariance
P = A * P * A' + Ex;
predic_var = [predic_var; P]
% predicted tracker measurement covariance
% Kalman Gain
K = 1000000*(P*C'*inv(C*P*C'+Ez));
% Update the state estimate.
Q_estimate = Q_estimate + K * (Q_loc_meas(t) - C * Q_estimate);
% update covariance estimation.
P = (eye(4)-K*C)*P;
%Store for plotting
Q_loc_estimate = [Q_loc_estimate; Q_estimate(2)];
vel_estimate = [vel_estimate; Q_estimate(4)];
P_mag_estimate = [P_mag_estimate; P(2,2)];
end
figure(2);clf
% Plot the results
figure(2)
tt = 0 : dt : duration;
plot(tt,Q_loc,'-r.',tt,Q_loc_meas,'-k.', tt,Q_loc_estimate,'-b.');
axis([0 10 -1000 80])
hold on
for t = 1:length(Q_loc_x)
% Predict next state of the ball with the last state and predicted motion.
Q_estimate_x = A * Q_estimate + B * u;
predic_state_x = [predic_state_x; Q_estimate_x(1)] ;
%predict next covariance
P = A * P * A' + Ex;
predic_var_x = [predic_var_x; P] ;
% predicted tracker measurement covariance
% Kalman Gain
K = P*C'*inv(C*P*C'+Ez);
% Update the state estimate.
Q_estimate_x = Q_estimate_x + K * (Q_loc_meas_x(t) - C * Q_estimate_x);
% update covariance estimation.
P = (eye(4)-K*C)*P;
%Store for plotting
Q_loc_estimate_x = [Q_loc_estimate_x; Q_estimate_x(1)];

vel_estimate_x = [vel_estimate_x; Q_estimate_x(3)];

P_mag_estimate_x = [P_mag_estimate_x; P(1,1)];
end
figure(5);clf
tt = 0 : dt : duration;
plot(tt,Q_loc_x,'-r.',tt,Q_loc_meas_x,'-k.', tt,Q_loc_estimate_x,'-g.');
axis([0 10 -300 80])
hold on
%plot the evolution of the distributions
figure(3);clf
for T = 1:length(Q_loc_estimate)
clf
x = Q_loc_estimate(T)-5:.01:Q_loc_estimate(T)+5; % range on x axis
%predicted next position of the ball
hold on
mu = predic_state(T); % mean
sigma = predic_var(T); % standard deviation
y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y,'Color','m'); % or use hold on and normal plot
%data measured by the tracker
mu = Q_loc_meas(T); % mean
sigma = Tracker_noise_mag; % standard deviation
y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y,'Color','b'); % or use hold on and normal plot
%combined position estimate
mu = Q_loc_estimate(T); % mean
sigma = P_mag_estimate(T); % standard deviation
y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y, 'Color','g'); % or use hold on and normal plot
axis([Q_loc_estimate(T)-5 Q_loc_estimate(T)+5 0 1]);
hold on
%actual position of the ball
plot(Q_loc(T));
ylim=get(gca,'ylim');
line([Q_loc(T);Q_loc(T)],ylim.','linewidth',2,'color','b');
legend('state predicted','measurement','state estimate','actual Ball positio
n')
hold on
% pause
end
%plot the evolution of the distributions
figure(3);clf
for T = 1:length(Q_loc_estimate_x)
clf
x = Q_loc_estimate_x(T)-5:.01:Q_loc_estimate_x(T)+5; % range on x axis
%predicted next position of the ball
hold on
mu = predic_state(T); % mean

sigma = predic_var(T); % standard deviation

y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y,'Color','m'); % or use hold on and normal plot
%data measured by the tracker
mu = Q_loc_meas(T); % mean
sigma = Tracker_noise_mag; % standard deviation
y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y,'Color','k'); % or use hold on and normal plot
%combined position estimate
mu = Q_loc_estimate(T); % mean
sigma = P_mag_estimate(T); % standard deviation
y = normpdf(x,mu,sigma); % pdf
y = y/(max(y));
hl = line(x,y, 'Color','g'); % or use hold on and normal plot
axis([Q_loc_estimate(T)-5 Q_loc_estimate(T)+5 0 1]);
%actual position of the ball
plot(Q_loc(T));
ylim=get(gca,'ylim');
line([Q_loc(T);Q_loc(T)],ylim.','linewidth',2,'color','b');
legend('state predicted','measurement','state estimate','actual Ball positio
n')
% pause
end