Object tracking with kalman filter! (in amazing 2-d!)
So, Mr. 80's Cat (reminds me of that guy from Biff's gang in that one Back to the Future movie...hmm..i digress lol). Soo, Mr. 80's Cat, you say 1-d isn't that cool?! You think 2-d tracking of an object in a 2-d is better for what you're studying?! It's better for what your Boss/Advisor/EvilWarLord expects of you?! hmm..really? ok, well them I guess you have a point there. lol Ok, so yea, here's how you apply the Kalman Filter to an 2-d object using a very simple position and velocity state update model. It's sufficient for tracking a bug but maybe not much more ..so email me if you have better code!
The link to the video frames can be found here:
https://docs.google.com/open?id=0BwvRg6SCVt_ndTFBUjJOT0F4Q0k
The link to the video frames can be found here:
https://docs.google.com/open?id=0BwvRg6SCVt_ndTFBUjJOT0F4Q0k
|
These tutorials are fueled by coffee and Ramen. If you would like to see more Espresso to tutorial/code transforms, please make a small donation, preferably > 1/∞ :P
|
|
| 2_d_kalman_filter_student_dave.m |
%Student Dave's tutorial on: Object tracking in image using 2-D kalman filter
%HEXBUG TRACKING!
%Copyright Student Dave's Tutorials 2012
%if you would like to use this code, please feel free, just remember to
%reference and tell your friends! :)
%here we take the hexbug extracted coordinates and apply the kalman fiter
%to the 2-dimensions of motion within the image
%to see if it can do better than the tracking alone
%requires matlabs image processing toolbox.
clear all;
close all;
clc;
set(0,'DefaultFigureWindowStyle','docked') %dock the figures..just a personal preference you don't need this.
base_dir = 'E:\Dropbox\Student_dave\hexbug_frames\';
cd(base_dir);
%% get listing of frames
f_list = dir('*png');
%% load tracking data
%load('CM_idx_easier.mat') %simple tracking: continuously monitored bug
%load('CM_idx_harder.mat') %hard tracking: segment of very noisy images and very bad tracking
load('CM_idx_no.mat'); %missing data tracking: segment with no tracking
%% define main variables
dt = 1; %our sampling rate
S_frame = 10; %starting frame
u = .005; % define acceleration magnitude
Q= [CM_idx(S_frame,1); CM_idx(S_frame,2); 0; 0]; %initized state--it has four components: [positionX; positionY; velocityX; velocityY] of the hexbug
Q_estimate = Q; %estimate of initial location estimation of where the hexbug is (what we are updating)
HexAccel_noise_mag = .1; %process noise: the variability in how fast the Hexbug is speeding up (stdv of acceleration: meters/sec^2)
tkn_x = 1; %measurement noise in the horizontal direction (x axis).
tkn_y = 1; %measurement noise in the horizontal direction (y axis).
Ez = [tkn_x 0; 0 tkn_y];
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].*HexAccel_noise_mag^2; % Ex convert the process noise (stdv) into covariance matrix
P = Ex; % estimate of initial Hexbug position variance (covariance matrix)
%% Define update equations in 2-D! (Coefficent matrices): A physics based model for where we expect the HEXBUG to be [state transition (state + velocity)] + [input control (acceleration)]
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; %state update matrice
B = [(dt^2/2); (dt^2/2); dt; dt];
C = [1 0 0 0; 0 1 0 0]; %this is our measurement function C, that we apply to the state estimate Q to get our expect next/new measurement
%% initize result variables
% Initialize for speed
Q_loc = []; % ACTUAL hexbug motion path
vel = []; % ACTUAL hexbug velocity
Q_loc_meas = []; % the hexbug path extracted by the tracking algo
%% initize estimation variables
Q_loc_estimate = []; % position estimate
vel_estimate = []; % velocity estimate
P_estimate = P;
predic_state = [];
predic_var = [];
r = 5; % r is the radius of the plotting circle
j=0:.01:2*pi; %to make the plotting circle
for t = S_frame:length(f_list)
% load the image
img_tmp = double(imread(f_list(t).name));
img = img_tmp(:,:,1);
% load the given tracking
Q_loc_meas(:,t) = [ CM_idx(t,1); CM_idx(t,2)];
%% do the kalman filter
% Predict next state of the Hexbug with the last state and predicted motion.
Q_estimate = A * Q_estimate + B * u;
predic_state = [predic_state; Q_estimate(1)] ;
%predict next covariance
P = A * P * A' + Ex;
predic_var = [predic_var; P] ;
% predicted Ninja measurement covariance
% Kalman Gain
K = P*C'*inv(C*P*C'+Ez);
% Update the state estimate.
if ~isnan(Q_loc_meas(:,t))
Q_estimate = Q_estimate + K * (Q_loc_meas(:,t) - C * Q_estimate);
end
% update covariance estimation.
P = (eye(4)-K*C)*P;
%% Store data
Q_loc_estimate = [Q_loc_estimate; Q_estimate(1:2)];
vel_estimate = [vel_estimate; Q_estimate(3:4)];
%% plot the images with the tracking
imagesc(img);
axis off
colormap(gray);
hold on;
plot(r*sin(j)+Q_loc_meas(2,t),r*cos(j)+Q_loc_meas(1,t),'.g'); % the actual tracking
plot(r*sin(j)+Q_estimate(2),r*cos(j)+Q_estimate(1),'.r'); % the kalman filtered tracking
hold off
pause(0.1)
end