diff --git a/tracking/computeHistogram.m b/tracking/computeHistogram.m
new file mode 100644
index 0000000..9207538
--- /dev/null
+++ b/tracking/computeHistogram.m
@@ -0,0 +1,24 @@
+function histogram = computeHistogram(patch, mask, n_bins, grayscale_sequence)
+%COMPUTEHISTOGRAM creates a colour (or grayscale) histogram of an image patch
+% MASK has the same size as the image patch and selects what should
+% be used when computing the histogram (i.e. out-of-frame regions are ignored)
+
+	[h, w, d] = size(patch);
+
+	assert(all([h w]==size(mask)) == 1, 'mask and image are not the same size');
+
+	bin_width = 256/n_bins;
+
+	% convert image to 1d array with same n channels of img patch
+	patch_array = reshape(double(patch), w*h, d);
+	% compute to which bin each pixel (for all 3 channels) belongs to
+	bin_indices = floor(patch_array/bin_width) + 1;
+
+	if grayscale_sequence
+		histogram = accumarray(bin_indices, mask(:), [n_bins 1])/sum(mask(:));
+	else
+		% the histogram is a cube of side n_bins
+		histogram = accumarray(bin_indices, mask(:), [n_bins n_bins n_bins])/sum(mask(:));
+	end
+
+end
diff --git a/tracking/cropFilterResponse.m b/tracking/cropFilterResponse.m
new file mode 100644
index 0000000..0d2e734
--- /dev/null
+++ b/tracking/cropFilterResponse.m
@@ -0,0 +1,26 @@
+function new_response = cropFilterResponse(response_cf, response_size)
+%CROPFILTERRESPONSE makes RESPONSE_CF of size RESPONSE_SIZE (i.e. same size of colour response)
+
+    [h,w] = size(response_cf);
+    b = response_size(1);
+    a = response_size(2);
+
+    % a and b must be odd, as we want an exact center
+    if ~all_odd([a, b])
+        error('dimensions must be odd');
+    end
+    half_width = floor(a/2);
+    half_height = floor(b/2);
+
+    new_response = response_cf(...
+        mod_one(-half_height:half_height, h), ...
+        mod_one(-half_width:half_width, w));
+end
+
+function y = mod_one(a, b)
+    y = mod(a-1, b)+1;
+end
+
+function y = all_odd(x)
+    y = all(mod(x, 2) == 1);
+end
diff --git a/tracking/display_statistics.m b/tracking/display_statistics.m
new file mode 100644
index 0000000..abfcd0f
--- /dev/null
+++ b/tracking/display_statistics.m
@@ -0,0 +1,14 @@
+function display_statistics(name, precision, reinitializations, fps)
+    %print statistics to command window
+    str = sprintf('Overlap:% 1.3f', precision);
+
+    if ~isnan(reinitializations),
+        if reinitializations == round(reinitializations),  %prettier for integer values
+            str = [str, sprintf(', reinitializations:% 2i', reinitializations)];
+        else
+            str = [str, sprintf(', reinitializations:% 1.2f', reinitializations)];
+        end
+    end
+
+    fprintf('%12s - %s, FPS:% 4.2f\n', name, str, fps)
+end
diff --git a/tracking/fhog.m b/tracking/fhog.m
new file mode 100644
index 0000000..f96fc0f
--- /dev/null
+++ b/tracking/fhog.m
@@ -0,0 +1,76 @@
+function H = fhog( I, binSize, nOrients, clip, crop )
+% Efficiently compute Felzenszwalb's HOG (FHOG) features.
+%
+% A fast implementation of the HOG variant used by Felzenszwalb et al.
+% in their work on discriminatively trained deformable part models.
+%  http://www.cs.berkeley.edu/~rbg/latent/index.html
+% Gives nearly identical results to features.cc in code release version 5
+% but runs 4x faster (over 125 fps on VGA color images).
+%
+% The computed HOG features are 3*nOrients+5 dimensional. There are
+% 2*nOrients contrast sensitive orientation channels, nOrients contrast
+% insensitive orientation channels, 4 texture channels and 1 all zeros
+% channel (used as a 'truncation' feature). Using the standard value of
+% nOrients=9 gives a 32 dimensional feature vector at each cell. This
+% variant of HOG, refered to as FHOG, has been shown to achieve superior
+% performance to the original HOG features. For details please refer to
+% work by Felzenszwalb et al. (see link above).
+%
+% This function is essentially a wrapper for calls to gradientMag()
+% and gradientHist(). Specifically, it is equivalent to the following:
+%  [M,O] = gradientMag( I,0,0,0,1 ); softBin = -1; useHog = 2;
+%  H = gradientHist(M,O,binSize,nOrients,softBin,useHog,clip);
+% See gradientHist() for more general usage.
+%
+% This code requires SSE2 to compile and run (most modern Intel and AMD
+% processors support SSE2). Please see: http://en.wikipedia.org/wiki/SSE2.
+%
+% USAGE
+%  H = fhog( I, [binSize], [nOrients], [clip], [crop] )
+%
+% INPUTS
+%  I        - [hxw] color or grayscale input image (must have type single)
+%  binSize  - [8] spatial bin size
+%  nOrients - [9] number of orientation bins
+%  clip     - [.2] value at which to clip histogram bins
+%  crop     - [0] if true crop boundaries
+%
+% OUTPUTS
+%  H        - [h/binSize w/binSize nOrients*3+5] computed hog features
+%
+% EXAMPLE
+%  I=imResample(single(imread('peppers.png'))/255,[480 640]);
+%  tic, for i=1:100, H=fhog(I,8,9); end; disp(100/toc) % >125 fps
+%  figure(1); im(I); V=hogDraw(H,25,1); figure(2); im(V)
+%
+% EXAMPLE
+%  % comparison to features.cc (requires DPM code release version 5)
+%  I=imResample(single(imread('peppers.png'))/255,[480 640]); Id=double(I);
+%  tic, for i=1:100, H1=features(Id,8); end; disp(100/toc)
+%  tic, for i=1:100, H2=fhog(I,8,9,.2,1); end; disp(100/toc)
+%  figure(1); montage2(H1); figure(2); montage2(H2);
+%  D=abs(H1-H2); mean(D(:))
+%
+% See also hog, hogDraw, gradientHist
+%
+% Piotr's Image&Video Toolbox      Version 3.23
+% Copyright 2013 Piotr Dollar.  [pdollar-at-caltech.edu]
+% Please email me if you find bugs, or have suggestions or questions!
+% Licensed under the Simplified BSD License [see external/bsd.txt]
+
+%Note: modified to be more self-contained
+
+if( nargin<2 ), binSize=8; end
+if( nargin<3 ), nOrients=9; end
+if( nargin<4 ), clip=.2; end
+if( nargin<5 ), crop=0; end
+
+softBin = -1; useHog = 2; b = binSize;
+
+[M,O]=gradientMex('gradientMag',I,0,1);
+
+H = gradientMex('gradientHist',M,O,binSize,nOrients,softBin,useHog,clip);
+
+if( crop ), e=mod(size(I),b)<b/2; H=H(2:end-e(1),2:end-e(2),:); end
+
+end
diff --git a/tracking/freezeColors.m b/tracking/freezeColors.m
new file mode 100644
index 0000000..6c7b465
--- /dev/null
+++ b/tracking/freezeColors.m
@@ -0,0 +1,275 @@
+function freezeColors(varargin)
+% freezeColors  Lock colors of plot, enabling multiple colormaps per figure. (v2.3)
+%
+%   Problem: There is only one colormap per figure. This function provides
+%       an easy solution when plots using different colomaps are desired 
+%       in the same figure.
+%
+%   freezeColors freezes the colors of graphics objects in the current axis so 
+%       that subsequent changes to the colormap (or caxis) will not change the
+%       colors of these objects. freezeColors works on any graphics object 
+%       with CData in indexed-color mode: surfaces, images, scattergroups, 
+%       bargroups, patches, etc. It works by converting CData to true-color rgb
+%       based on the colormap active at the time freezeColors is called.
+%
+%   The original indexed color data is saved, and can be restored using
+%       unfreezeColors, making the plot once again subject to the colormap and
+%       caxis.
+%
+%
+%   Usage:
+%       freezeColors        applies to all objects in current axis (gca),
+%       freezeColors(axh)   same, but works on axis axh.
+%
+%   Example:
+%       subplot(2,1,1); imagesc(X); colormap hot; freezeColors
+%       subplot(2,1,2); imagesc(Y); colormap hsv; freezeColors etc...
+%
+%       Note: colorbars must also be frozen. Due to Matlab 'improvements' this can
+%				no longer be done with freezeColors. Instead, please
+%				use the function CBFREEZE by Carlos Adrian Vargas Aguilera
+%				that can be downloaded from the MATLAB File Exchange
+%				(http://www.mathworks.com/matlabcentral/fileexchange/24371)
+%
+%       h=colorbar; cbfreeze(h), or simply cbfreeze(colorbar)
+%
+%       For additional examples, see test/test_main.m
+%
+%   Side effect on render mode: freezeColors does not work with the painters
+%       renderer, because Matlab doesn't support rgb color data in
+%       painters mode. If the current renderer is painters, freezeColors
+%       changes it to zbuffer. This may have unexpected effects on other aspects
+%	      of your plots.
+%
+%       See also unfreezeColors, freezeColors_pub.html, cbfreeze.
+%
+%
+%   John Iversen (iversen@nsi.edu) 3/23/05
+%
+
+%   Changes:
+%   JRI (iversen@nsi.edu) 4/19/06   Correctly handles scaled integer cdata
+%   JRI 9/1/06   should now handle all objects with cdata: images, surfaces, 
+%                scatterplots. (v 2.1)
+%   JRI 11/11/06 Preserves NaN colors. Hidden option (v 2.2, not uploaded)
+%   JRI 3/17/07  Preserve caxis after freezing--maintains colorbar scale (v 2.3)
+%   JRI 4/12/07  Check for painters mode as Matlab doesn't support rgb in it.
+%   JRI 4/9/08   Fix preserving caxis for objects within hggroups (e.g. contourf)
+%   JRI 4/7/10   Change documentation for colorbars
+
+% Hidden option for NaN colors:
+%   Missing data are often represented by NaN in the indexed color
+%   data, which renders transparently. This transparency will be preserved
+%   when freezing colors. If instead you wish such gaps to be filled with 
+%   a real color, add 'nancolor',[r g b] to the end of the arguments. E.g. 
+%   freezeColors('nancolor',[r g b]) or freezeColors(axh,'nancolor',[r g b]),
+%   where [r g b] is a color vector. This works on images & pcolor, but not on
+%   surfaces.
+%   Thanks to Fabiano Busdraghi and Jody Klymak for the suggestions. Bugfixes 
+%   attributed in the code.
+
+% Free for all uses, but please retain the following:
+%   Original Author:
+%   John Iversen, 2005-10
+%   john_iversen@post.harvard.edu
+
+appdatacode = 'JRI__freezeColorsData';
+
+[h, nancolor] = checkArgs(varargin);
+
+%gather all children with scaled or indexed CData
+cdatah = getCDataHandles(h);
+
+%current colormap
+cmap = colormap;
+nColors = size(cmap,1);
+cax = caxis;
+
+% convert object color indexes into colormap to true-color data using 
+%  current colormap
+for hh = cdatah',
+    g = get(hh);
+    
+    %preserve parent axis clim
+    parentAx = getParentAxes(hh);
+    originalClim = get(parentAx, 'clim');    
+   
+    %   Note: Special handling of patches: For some reason, setting
+    %   cdata on patches created by bar() yields an error,
+    %   so instead we'll set facevertexcdata instead for patches.
+    if ~strcmp(g.Type,'patch'),
+        cdata = g.CData;
+    else
+        cdata = g.FaceVertexCData; 
+    end
+    
+    %get cdata mapping (most objects (except scattergroup) have it)
+    if isfield(g,'CDataMapping'),
+        scalemode = g.CDataMapping;
+    else
+        scalemode = 'scaled';
+    end
+    
+    %save original indexed data for use with unfreezeColors
+    siz = size(cdata);
+    setappdata(hh, appdatacode, {cdata scalemode});
+
+    %convert cdata to indexes into colormap
+    if strcmp(scalemode,'scaled'),
+        %4/19/06 JRI, Accommodate scaled display of integer cdata:
+        %       in MATLAB, uint * double = uint, so must coerce cdata to double
+        %       Thanks to O Yamashita for pointing this need out
+        idx = ceil( (double(cdata) - cax(1)) / (cax(2)-cax(1)) * nColors);
+    else %direct mapping
+        idx = cdata;
+        %10/8/09 in case direct data is non-int (e.g. image;freezeColors)
+        % (Floor mimics how matlab converts data into colormap index.)
+        % Thanks to D Armyr for the catch
+        idx = floor(idx);
+    end
+    
+    %clamp to [1, nColors]
+    idx(idx<1) = 1;
+    idx(idx>nColors) = nColors;
+
+    %handle nans in idx
+    nanmask = isnan(idx);
+    idx(nanmask)=1; %temporarily replace w/ a valid colormap index
+
+    %make true-color data--using current colormap
+    realcolor = zeros(siz);
+    for i = 1:3,
+        c = cmap(idx,i);
+        c = reshape(c,siz);
+        c(nanmask) = nancolor(i); %restore Nan (or nancolor if specified)
+        realcolor(:,:,i) = c;
+    end
+    
+    %apply new true-color color data
+    
+    %true-color is not supported in painters renderer, so switch out of that
+    if strcmp(get(gcf,'renderer'), 'painters'),
+        set(gcf,'renderer','zbuffer');
+    end
+    
+    %replace original CData with true-color data
+    if ~strcmp(g.Type,'patch'),
+        set(hh,'CData',realcolor);
+    else
+        set(hh,'faceVertexCData',permute(realcolor,[1 3 2]))
+    end
+    
+    %restore clim (so colorbar will show correct limits)
+    if ~isempty(parentAx),
+        set(parentAx,'clim',originalClim)
+    end
+    
+end %loop on indexed-color objects
+
+
+% ============================================================================ %
+% Local functions
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% getCDataHandles -- get handles of all descendents with indexed CData
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function hout = getCDataHandles(h)
+% getCDataHandles  Find all objects with indexed CData
+
+%recursively descend object tree, finding objects with indexed CData
+% An exception: don't include children of objects that themselves have CData:
+%   for example, scattergroups are non-standard hggroups, with CData. Changing
+%   such a group's CData automatically changes the CData of its children, 
+%   (as well as the children's handles), so there's no need to act on them.
+
+error(nargchk(1,1,nargin,'struct'))
+
+hout = [];
+if isempty(h),return;end
+
+ch = get(h,'children');
+for hh = ch'
+    g = get(hh);
+    if isfield(g,'CData'),     %does object have CData?
+        %is it indexed/scaled?
+        if ~isempty(g.CData) && isnumeric(g.CData) && size(g.CData,3)==1, 
+            hout = [hout; hh]; %#ok<AGROW> %yes, add to list
+        end
+    else %no CData, see if object has any interesting children
+            hout = [hout; getCDataHandles(hh)]; %#ok<AGROW>
+    end
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% getParentAxes -- return handle of axes object to which a given object belongs
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+function hAx = getParentAxes(h)
+% getParentAxes  Return enclosing axes of a given object (could be self)
+
+error(nargchk(1,1,nargin,'struct'))
+%object itself may be an axis
+if strcmp(get(h,'type'),'axes'),
+    hAx = h;
+    return
+end
+
+parent = get(h,'parent');
+if (strcmp(get(parent,'type'), 'axes')),
+    hAx = parent;
+else
+    hAx = getParentAxes(parent);
+end
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%% checkArgs -- Validate input arguments
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+function [h, nancolor] = checkArgs(args)
+% checkArgs  Validate input arguments to freezeColors
+
+nargs = length(args);
+error(nargchk(0,3,nargs,'struct'))
+
+%grab handle from first argument if we have an odd number of arguments
+if mod(nargs,2),
+    h = args{1};
+    if ~ishandle(h),
+        error('JRI:freezeColors:checkArgs:invalidHandle',...
+            'The first argument must be a valid graphics handle (to an axis)')
+    end
+    % 4/2010 check if object to be frozen is a colorbar
+    if strcmp(get(h,'Tag'),'Colorbar'),
+      if ~exist('cbfreeze.m'),
+        warning('JRI:freezeColors:checkArgs:cannotFreezeColorbar',...
+            ['You seem to be attempting to freeze a colorbar. This no longer'...
+            'works. Please read the help for freezeColors for the solution.'])
+      else
+        cbfreeze(h);
+        return
+      end
+    end
+    args{1} = [];
+    nargs = nargs-1;
+else
+    h = gca;
+end
+
+%set nancolor if that option was specified
+nancolor = [nan nan nan];
+if nargs == 2,
+    if strcmpi(args{end-1},'nancolor'),
+        nancolor = args{end};
+        if ~all(size(nancolor)==[1 3]),
+            error('JRI:freezeColors:checkArgs:badColorArgument',...
+                'nancolor must be [r g b] vector');
+        end
+        nancolor(nancolor>1) = 1; nancolor(nancolor<0) = 0;
+    else
+        error('JRI:freezeColors:checkArgs:unrecognizedOption',...
+            'Unrecognized option (%s). Only ''nancolor'' is valid.',args{end-1})
+    end
+end
+
+
diff --git a/tracking/gaussianResponse.m b/tracking/gaussianResponse.m
new file mode 100644
index 0000000..b26204d
--- /dev/null
+++ b/tracking/gaussianResponse.m
@@ -0,0 +1,15 @@
+function y = gaussianResponse(rect_size, sigma)
+%GAUSSIANRESPONSE create the (fixed) target response of the correlation filter response
+    half = floor((rect_size-1) / 2);
+    i_range = -half(1):half(1);
+    j_range = -half(2):half(2);
+    [i, j] = ndgrid(i_range, j_range);
+    i_mod_range = mod_one(i_range, rect_size(1));
+    j_mod_range = mod_one(j_range, rect_size(2));
+    y = zeros(rect_size);
+    y(i_mod_range, j_mod_range) = exp(-(i.^2 + j.^2) / (2 * sigma^2));
+end
+
+function y = mod_one(a, b)
+    y = mod(a-1, b)+1;
+end
diff --git a/tracking/gaussian_correlation.m b/tracking/gaussian_correlation.m
new file mode 100644
index 0000000..9c7411b
--- /dev/null
+++ b/tracking/gaussian_correlation.m
@@ -0,0 +1,26 @@
+function kf = gaussian_correlation(xf, yf, sigma)
+%GAUSSIAN_CORRELATION Gaussian Kernel at all shifts, i.e. kernel correlation.
+%   Evaluates a Gaussian kernel with bandwidth SIGMA for all relative
+%   shifts between input images X and Y, which must both be MxN. They must 
+%   also be periodic (ie., pre-processed with a cosine window). The result
+%   is an MxN map of responses.
+%
+%   Inputs and output are all in the Fourier domain.
+%
+%   Joao F. Henriques, 2014
+%   http://www.isr.uc.pt/~henriques/
+	
+	N = size(xf,1) * size(xf,2);
+	xx = xf(:)' * xf(:) / N;  %squared norm of x
+	yy = yf(:)' * yf(:) / N;  %squared norm of y
+	
+	%cross-correlation term in Fourier domain
+	xyf = xf .* conj(yf);
+	xy = sum(real(ifft2(xyf)), 3);  %to spatial domain
+	
+	%calculate gaussian response for all positions, then go back to the
+	%Fourier domain
+	kf = fft2(exp(-1 / sigma^2 * max(0, (xx + yy - 2 * xy) / numel(xf))));
+
+end
+
diff --git a/tracking/gaussian_correlation_scale_single.m b/tracking/gaussian_correlation_scale_single.m
new file mode 100644
index 0000000..9ee189d
--- /dev/null
+++ b/tracking/gaussian_correlation_scale_single.m
@@ -0,0 +1,10 @@
+function kf = gaussian_correlation_scale_single(base, y, sigma)
+	
+    N = size(y,1) * size(y,2);%*size(y,3);
+    for i =1:size(base,2)
+        n = sum(sum(sum((base(:,i) - y).^2)))/N^2;
+        k(i) = exp(-n / sigma^2 ) ;
+    end
+    
+    kf = fft(k');
+end
diff --git a/tracking/getCenterLikelihood.m b/tracking/getCenterLikelihood.m
new file mode 100644
index 0000000..e273027
--- /dev/null
+++ b/tracking/getCenterLikelihood.m
@@ -0,0 +1,30 @@
+function center_likelihood = getCenterLikelihood(object_likelihood, m)
+%GETCENTERLIKELIHOOD computes the sum over rectangles of size M.
+% CENTER_LIKELIHOOD is the 'colour response'
+    [h,w] = size(object_likelihood);
+    n1 = h - m(1) + 1;
+    n2 = w - m(2) + 1;
+
+    %% integral images
+    % compute summed area table
+%     SAT = zeros(h, w);
+%     for y=1:h
+%         for x=1:w
+%             if x>1, SAT_left = SAT(y,x-1);
+%             else    SAT_left = 0; end
+%             if y>1, SAT_up = SAT(y-1,x);
+%             else    SAT_up = 0; end
+%             if y>1 && x>1, SAT_left_up = SAT(y-1,x-1);
+%             else    SAT_left_up = 0; end
+%
+%             SAT(y,x) = object_likelihood(y,x) + SAT_left + SAT_up - SAT_left_up;
+%         end
+%     end
+%     SAT = padarray(SAT,[1 1], 'pre');
+
+%% equivalent MATLAB function
+    SAT = integralImage(object_likelihood);
+    i = 1:n1;
+    j = 1:n2;
+    center_likelihood = (SAT(i,j) + SAT(i+m(1), j+m(2)) - SAT(i+m(1), j) - SAT(i, j+m(2))) / prod(m);
+end
diff --git a/tracking/getColourMap.m b/tracking/getColourMap.m
new file mode 100644
index 0000000..7380881
--- /dev/null
+++ b/tracking/getColourMap.m
@@ -0,0 +1,17 @@
+function [P_O] = getColourMap(patch, bg_hist, fg_hist, n_bins, grayscale_sequence)
+%% GETCOLOURMAP computes pixel-wise probabilities (PwP) given PATCH and models BG_HIST and FG_HIST
+    % check whether the patch has 3 channels
+    [h, w, d] = size(patch);
+    % figure out which bin each pixel falls into
+    bin_width = 256/n_bins;
+    % convert image to d channels array
+    patch_array = reshape(double(patch), w*h, d);
+    % to which bin each pixel (for all d channels) belongs to
+    bin_indices = floor(patch_array/bin_width) + 1;
+    % Get pixel-wise posteriors (PwP)
+    P_bg = getP(bg_hist, h, w, bin_indices, grayscale_sequence);
+    P_fg = getP(fg_hist, h, w, bin_indices, grayscale_sequence);
+
+    % Object-likelihood map
+    P_O = P_fg ./ (P_fg + P_bg);
+end
diff --git a/tracking/getDesiredResponse.m b/tracking/getDesiredResponse.m
new file mode 100644
index 0000000..94a5f02
--- /dev/null
+++ b/tracking/getDesiredResponse.m
@@ -0,0 +1,15 @@
+function [desiredResponse, x0, y0] = getDesiredResponse(saliency_map, bg_area, target_sz, p)
+% GETDESIREDRESPONSE is a wrapper for getGaussian, that computes the (object-centered) desired response
+
+% If the saliency patch is empty, use a patch centered desired response only for one frame
+if(sum(sum(saliency_map))==0)
+    p.gaussianType = 'bboxCentered';
+    [desiredResponse, ~, ~] = getGaussian(saliency_map, bg_area, target_sz, p);
+    x0 = 0;
+    y0 = 0;
+    p.gaussianType = 'objectCentered';
+else
+    [desiredResponse, x0, y0] = getGaussian(saliency_map, bg_area, target_sz, p);
+end
+
+end
\ No newline at end of file
diff --git a/tracking/getFeatureMap.m b/tracking/getFeatureMap.m
new file mode 100644
index 0000000..7cf9927
--- /dev/null
+++ b/tracking/getFeatureMap.m
@@ -0,0 +1,31 @@
+function out = getFeatureMap(im_patch, feature_type, cf_response_size, hog_cell_size)
+
+% code from DSST
+
+% allocate space
+switch feature_type
+    case 'fhog'
+        temp = fhog(single(im_patch), hog_cell_size);
+        h = cf_response_size(1);
+        w = cf_response_size(2);
+        out = zeros(h, w, 28, 'single');
+        out(:,:,2:28) = temp(:,:,1:27);
+        if hog_cell_size > 1
+            im_patch = mexResize(im_patch, [h, w] ,'auto');
+        end
+        % if color image
+        if size(im_patch, 3) > 1
+            im_patch = rgb2gray(im_patch);
+        end
+        out(:,:,1) = single(im_patch)/255 - 0.5;
+    case 'gray'
+        if hog_cell_size > 1, im_patch = mexResize(im_patch,cf_response_size,'auto');   end
+        if size(im_patch, 3) == 1
+            out = single(im_patch)/255 - 0.5;
+        else
+            out = single(rgb2gray(im_patch))/255 - 0.5;
+        end        
+end
+        
+end
+
diff --git a/tracking/getScalePatch.m b/tracking/getScalePatch.m
new file mode 100644
index 0000000..aec42a2
--- /dev/null
+++ b/tracking/getScalePatch.m
@@ -0,0 +1,18 @@
+function out = getScalePatch(img, pos, target_size, search_size, rotation, scale_model_sz, hog_scale_cell_size)
+
+% code from DSST
+padding=0;
+if size(img,3)>1
+img= single(rgb2gray(img))/255.0;
+end
+
+tmp_sz = floor((target_size * (1 + padding))*search_size);
+param0 = [pos(2), pos(1), tmp_sz(2)/target_size(2), rotation/180*pi,...
+            tmp_sz(1)/target_size(2)/(target_size(1)/target_size(2)),0];
+param0 = affparam2mat(param0); 
+patch = warpimg(double(img), param0, target_size);
+patch = fhog(single(patch), hog_scale_cell_size);
+patch = patch(:,:,1:31);
+out = patch(:);
+
+
diff --git a/tracking/getScaleSubwindow.m b/tracking/getScaleSubwindow.m
new file mode 100644
index 0000000..3113105
--- /dev/null
+++ b/tracking/getScaleSubwindow.m
@@ -0,0 +1,38 @@
+function out = getScaleSubwindow(im, pos, base_target_sz, scale_factors, scale_window, scale_model_sz, hog_scale_cell_size)
+
+% code from DSST
+
+num_scales = length(scale_factors);
+
+for s = 1:num_scales
+    patch_sz = floor(base_target_sz * scale_factors(s));
+    %make sure the size is not to small
+    patch_sz = max(patch_sz, 2);
+ 
+    xs = floor(pos(2)) + (1:patch_sz(2)) - floor(patch_sz(2)/2);
+    ys = floor(pos(1)) + (1:patch_sz(1)) - floor(patch_sz(1)/2);
+    
+    %check for out-of-bounds coordinates, and set them to the values at
+    %the borders
+    xs(xs < 1) = 1;
+    ys(ys < 1) = 1;
+    xs(xs > size(im,2)) = size(im,2);
+    ys(ys > size(im,1)) = size(im,1);
+    
+    %extract image
+    im_patch = im(ys, xs, :);
+    
+    % resize image to model size
+    im_patch_resized = mexResize(im_patch, scale_model_sz, 'auto');
+    
+    % extract scale features
+    temp_hog = fhog(single(im_patch_resized), hog_scale_cell_size);
+    temp = temp_hog(:,:,1:31);
+    
+    if s == 1
+        out = zeros(numel(temp), num_scales, 'single');
+    end
+    
+    % window
+    out(:,s) = temp(:) * scale_window(s); 
+end
diff --git a/tracking/getScaleSubwindow_v1.m b/tracking/getScaleSubwindow_v1.m
new file mode 100644
index 0000000..2153a7c
--- /dev/null
+++ b/tracking/getScaleSubwindow_v1.m
@@ -0,0 +1,25 @@
+function out = getScaleSubwindow_v1(img, pos, target_size, search_size, rotation, scale_model_sz, hog_scale_cell_size)
+
+% code from DSST
+padding=0;
+num_scales = length(search_size);
+if size(img,3)>1
+img= single(rgb2gray(img))/255.0;
+end
+for s = 1:num_scales
+    tmp_sz = floor((target_size * (1 + padding))*search_size(s));
+    param0 = [pos(2), pos(1), tmp_sz(2)/target_size(2), rotation/180*pi,...
+                tmp_sz(1)/target_size(2)/(target_size(1)/target_size(2)),0];
+    param0 = affparam2mat(param0); 
+    patch = warpimg(double(img), param0, target_size);
+    patch = fhog(single(patch), hog_scale_cell_size);
+    patch = patch(:,:,1:31);
+    
+    if s == 1
+        out = zeros(numel(patch), num_scales, 'single');
+    end
+    
+    % window
+    out(:,s) = patch(:);%* scale_window(s);
+end
+
diff --git a/tracking/getSubwindow.m b/tracking/getSubwindow.m
new file mode 100644
index 0000000..2fef312
--- /dev/null
+++ b/tracking/getSubwindow.m
@@ -0,0 +1,42 @@
+function im_patch = getSubwindow(im, pos, model_sz, scaled_sz)
+%GET_SUBWINDOW Obtain image sub-window, padding is done by replicating border values.
+%   Returns sub-window of image IM centered at POS ([y, x] coordinates),
+%   with size MODEL_SZ ([height, width]). If any pixels are outside of the image,
+%   they will replicate the values at the borders
+
+% with 3 input, no scale. With 4 params, scale adaptation
+if nargin < 4, sz = model_sz;   
+else, sz = scaled_sz;    
+end
+
+%make sure the size is not to small
+sz = max(sz, 2);
+%if sz(1) < 1, sz(1) = 2; end;
+%if sz(2) < 1, sz(2) = 2; end;
+
+%xs = floor(pos(2)) + (1:sz(2)) - floor(sz(2)/2);
+%ys = floor(pos(1)) + (1:sz(1)) - floor(sz(1)/2);
+xs = round(pos(2) + (1:sz(2)) - sz(2)/2);
+ys = round(pos(1) + (1:sz(1)) - sz(1)/2);
+
+%check for out-of-bounds coordinates, and set them to the values at
+%the borders
+xs(xs < 1) = 1;
+ys(ys < 1) = 1;
+xs(xs > size(im,2)) = size(im,2);
+ys(ys > size(im,1)) = size(im,1);
+
+%extract image
+im_patch_original = im(ys, xs, :);
+
+% (if rescaling is introduced) resize image to model size
+% im_patch = imresize(im_patch, model_sz, 'bilinear');
+if nargin>=4
+%     im_patch = mexResize(im_patch_original, model_sz, 'auto');
+    im_patch = mexResize(im_patch_original, model_sz, 'auto');
+else
+    im_patch = im_patch_original;
+end
+
+end
+
diff --git a/tracking/get_axis_aligned_BB.m b/tracking/get_axis_aligned_BB.m
new file mode 100644
index 0000000..7cc45a9
--- /dev/null
+++ b/tracking/get_axis_aligned_BB.m
@@ -0,0 +1,14 @@
+function [cx, cy, w, h] = get_axis_aligned_BB(region)
+% GETAXISALIGNEDBB extracts an axis aligned bbox from the ground truth REGION with same area as the rotated one
+    cx = mean(region(1:2:end));
+    cy = mean(region(2:2:end));
+    x1 = min(region(1:2:end));
+    x2 = max(region(1:2:end));
+    y1 = min(region(2:2:end));
+    y2 = max(region(2:2:end));
+    A1 = norm(region(1:2) - region(3:4)) * norm(region(3:4) - region(5:6));
+    A2 = (x2 - x1) * (y2 - y1);
+    s = sqrt(A1/A2);
+    w = s * (x2 - x1) + 1;
+    h = s * (y2 - y1) + 1;
+end
diff --git a/tracking/get_feature_map.m b/tracking/get_feature_map.m
new file mode 100644
index 0000000..baadc66
--- /dev/null
+++ b/tracking/get_feature_map.m
@@ -0,0 +1,61 @@
+function out = get_feature_map(im_patch, features, w2c)
+
+% out = get_feature_map(im_patch, features, w2c)
+%
+% Extracts the given features from the image patch. w2c is the
+% Color Names matrix, if used.
+
+if nargin < 3
+    w2c = [];
+end
+
+% the names of the features that can be used
+valid_features = {'gray', 'cn'};
+
+% the dimension of the valid features
+feature_levels = [1 10]';
+
+num_valid_features = length(valid_features);
+used_features = false(num_valid_features, 1);
+
+% get the used features
+for i = 1:num_valid_features
+    used_features(i) = any(strcmpi(valid_features{i}, features));
+end
+
+% total number of used feature levels
+num_feature_levels = sum(feature_levels .* used_features);
+
+level = 0;
+
+% If grayscale image
+if size(im_patch, 3) == 1
+    % Features that are available for grayscale sequances
+    
+    % Grayscale values (image intensity)
+    out = single(im_patch)/255 - 0.5;
+else
+    % Features that are available for color sequances
+    
+    % allocate space (for speed)
+    out = zeros(size(im_patch, 1), size(im_patch, 2), num_feature_levels, 'single');
+    
+    % Grayscale values (image intensity)
+    if used_features(1)
+        out(:,:,level+1) = single(rgb2gray(im_patch))/255 - 0.5;
+        level = level + feature_levels(1);
+    end
+    
+    % Color Names
+    if used_features(2)
+        if isempty(w2c)
+            % load the RGB to color name matrix if not in input
+            temp = load('w2crs');
+            w2c = temp.w2crs;
+        end
+        
+        % extract color descriptor
+        out(:,:,level+(1:10)) = im2c(single(im_patch), w2c, -2);
+        level = level + feature_levels(2);
+    end
+end
\ No newline at end of file
diff --git a/tracking/initializeAllAreas.m b/tracking/initializeAllAreas.m
new file mode 100644
index 0000000..95ecd6b
--- /dev/null
+++ b/tracking/initializeAllAreas.m
@@ -0,0 +1,40 @@
+function [params, bg_area, fg_area, area_resize_factor] = initializeAllAreas(im, params)
+
+	% we want a regular frame surrounding the object
+	avg_dim = sum(params.target_sz)/2;
+	% size from which we extract features
+	bg_area = round(params.target_sz + avg_dim);
+	% pick a "safe" region smaller than bbox to avoid mislabeling
+	fg_area = round(params.target_sz - avg_dim * params.inner_padding);
+	% saturate to image size
+	if(bg_area(2)>size(im,2)), bg_area(2)=size(im,2)-1; end
+	if(bg_area(1)>size(im,1)), bg_area(1)=size(im,1)-1; end
+	% make sure the differences are a multiple of 2 (makes things easier later in color histograms)
+	bg_area = bg_area - mod(bg_area - params.target_sz, 2);
+	fg_area = fg_area + mod(bg_area - fg_area, 2);
+
+	% Compute the rectangle with (or close to) params.fixedArea and
+	% same aspect ratio as the target bbox
+	area_resize_factor = sqrt(params.fixed_area/prod(bg_area));
+	params.norm_bg_area = round(bg_area * area_resize_factor);
+	% Correlation Filter (HOG) feature space
+	% It smaller that the norm bg area if HOG cell size is > 1
+	params.cf_response_size = floor(params.norm_bg_area / params.hog_cell_size);
+	% given the norm BG area, which is the corresponding target w and h?
+ 	norm_target_sz_w = 0.75*params.norm_bg_area(2) - 0.25*params.norm_bg_area(1);
+ 	norm_target_sz_h = 0.75*params.norm_bg_area(1) - 0.25*params.norm_bg_area(2);
+%    norm_target_sz_w = params.target_sz(2) * params.norm_bg_area(2) / bg_area(2);
+%	norm_target_sz_h = params.target_sz(1) * params.norm_bg_area(1) / bg_area(1);
+    params.norm_target_sz = round([norm_target_sz_h norm_target_sz_w]);
+	% distance (on one side) between target and bg area
+	norm_pad = floor((params.norm_bg_area - params.norm_target_sz) / 2);
+	radius = min(norm_pad);
+	% norm_delta_area is the number of rectangles that are considered.
+	% it is the "sampling space" and the dimension of the final merged resposne
+	% it is squared to not privilege any particular direction
+	params.norm_delta_area = (2*radius+1) * [1, 1];
+	% Rectangle in which the integral images are computed.
+	% Grid of rectangles ( each of size norm_target_sz) has size norm_delta_area.
+	params.norm_pwp_search_area = params.norm_target_sz + params.norm_delta_area - 1;
+
+end
diff --git a/tracking/isToolboxAvailable.m b/tracking/isToolboxAvailable.m
new file mode 100644
index 0000000..3c1913e
--- /dev/null
+++ b/tracking/isToolboxAvailable.m
@@ -0,0 +1,41 @@
+function result = isToolboxAvailable(toolboxName,action)
+%% check if an specific toolbox exists
+%
+% This small piece of code checks to find whether a specific toolbox is
+% installed on MATLAB.
+%
+% The input is the name of the toolbox as a string
+%
+% The output is 1 if the toolbox is found and 0 otherwise
+%
+% Example:
+%
+%   result = isToolboxAvailable('Image Processing Toolbox','warning')
+%
+% -----------------------------------------------
+% code by: Reza Ahmadzadeh (reza.ahmadzadeh@iit.it)
+% -----------------------------------------------
+%
+%% check the input arguments
+if nargin < 1 || nargin > 2
+    error('usage: isToolboxAvailable(toolboxName,action)');
+elseif nargin == 1
+    action = 'warning';
+end;
+
+%% Find the toolbox and give proper output
+v_=ver;
+[installedToolboxes{1:length(v_)}] = deal(v_.Name);
+result = all(ismember(toolboxName,installedToolboxes));
+switch action
+    case 'error'
+        % if you need to end the program you can use the following line of code
+        assert(result,['Error! ' toolboxName ' is not installed!']);
+    case 'warning'
+        % if you need to just give a warning you can use the following line of code
+        if ~result
+            warning([toolboxName ' is not installed!']);
+        end
+    otherwise
+end
+end
\ No newline at end of file
diff --git a/tracking/mergeResponses.m b/tracking/mergeResponses.m
new file mode 100644
index 0000000..dd343bd
--- /dev/null
+++ b/tracking/mergeResponses.m
@@ -0,0 +1,21 @@
+function [response] = mergeResponses(response_cf, response_pwp, alpha, merge_method)
+%MERGERESPONSES interpolates the two responses with the hyperparameter ALPHA
+    if strcmp(merge_method, 'const_factor')
+        response = (1 - alpha) * response_cf + alpha * response_pwp;
+    end
+
+    if strcmp(merge_method, 'fit_gaussian')
+        [~, cov2D_cf] = fitGaussian(response_cf);
+        [~, cov2D_pwp] = fitGaussian(response_pwp);
+        response_u = ones(size(response_cf));
+        [~, cov2D_u] = fitGaussian(response_u);
+        w_cf = 1 - sqrt(det(cov2D_cf)/det(cov2D_u));
+        w_pwp = 1 - sqrt(det(cov2D_pwp)/det(cov2D_u));
+        sum_cf_pwp = w_cf + w_pwp;
+        w_cf = w_cf / sum_cf_pwp;
+        w_pwp = w_pwp / sum_cf_pwp;
+
+        response = w_cf * response_cf + w_pwp * response_pwp;
+        fprintf('w_cf: %.3f    w_pwp: %.3f\n', w_cf, w_pwp);
+    end
+end
diff --git a/tracking/params.txt b/tracking/params.txt
new file mode 100644
index 0000000..888c792
--- /dev/null
+++ b/tracking/params.txt
@@ -0,0 +1,36 @@
+% configurable params (as in CVPR paper)
+params.hog_cell_size = 4;
+params.fixed_area = 150^2;           % standard area to which we resize the target
+params.n_bins = 2^5;                            % number of bins for the color histograms (bg and fg models)
+params.learning_rate_pwp = 0.04;           % bg and fg color models learning rate 
+params.inner_padding = 0.2;             % defines inner area used to sample colors from the foreground
+params.output_sigma_factor = 1/16 ;             % standard deviation for the desired translation filter output
+params.lambda = 1e-3;      
+params.lambda_scale = 1e-3;                              % regularization weight
+params.learning_rate_cf = 0.01;            % HOG model learning rate
+params.merge_factor = 0.3;              % fixed interpolation factor - how to linearly combine the two responses
+params.learning_rate_scale = 0.01  %0.025;
+params.scale_sigma_factor = 1/16;
+params.scale_sigma = 0.1;
+params.num_scales = 33;
+params.scale_step = 1.02;
+
+params.num_rotation = 1;
+params.rotation_step = 1;
+params.rotation_sigma = 0.1;
+
+
+% fixed setup
+params.hog_scale_cell_size = 4;                % Default DSST=4
+params.scale_model_factor = 1.0;
+params.scale_model_max_area = 32*16;
+params.feature_type = 'fhog';
+params.scale_adaptation = true;
+params.rotation_adaptation = 0;
+params.grayscale_sequence = false;	% suppose that sequence is colour
+params.merge_method = 'const_factor';
+params.den_per_channel = false;
+
+%% debugging stuff
+params.visualization = 0;                               % show output bbox on frame
+params.visualization_dbg = 0;                           % show also segmentation, desired response and filter output
diff --git a/tracking/precision_plot.m b/tracking/precision_plot.m
new file mode 100644
index 0000000..63ec857
--- /dev/null
+++ b/tracking/precision_plot.m
@@ -0,0 +1,46 @@
+function precisions = precision_plot(positions, ground_truth, title, show)
+%PRECISION_PLOT
+%   Calculates precision for a series of distance thresholds (percentage of
+%   frames where the distance to the ground truth is within the threshold).
+%   The results are shown in a new figure if SHOW is true.
+%
+%   Accepts positions and ground truth as Nx2 matrices (for N frames), and
+%   a title string.
+%
+%   Joao F. Henriques, 2014
+%   http://www.isr.uc.pt/~henriques/
+
+	
+	max_threshold = 50;  %used for graphs in the paper
+	
+	
+	precisions = zeros(max_threshold, 1);
+	
+	if size(positions,1) ~= size(ground_truth,1),
+% 		fprintf('%12s - Number of ground truth frames does not match number of tracked frames.\n', title)
+		
+		%just ignore any extra frames, in either results or ground truth
+		n = min(size(positions,1), size(ground_truth,1));
+		positions(n+1:end,:) = [];
+		ground_truth(n+1:end,:) = [];
+	end
+	
+	%calculate distances to ground truth over all frames
+	distances = sqrt((positions(:,1) - ground_truth(:,1)).^2 + ...
+				 	 (positions(:,2) - ground_truth(:,2)).^2);
+	distances(isnan(distances)) = [];
+
+	%compute precisions
+	for p = 1:max_threshold,
+		precisions(p) = nnz(distances <= p) / numel(distances);
+	end
+	
+	%plot the precisions
+	if show == 1,
+		figure('Number','off', 'Name',['Precisions - ' title])
+		plot(precisions, 'k-', 'LineWidth',2)
+		xlabel('Threshold'), ylabel('Precision')
+	end
+	
+end
+
diff --git a/tracking/readParams.m b/tracking/readParams.m
new file mode 100644
index 0000000..8387b36
--- /dev/null
+++ b/tracking/readParams.m
@@ -0,0 +1,26 @@
+function params = readParams(params_file_name, hp_name, hp_value)
+% read params.txt and convert into struct
+	fparams = fopen(params_file_name);
+	C = textscan(fparams, '%s', 'Delimiter', '', 'CommentStyle', '%');
+	fclose(fparams);
+	% feed the lines one by one into eval to create the variables in the MATLAB workspace:
+	cellfun(@evalc, C{1}, 'UniformOutput', false);
+	% clear all the variables not in the params file
+	clear C fparams
+	% save current workspace in a struct
+	this_workspace = evalin('caller','who');
+	% save everything in a struct
+	for i=1:size(this_workspace,1)
+	    thisvar=evalin('caller', this_workspace{i});
+	    params.(this_workspace{i})=thisvar;
+	end
+
+	if(isfield(params,'ans'))
+	    rmfield(params,'ans');
+    end
+
+    if nargin>1
+        % if hyperparam is set: override default parameters with specified one
+        params.(hp_name) = hp_value;
+    end
+end
\ No newline at end of file
diff --git a/tracking/run_KCC.m b/tracking/run_KCC.m
new file mode 100644
index 0000000..a87068f
--- /dev/null
+++ b/tracking/run_KCC.m
@@ -0,0 +1,41 @@
+function results = run_KCC(seq, res_path, bSaveImage)
+% Entry point for the Wu - CVPR2013 benchmark
+
+    %% Read params.txt
+    params = readParams('params.txt');
+    params.img_files = seq.s_frames;
+    params.img_path = '';
+
+    im = imread(params.img_files{1});
+    % grayscale sequence? --> use 1D instead of 3D histograms
+    if(size(im,3)==1)
+        params.grayscale_sequence = true;
+    end
+
+    region = seq.init_rect;
+
+    if(numel(region)==8)
+        % polygon format (VOT14, VOT15)
+        [cx, cy, w, h] = getAxisAlignedBB(region);
+    else % rectangle format (WuCVPR13)
+        x = region(1);
+        y = region(2);
+        w = region(3);
+        h = region(4);
+        cx = x+w/2;
+        cy = y+h/2;
+    end
+
+    % init_pos is the centre of the initial bounding box
+    params.init_pos = [cy cx];
+    params.target_sz = round([h w]);
+
+    [params, bg_area, fg_area, area_resize_factor] = initializeAllAreas(im, params);
+
+	% in runTracker we do not output anything because it is just for debug
+	params.fout = -1;
+
+	% start the actual tracking
+	results = trackerMain_otb_wangchen(params, im, bg_area, fg_area, area_resize_factor);
+    fclose('all');
+end
\ No newline at end of file
diff --git a/tracking/trackerMain_otb_wangchen.m b/tracking/trackerMain_otb_wangchen.m
new file mode 100644
index 0000000..ddae5fd
--- /dev/null
+++ b/tracking/trackerMain_otb_wangchen.m
@@ -0,0 +1,276 @@
+function [results] = trackerMain_otb_wangchen(p, im, bg_area, fg_area, area_resize_factor)
+%TRACKERMAIN contains the main loop of the tracker, P contains all the parameters set in runTracker
+    %% INITIALIZATION
+   num_frames = numel(p.img_files);
+    % used for OTB-13 benchmark
+    OTB_rect_positions = zeros(num_frames, 4);
+	pos = p.init_pos;
+    target_sz = p.target_sz;
+	num_frames = numel(p.img_files);
+    % patch of the target + padding
+    patch_padded = getSubwindow(im, pos, p.norm_bg_area, bg_area);
+    % initialize hist model
+    new_pwp_model = true;
+    [bg_hist, fg_hist] = updateHistModel(new_pwp_model, patch_padded, bg_area, fg_area, target_sz, p.norm_bg_area, p.n_bins, p.grayscale_sequence);
+    new_pwp_model = false;
+    % Hann (cosine) window
+    if isToolboxAvailable('Signal Processing Toolbox')
+        hann_window = single(hann(p.cf_response_size(1)) * hann(p.cf_response_size(2))');
+    else
+        hann_window = single(myHann(p.cf_response_size(1)) * myHann(p.cf_response_size(2))');
+    end
+    % gaussian-shaped desired response, centred in (1,1)
+    % bandwidth proportional to target size
+    output_sigma = sqrt(prod(p.norm_target_sz)) * p.output_sigma_factor / p.hog_cell_size;
+    y = gaussianResponse(p.cf_response_size, output_sigma);
+    yf = fft2(y);
+    %% SCALE ADAPTATION INITIALIZATION
+    if p.scale_adaptation
+        % Code from DSST
+        scale_factor = 1;
+        base_target_sz = target_sz;
+        scale_sigma = sqrt(p.num_scales) * p.scale_sigma_factor;
+        ss = (1:p.num_scales) - ceil(p.num_scales/2);
+        
+        rotation_factor = 0;
+        ys=zeros(size(ss));
+        ys(1)=1;
+        ysf = (fft(ys));
+        
+        yr = zeros(p.num_rotation,1);
+        yr(1) = 1;
+        yrf = fft(yr);
+        rotation_factors = p.rotation_step*(-floor(p.num_rotation/2):floor(p.num_rotation/2));
+
+
+        if mod(p.num_scales,2) == 0
+            scale_window = single(hann(p.num_scales+1));
+            scale_window = scale_window(2:end);
+        else
+            scale_window = single(hann(p.num_scales));
+        end;
+
+        ss = 1:p.num_scales;
+        scale_factors = p.scale_step.^(ceil(p.num_scales/2) - ss);
+
+        if p.scale_model_factor^2 * prod(p.norm_target_sz) > p.scale_model_max_area
+            p.scale_model_factor = sqrt(p.scale_model_max_area/prod(p.norm_target_sz));
+        end
+
+        scale_model_sz = floor(p.norm_target_sz * p.scale_model_factor);
+        % find maximum and minimum scales
+        min_scale_factor = p.scale_step ^ ceil(log(max(5 ./ bg_area)) / log(p.scale_step));
+        max_scale_factor = p.scale_step ^ floor(log(min([size(im,1) size(im,2)] ./ target_sz)) / log(p.scale_step));
+    end
+
+    %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%
+    t_imread = 0;
+    %% MAIN LOOP
+    tic;
+    for frame = 1:num_frames
+         
+        if frame>1
+            tic_imread = tic;
+            im = imread([p.img_path p.img_files{frame}]);
+            t_imread = t_imread + toc(tic_imread);
+	    %% TESTING step
+            % extract patch of size bg_area and resize to norm_bg_area
+            im_patch_cf = getSubwindow(im, pos, p.norm_bg_area, bg_area);
+            pwp_search_area = round(p.norm_pwp_search_area / area_resize_factor);
+            % extract patch of size pwp_search_area and resize to norm_pwp_search_area
+            im_patch_pwp = getSubwindow(im, pos, p.norm_pwp_search_area, pwp_search_area);
+            % compute feature map
+            xt = getFeatureMap(im_patch_cf, p.feature_type, p.cf_response_size, p.hog_cell_size);
+            % apply Hann window
+            xt_windowed = bsxfun(@times, hann_window, xt);
+            % compute FFT
+            xtf = fft2(xt_windowed);
+            % Correlation between filter and test patch gives the response
+            % Solve diagonal system per pixel.
+            if p.den_per_channel
+                hf = hf_num ./ (hf_den + p.lambda);
+            else
+                hf = bsxfun(@rdivide, hf_num, sum(hf_den, 3)+p.lambda);
+            end
+            response_cf = ensure_real(ifft2(sum(conj(hf) .* xtf, 3)));
+
+            % Crop square search region (in feature pixels).
+            response_cf = cropFilterResponse(response_cf, ...
+                floor_odd(p.norm_delta_area / p.hog_cell_size));
+            if p.hog_cell_size > 1
+                % Scale up to match center likelihood resolution.
+                response_cf = mexResize(response_cf, p.norm_delta_area,'auto');
+            end
+
+            [likelihood_map] = getColourMap(im_patch_pwp, bg_hist, fg_hist, p.n_bins, p.grayscale_sequence);
+            % (TODO) in theory it should be at 0.5 (unseen colors shoud have max entropy)
+            likelihood_map(isnan(likelihood_map)) = 0;
+
+            % each pixel of response_pwp loosely represents the likelihood that
+            % the target (of size norm_target_sz) is centred on it
+            response_pwp = getCenterLikelihood(likelihood_map, p.norm_target_sz);
+
+            %% ESTIMATION
+            response = mergeResponses(response_cf, response_pwp, p.merge_factor, p.merge_method);
+            [row, col] = find(response == max(response(:)), 1);
+            center = (1+p.norm_delta_area) / 2;
+            pos = pos + ([row, col] - center) / area_resize_factor;
+            rect_position = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])];
+
+            %% SCALE SPACE SEARCH
+            if p.scale_adaptation
+                current_patch = getScalePatch(im, pos, base_target_sz,  scale_factor, -rotation_factor, scale_model_sz, p.hog_scale_cell_size);
+                ksf = gaussian_correlation_scale_single(im_patch_scale, current_patch, p.scale_sigma);                    
+                scale_response = abs(ifft((model_hsf.*ksf)));
+                [max_value, recovered_scale] = max(scale_response(:));
+                
+                %set the scale
+                scale_factor = scale_factor / scale_factors(recovered_scale);
+
+                if scale_factor < min_scale_factor
+                    scale_factor = min_scale_factor;
+                elseif scale_factor > max_scale_factor
+                    scale_factor = max_scale_factor;
+                end
+                % use new scale to update bboxes for target, filter, bg and fg models
+                target_sz = round(base_target_sz * scale_factor);
+                avg_dim = sum(target_sz)/2;
+                bg_area = round(target_sz + avg_dim);
+                if(bg_area(2)>size(im,2)),  bg_area(2)=size(im,2)-1;    end
+                if(bg_area(1)>size(im,1)),  bg_area(1)=size(im,1)-1;    end
+
+                bg_area = bg_area - mod(bg_area - target_sz, 2);
+                fg_area = round(target_sz - avg_dim * p.inner_padding);
+                fg_area = fg_area + mod(bg_area - fg_area, 2);
+                % Compute the rectangle with (or close to) params.fixed_area and
+                % same aspect ratio as the target bboxgetScaleSubwindow_v1
+                area_resize_factor = sqrt(p.fixed_area/prod(bg_area));
+            end
+            
+            %% ROTATION SPACE SEARCH
+            if p.rotation_adaptation
+                current_rotation_patch = getRotationPatch(im, pos, base_target_sz,  scale_factor, -rotation_factor, p.hog_scale_cell_size);
+                krf = gaussian_correlation_scale_single(im_patch_rotation, current_rotation_patch, p.rotation_sigma);                    
+                rotation_response = abs(ifft((model_hrf.*krf)));
+                [max_value, recovered_rotation] = max(rotation_response(:));
+                
+                %set the scale
+                rotation_factor = rotation_factor + rotation_factors(recovered_rotation);
+                fprintf('frame %d: current sclae factor is %.4f; current rotation %.4f:\n', frame, scale_factor, rotation_factor)
+                
+                
+                if rotation_factor <= -180
+                    rotation_factor = rotation_factor + 360;
+                elseif rotation_factor > 180
+                    rotation_factor = rotation_factor - 360;
+                end
+            end
+
+            if p.visualization_dbg==1
+                mySubplot(2,1,5,1,im_patch_cf,'FG+BG','gray');
+                mySubplot(2,1,5,2,likelihood_map,'obj.likelihood','parula');
+                mySubplot(2,1,5,3,response_cf,'CF response','parula');
+                mySubplot(2,1,5,4,response_pwp,'center likelihood','parula');
+                mySubplot(2,1,5,5,response,'merged response','parula');
+                drawnow
+            end
+        end
+
+        %% TRAINING
+        % extract patch of size bg_area and resize to norm_bg_area
+        im_patch_bg = getSubwindow(im, pos, p.norm_bg_area, bg_area);
+        % compute feature map, of cf_response_size
+        xt = getFeatureMap(im_patch_bg, p.feature_type, p.cf_response_size, p.hog_cell_size);
+        % apply Hann window
+        xt = bsxfun(@times, hann_window, xt);
+        % compute FFT
+        xtf = fft2(xt);
+        %% FILTER UPDATE
+        % Compute expectations over circular shifts,
+        % therefore divide by number of pixels.
+		new_hf_num = bsxfun(@times, conj(yf), xtf) / prod(p.cf_response_size);
+		new_hf_den = (conj(xtf) .* xtf) / prod(p.cf_response_size);
+
+        if frame == 1
+            % first frame, train with a single image
+		    hf_den = new_hf_den;
+		    hf_num = new_hf_num;
+		else
+		    % subsequent frames, update the model by linear interpolation
+        	hf_den = (1 - p.learning_rate_cf) * hf_den + p.learning_rate_cf * new_hf_den;
+	   	 	hf_num = (1 - p.learning_rate_cf) * hf_num + p.learning_rate_cf * new_hf_num;
+
+            % BG/FG MODEL UPDATE
+            % patch of the target + padding
+            [bg_hist, fg_hist] = updateHistModel(new_pwp_model, im_patch_bg, bg_area, fg_area, target_sz, p.norm_bg_area, p.n_bins, p.grayscale_sequence, bg_hist, fg_hist, p.learning_rate_pwp);
+        end
+
+        %% SCALE UPDATE
+        if p.scale_adaptation
+            im_patch_scale_frame = getScaleSubwindow_v1(im, pos, base_target_sz, scale_factors*scale_factor, -rotation_factor, scale_model_sz, p.hog_scale_cell_size);
+
+            if frame == 1
+                im_patch_scale=im_patch_scale_frame;
+            else
+                im_patch_scale=(1 - p.learning_rate_scale)*im_patch_scale + p.learning_rate_scale*im_patch_scale_frame;
+            end
+            
+            ksf = gaussian_correlation_scale_single(im_patch_scale, im_patch_scale(:,uint8(size(im_patch_scale,2)/2)), p.scale_sigma);
+            model_hsf = ysf'./(ksf+0.1);
+        end
+        
+                %% ROTATION UPDATE
+        if p.rotation_adaptation
+            im_patch_rotation_frame = getRotationSubwindow(im, pos, base_target_sz, scale_factor, rotation_factor + rotation_factors, p.hog_scale_cell_size);
+
+            if frame == 1
+                im_patch_rotation=im_patch_rotation_frame;
+            else
+                im_patch_rotation=(1 - p.learning_rate_scale)*im_patch_rotation + p.learning_rate_scale*im_patch_rotation_frame;
+            end
+            
+            krf = gaussian_correlation_scale_single(im_patch_rotation, im_patch_rotation(:,uint8(size(im_patch_rotation,2)/2)), p.rotation_sigma);
+            model_hrf = yrf./(krf+0.1);
+        end
+
+        %% update bbox position
+        if frame==1, rect_position = [pos([2,1]) - target_sz([2,1])/2, target_sz([2,1])]; end
+
+        rect_position_padded = [pos([2,1]) - bg_area([2,1])/2, bg_area([2,1])];
+
+        OTB_rect_positions(frame,:) = rect_position;
+
+        if p.fout > 0,  fprintf(p.fout,'%.2f,%.2f,%.2f,%.2f\n', rect_position(1),rect_position(2),rect_position(3),rect_position(4));   end
+
+        %% VISUALIZATION
+        if p.visualization == 1
+           
+                figure(1)
+                imshow(im)
+                rectangle('Position',rect_position, 'LineWidth',2, 'EdgeColor','g');
+                rectangle('Position',rect_position_padded, 'LineWidth',2, 'LineStyle','--', 'EdgeColor','b');
+                drawnow
+           
+        end
+    end
+    elapsed_time = toc;
+    % save data for OTB-13 benchmark
+    results.type = 'rect';
+    results.res = OTB_rect_positions;
+    results.fps = num_frames/(elapsed_time - t_imread);
+end
+
+% Reimplementation of Hann window (in case signal processing toolbox is missing)
+function H = myHann(X)
+    H = .5*(1 - cos(2*pi*(0:X-1)'/(X-1)));
+end
+
+% We want odd regions so that the central pixel can be exact
+function y = floor_odd(x)
+    y = 2*floor((x-1) / 2) + 1;
+end
+
+function y = ensure_real(x)
+    assert(norm(imag(x(:))) <= 1e-5 * norm(real(x(:))));
+    y = real(x);
+end
\ No newline at end of file
diff --git a/tracking/unfreezeColors.m b/tracking/unfreezeColors.m
new file mode 100644
index 0000000..0334f4f
--- /dev/null
+++ b/tracking/unfreezeColors.m
@@ -0,0 +1,106 @@
+function unfreezeColors(h)
+% unfreezeColors  Restore colors of a plot to original indexed color. (v2.3)
+%
+%   Useful if you want to apply a new colormap to plots whose
+%       colors were previously frozen with freezeColors.
+%
+%   Usage:
+%       unfreezeColors          unfreezes all objects in current axis, 
+%       unfreezeColors(axh)     same, but works on axis axh. axh can be vector.
+%       unfreezeColors(figh)    same, but for all objects in figure figh.
+%
+%       Has no effect on objects on which freezeColors was not already called.
+%				(Note: if colorbars were frozen using cbfreeze, use cbfreeze('off') to 
+%       unfreeze them. See freezeColors for information on cbfreeze.)
+%
+%
+%   See also freezeColors, freezeColors_pub.html, cbfreeze.
+%
+%
+%   John Iversen (iversen@nsi.edu) 3/23/05
+%
+
+%   Changes:
+%   JRI 9/1/06 now restores any object with frozen CData;
+%              can unfreeze an entire figure at once.
+%   JRI 4/7/10 Change documentation for colorbars
+
+% Free for all uses, but please retain the following:
+%
+%   Original Author:
+%   John Iversen, 2005-10
+%   john_iversen@post.harvard.edu
+
+error(nargchk(0,1,nargin,'struct'))
+
+appdatacode = 'JRI__freezeColorsData';
+
+%default: operate on gca
+if nargin < 1,
+    h = gca;
+end
+
+if ~ishandle(h),
+     error('JRI:unfreezeColors:invalidHandle',...
+            'The argument must be a valid graphics handle to a figure or axis')
+end
+
+%if h is a figure, loop on its axes
+if strcmp(get(h,'type'),'figure'),
+    h = get(h,'children');
+end
+
+for h1 = h', %loop on axes
+
+    %process all children, acting only on those with saved CData
+    %   ( in appdata JRI__freezeColorsData)
+    ch = findobj(h1);
+    
+    for hh = ch',
+        
+        %some object handles may be invalidated when their parent changes 
+        %   (e.g. restoring colors of a scattergroup unfortunately changes
+        %   the handles of all its children). So, first check to make sure
+        %   it's a valid handle
+        if ishandle(hh)
+            if isappdata(hh,appdatacode),
+                ad = getappdata(hh,appdatacode);
+                %get oroginal cdata
+                %patches have to be handled separately (see note in freezeColors)
+                if ~strcmp(get(hh,'type'),'patch'),
+                    cdata = get(hh,'CData');
+                else
+                    cdata = get(hh,'faceVertexCData');
+                    cdata = permute(cdata,[1 3 2]);
+                end
+                indexed = ad{1};
+                scalemode = ad{2};
+                
+                %size consistency check
+                if all(size(indexed) == size(cdata(:,:,1))),
+                    %ok, restore indexed cdata
+                    if ~strcmp(get(hh,'type'),'patch'),
+                        set(hh,'CData',indexed);
+                    else
+                        set(hh,'faceVertexCData',indexed);
+                    end
+                    %restore cdatamapping, if needed
+                    g = get(hh);
+                    if isfield(g,'CDataMapping'),
+                        set(hh,'CDataMapping',scalemode);
+                    end
+                    %clear appdata
+                    rmappdata(hh,appdatacode)
+                else
+                    warning('JRI:unfreezeColors:internalCdataInconsistency',...
+                        ['Could not restore indexed data: it is the wrong size. ' ...
+                        'Were the axis contents changed since the call to freezeColors?'])
+                end
+                
+            end %test if has our appdata
+        end %test ishandle
+
+    end %loop on children
+
+end %loop on axes
+
diff --git a/tracking/updateHistModel.m b/tracking/updateHistModel.m
new file mode 100644
index 0000000..dfee53c
--- /dev/null
+++ b/tracking/updateHistModel.m
@@ -0,0 +1,32 @@
+function [bg_hist_new, fg_hist_new] = updateHistModel(new_model, patch, bg_area, fg_area, target_sz, norm_area, n_bins, grayscale_sequence, bg_hist, fg_hist, learning_rate_pwp)
+%UPDATEHISTMODEL create new models for foreground and background or update the current ones
+
+	% Get BG (frame around target_sz) and FG masks (inner portion of target_sz)
+	pad_offset1 = (bg_area-target_sz)/2; % we constrained the difference to be mod2, so we do not have to round here
+% 	assert(sum(pad_offset1==round(pad_offset1))==2, 'difference between bg_area and target_sz has to be even.');
+
+	bg_mask = true(bg_area); % init bg_mask
+	pad_offset1(pad_offset1<=0)=1;
+	bg_mask(pad_offset1(1)+1:end-pad_offset1(1), pad_offset1(2)+1:end-pad_offset1(2)) = false;
+
+	pad_offset2 = (bg_area-fg_area)/2; % we constrained the difference to be mod2, so we do not have to round here
+% 	assert(sum(pad_offset2==round(pad_offset2))==2, 'difference between bg_area and fg_area has to be even.');
+	fg_mask = false(bg_area); % init fg_mask
+	pad_offset2(pad_offset2<=0)=1;
+	fg_mask(pad_offset2(1)+1:end-pad_offset2(1), pad_offset2(2)+1:end-pad_offset2(2)) = true;
+
+	fg_mask = mexResize(fg_mask, norm_area, 'auto');
+	bg_mask = mexResize(bg_mask, norm_area, 'auto');
+
+	%% (TRAIN) BUILD THE MODEL
+	if new_model
+		% from scratch (frame=1)
+		bg_hist_new = computeHistogram(patch, bg_mask, n_bins, grayscale_sequence);
+		fg_hist_new = computeHistogram(patch, fg_mask, n_bins, grayscale_sequence);
+	else
+		% update the model
+		bg_hist_new = (1 - learning_rate_pwp)*bg_hist + learning_rate_pwp*computeHistogram(patch, bg_mask, n_bins, grayscale_sequence);
+		fg_hist_new = (1 - learning_rate_pwp)*fg_hist + learning_rate_pwp*computeHistogram(patch, fg_mask, n_bins, grayscale_sequence);
+	end
+
+end
\ No newline at end of file
diff --git a/tracking/utility/affparam2geom.m b/tracking/utility/affparam2geom.m
new file mode 100644
index 0000000..bef3e6d
--- /dev/null
+++ b/tracking/utility/affparam2geom.m
@@ -0,0 +1,39 @@
+function q = affparam2geom(p)
+% function q = affparam2geom(p)
+%
+% The functions affparam2geom and affparam2mat convert a 'geometric'
+% affine parameter to/from a matrix form (2x3 matrix).
+% 
+% affparam2geom converts a 2x3 matrix to 6 affine parameters
+% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse.
+%
+%    p(6) : [p(1) p(3) p(4); p(2) p(5) p(6)]
+%    q(6) : [dx dy sc th sr phi]
+%
+% Reference "Multiple View Geometry in Computer Vision" by Richard
+% Hartley and Andrew Zisserman. 
+
+% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.
+
+A = [ p(3), p(4); p(5), p(6) ];
+%%  A = USVt = (UVt)(VSVt) = R(th)R(-phi)SR(phi)
+[U,S,V] = svd(A);
+if (det(U) < 0)
+  U = U(:,2:-1:1);  V = V(:,2:-1:1);  S = S(2:-1:1,2:-1:1);
+end
+q(1) = p(1);  q(2) = p(2);
+q(4) = atan2(U(2,1)*V(1,1)+U(2,2)*V(1,2), U(1,1)*V(1,1)+U(1,2)*V(1,2));
+
+phi = atan2(V(1,2),V(1,1));
+if (phi <= -pi/2)
+  c = cos(-pi/2); s = sin(-pi/2);
+  R = [c -s; s c];  V = V * R;  S = R'*S*R;
+end
+if (phi >= pi/2)
+  c = cos(pi/2); s = sin(pi/2);
+  R = [c -s; s c];  V = V * R;  S = R'*S*R;
+end
+q(3) = S(1,1);
+q(5) = S(2,2)/S(1,1);
+q(6) = atan2(V(1,2),V(1,1));
+q = reshape(q, size(p));
diff --git a/tracking/utility/affparam2mat.m b/tracking/utility/affparam2mat.m
new file mode 100644
index 0000000..e316da6
--- /dev/null
+++ b/tracking/utility/affparam2mat.m
@@ -0,0 +1,30 @@
+function q = affparam2mat(p)
+% function q = affparam2mat(p)
+%
+% The functions affparam2geom and affparam2mat convert a 'geometric'
+% affine parameter to/from a matrix form (2x3 matrix).
+% 
+% affparam2geom converts a 2x3 matrix to 6 affine parameters
+% (x, y, th, scale, aspect, skew), and affparam2mat does the inverse.
+%
+%    p(6,n) : [dx dy sc th sr phi]'
+%    q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)]
+%
+% Reference "Multiple View Geometry in Computer Vision" by Richard
+% Hartley and Andrew Zisserman. 
+
+% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.
+
+
+sz = size(p);
+if (length(p(:)) == 6)
+  p = p(:);
+end
+s = p(3,:);  th = p(4,:);  r = p(5,:);  phi = p(6,:);
+cth = cos(th);  sth = sin(th);  cph = cos(phi);  sph = sin(phi);
+ccc = cth.*cph.*cph;  ccs = cth.*cph.*sph;  css = cth.*sph.*sph;
+scc = sth.*cph.*cph;  scs = sth.*cph.*sph;  sss = sth.*sph.*sph;
+q(1,:) = p(1,:);  q(2,:) = p(2,:);
+q(3,:) = s.*(ccc +scs +r.*(css -scs));  q(4,:) = s.*(r.*(ccs -scc) -ccs -sss);
+q(5,:) = s.*(scc -ccs +r.*(ccs +sss));  q(6,:) = s.*(r.*(ccc +scs) -scs +css);
+q = reshape(q, sz);
diff --git a/tracking/utility/affparaminv.m b/tracking/utility/affparaminv.m
new file mode 100644
index 0000000..ef86a12
--- /dev/null
+++ b/tracking/utility/affparaminv.m
@@ -0,0 +1,17 @@
+function q = affparaminv(p,q)
+% function q = affparaminv(p[, q])
+%
+%    p(6,n) : [dx dy sc th sr phi]'
+%    q(6,n) : [q(1) q(3) q(4); q(2) q(5) q(6)]
+
+% Copyright (C) Jongwoo Lim and David Ross.  All rights reserved.
+
+if (length(p) == 6)
+  p = p(:);
+end
+if (nargin > 1)
+  q = inv([p(3) p(4); p(5) p(6)]) * [q(1)-p(1) q(3:4); q(2)-p(2) q(5:6)];
+else
+  q = inv([p(3) p(4); p(5) p(6)]) * [-p(1) 1 0; -p(2) 0 1];
+end
+q = q([1,2,3,5,4,6]);
diff --git a/tracking/utility/affwarpimg.m b/tracking/utility/affwarpimg.m
new file mode 100644
index 0000000..df6a8e3
--- /dev/null
+++ b/tracking/utility/affwarpimg.m
@@ -0,0 +1,24 @@
+function wimg = affwarpimg(img, p, sz)
+% function wimg = affwarpimg(img, p, sz)
+%
+%    img(h,w)
+%    p(6,n) : mat format affine parameter
+%    sz(th,tw)
+%
+
+%% Copyright (C) Jongwoo Lim and David Ross.
+%% All rights reserved.
+
+
+if (nargin < 3)
+    sz = size(img);
+end
+if (size(p,1) == 1)
+    p = p(:);
+end
+w = sz(2);  h = sz(1);  n = size(p,2);
+[x,y] = meshgrid(1:w, 1:h);
+pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ...
+              * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]);
+wimg = squeeze(interp2(img, pos(:,:,:,1), pos(:,:,:,2)));
+wimg(find(isnan(wimg))) = 0;
diff --git a/tracking/utility/box_to_p.m b/tracking/utility/box_to_p.m
new file mode 100644
index 0000000..cfcee01
--- /dev/null
+++ b/tracking/utility/box_to_p.m
@@ -0,0 +1,39 @@
+function [ p ] = box_to_p( varargin )
+%BOX_TO_P Summary of this function goes here
+%   Detailed explanation goes here
+% function p_to_box(width,height, param, properties)
+%                 ([width,height], param, properties)
+%
+%
+
+%----------------------------------------------------------
+% Process the input.
+%----------------------------------------------------------
+if (length(varargin{1}) == 2)
+  w = varargin{1}(1);
+  h = varargin{1}(2);
+  varargin(1) = [];
+else
+  [w,h] = deal(varargin{1:2});
+  varargin(1:2) = [];
+end
+%%***********获得宽，高***********%%
+
+if (length(varargin) < 1 )%|| any(length(varargin{1}) ~= 2))
+  M = [0,1,0; 0,0,1];
+else
+  corners = cell2mat(varargin{1});
+  varargin(1) = [];
+%  M = [p(1) p(3) p(4); p(2) p(5) p(6)];     %%affine变换参数
+end
+
+%----------------------------------------------------------
+% Draw the box.
+%----------------------------------------------------------
+W = [ 1,-w/2,-h/2; 1,w/2,-h/2; 1,w/2,h/2; 1,-w/2,h/2; 1,-w/2,-h/2 ]';
+M = corners/W;
+
+p = [M(1,1), M(2,1),M(1,2),M(1,3),M(2,2),M(2,3) ];        %%中心
+
+end
+
diff --git a/tracking/utility/interp2.cpp b/tracking/utility/interp2.cpp
new file mode 100644
index 0000000..914d76f
--- /dev/null
+++ b/tracking/utility/interp2.cpp
@@ -0,0 +1,110 @@
+// interp2.cpp
+//
+//   Copyright (C) Jongwoo Lim.
+//   All rights reserved.
+//
+//////////////////////////////////////////////////////////////////////
+
+#include "mex.h"
+#include <math.h>
+
+//#include "cvbase/cvimage.h"
+
+#define ARR(A,r,c,nr,nc)  (((r)<0 || (r)>=(nr) || (c)<0 || (c)>=(nc))? 0 : (A)[(c)*(nr)+(r)])
+
+inline double interp(double *img, int w, int h, double x, double y)
+{
+  register int x0 = (int) x, y0 = (int) y, x1 = x0+1, y1 = y0+1;
+  register double rx0 = x-x0, rx1 = 1-rx0, ry = y-y0;
+  return ((rx1*ARR(img,y0,x0,h,w) + rx0*ARR(img,y0,x1,h,w))*(1-ry) +
+          (rx1*ARR(img,y1,x0,h,w) + rx0*ARR(img,y1,x1,h,w))*ry);
+}
+
+//-----------------------------------------------------------------------------
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+  // check inputs
+  if (nrhs < 3 || nrhs > 6)
+    mexErrMsgTxt("invalid number of inputs: [x,y,]z,xi,yi[,method]");
+  if (nlhs != 1)
+    mexErrMsgTxt("invalid number of outputs: zi");
+
+  const char *_argin[] = { "x","y","z","xi","yi","method" };
+  const char **argin = (nrhs < 4)? &_argin[2] : _argin;
+  char buf[1024];
+
+  // Check input data
+  const mxArray *X = NULL, *Y = NULL, *Z, *XI, *YI;
+  if (nrhs < 4)
+    Z = prhs[0], XI = prhs[1], YI = prhs[2];
+  else
+    X = prhs[0], Y = prhs[1], Z = prhs[2], XI = prhs[3], YI = prhs[4];
+
+  if (nrhs == 4 || nrhs == 6)
+    nrhs--;  // Ignore method
+
+  for (int ri = 0; ri < nrhs; ri++) {
+    if (!prhs[ri])
+    { sprintf(buf,"invalid mxArray(%s)", argin[ri]);  mexErrMsgTxt(buf); }
+    if (!mxIsDouble(prhs[ri]))
+    { sprintf(buf,"data must be double (%s)", argin[ri]);  mexErrMsgTxt(buf); }
+  }
+  if (mxGetNumberOfDimensions(Z) != 2)
+    mexErrMsgTxt("invalid Z dimension: should be 2");
+  // Need to check whether dims of X,Y,Z matche
+  if (mxGetNumberOfElements(XI) != mxGetNumberOfElements(YI))
+    mexErrMsgTxt("XI and YI does not match");
+
+  // Build output data
+  const int *dimz = mxGetDimensions(Z), *dim = mxGetDimensions(XI);
+  int ndim = mxGetNumberOfDimensions(XI);
+
+  register double dx = -1, dy = -1, sx = 1, sy = 1;
+  if (nrhs >= 4) {
+    int nx = mxGetNumberOfElements(X), ny = mxGetNumberOfElements(Y);
+    double *pX = mxGetPr(X), *pY = mxGetPr(Y);
+    if (nx == dimz[1] && ny == dimz[0])
+      dx = -pX[0], sx = (dimz[1]-1)/(pX[nx-1]-pX[0]),
+      dy = -pY[0], sy = (dimz[0]-1)/(pY[ny-1]-pY[0]);
+    else if (nx == ny && ny == mxGetNumberOfElements(Z))
+      dx = -pX[0], sx = (dimz[1]-1)/(pX[(dimz[1]-1)*dimz[0]]-pX[0]),
+      dy = -pY[0], sy = (dimz[0]-1)/(pY[dimz[0]-1]-pY[0]);
+    else
+      mexErrMsgTxt("X and Y do not match with Z");
+  }
+  if (mxIsComplex(Z)) {
+    mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxCOMPLEX);
+
+    // Load input data and fill output array
+    double *pZr = mxGetPr(Z), *pZi = mxGetPi(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI);
+    double *pZIr = mxGetPr(ZI), *pZIi = mxGetPi(ZI);
+
+//    cvImage<double> imgr(pZr, dimz[0], dimz[1]), imgi(pZi, dimz[0], dimz[1]);
+    int h = dimz[0], w = dimz[1];
+    for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) {
+      pZIr[i] = interp(pZr, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy));
+      pZIi[i] = interp(pZi, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy));
+//      cvtPoint<double> p(sy*(pYI[i]+dy),sx*(pXI[i]+dx));
+//      pZIr[i] = imgr.interp<double,double>(p);
+//      pZIi[i] = imgi.interp<double,double>(p);
+    }
+  }
+  else {
+    mxArray *ZI = plhs[0] = mxCreateNumericArray(ndim, dim, mxDOUBLE_CLASS, mxREAL);
+
+    // Load input data and fill output array
+    double *pZ = mxGetPr(Z), *pXI = mxGetPr(XI), *pYI = mxGetPr(YI);
+    double *pZI = mxGetPr(ZI);
+
+//    cvImage<double> img(pZ, dimz[0], dimz[1]);
+    int h = dimz[0], w = dimz[1];
+    for (int i = 0, n = mxGetNumberOfElements(XI); i < n; i++) {
+      pZI[i] = interp(pZ, w,h, sx*(pXI[i]+dx), sy*(pYI[i]+dy));
+//      cvtPoint<double> p(sy*(pYI[i]+dy),sx*(pXI[i]+dx));
+//      pZI[i] = img.interp<double,double>(p);
+    }
+  }
+}
+
+//-----------------------------------------------------------------------------
diff --git a/tracking/utility/interp2.dll b/tracking/utility/interp2.dll
new file mode 100644
index 0000000..921b8d0
Binary files /dev/null and b/tracking/utility/interp2.dll differ
diff --git a/tracking/utility/interp2.mexa64 b/tracking/utility/interp2.mexa64
new file mode 100644
index 0000000..989d748
Binary files /dev/null and b/tracking/utility/interp2.mexa64 differ
diff --git a/tracking/utility/interp2.mexglx b/tracking/utility/interp2.mexglx
new file mode 100644
index 0000000..e1c5d6a
Binary files /dev/null and b/tracking/utility/interp2.mexglx differ
diff --git a/tracking/utility/interp2.mexw64 b/tracking/utility/interp2.mexw64
new file mode 100644
index 0000000..787c0a0
Binary files /dev/null and b/tracking/utility/interp2.mexw64 differ
diff --git a/tracking/utility/p_to_box.m b/tracking/utility/p_to_box.m
new file mode 100644
index 0000000..20989c3
--- /dev/null
+++ b/tracking/utility/p_to_box.m
@@ -0,0 +1,38 @@
+function [ center corners ] = p_to_box(varargin)
+% function p_to_box(width,height, param, properties)
+%                 ([width,height], param, properties)
+%
+%
+
+%----------------------------------------------------------
+% Process the input.
+%----------------------------------------------------------
+if (length(varargin{1}) == 2)
+  w = varargin{1}(1);
+  h = varargin{1}(2);
+  varargin(1) = [];
+else
+  [w,h] = deal(varargin{1:2});
+  varargin(1:2) = [];
+end
+%%***********获得宽，高***********%%
+
+if (length(varargin) < 1 || any(length(varargin{1}) ~= 6))
+  M = [0,1,0; 0,0,1];
+else
+  p = varargin{1};
+  if (length(varargin) > 1 && strcmp(varargin{2},'geom'))
+    p = affparam2mat(p);
+    varargin(1:2) = [];
+  else
+    varargin(1) = [];
+  end
+  M = [p(1) p(3) p(4); p(2) p(5) p(6)];     %%affine变换参数
+end
+
+%----------------------------------------------------------
+% Draw the box.
+%----------------------------------------------------------
+corners = [ 1,-w/2,-h/2; 1,w/2,-h/2; 1,w/2,h/2; 1,-w/2,h/2; 1,-w/2,-h/2 ]';
+corners = M * corners;                  %%顶点
+center = mean(corners(:,1:4),2);        %%中心
diff --git a/tracking/utility/warpimg.m b/tracking/utility/warpimg.m
new file mode 100644
index 0000000..109f02d
--- /dev/null
+++ b/tracking/utility/warpimg.m
@@ -0,0 +1,29 @@
+function wimg = warpimg(img, p, sz)
+% function wimg = warpimg(img, p, sz)
+%
+%    img(h,w)
+%    p(6,n) : mat format
+%    sz(th,tw)
+%
+
+%% Copyright (C) 2005 Jongwoo Lim and David Ross.
+%% All rights reserved.
+
+
+if (nargin < 3)
+    sz = size(img);
+end
+if (size(p,1) == 1)
+    p = p(:);
+end
+w = sz(2);  h = sz(1);  n = size(p,2);
+%[x,y] = meshgrid(1:w, 1:h);
+[x,y] = meshgrid([1:w]-w/2+0.5, [1:h]-h/2);
+pos = reshape(cat(2, ones(h*w,1),x(:),y(:)) ...
+              * [p(1,:) p(2,:); p(3:4,:) p(5:6,:)], [h,w,n,2]);
+cn=size(img,3);
+wimg=zeros([sz cn]);
+for i=1:cn
+    wimg(:,:,i) = squeeze(interp2(img(:,:,i), pos(:,:,:,1), pos(:,:,:,2)));
+end
+wimg(find(isnan(wimg))) = 0;
diff --git a/tracking/wrappers.hpp b/tracking/wrappers.hpp
new file mode 100644
index 0000000..3009281
--- /dev/null
+++ b/tracking/wrappers.hpp
@@ -0,0 +1,42 @@
+/*******************************************************************************
+* Piotr's Computer Vision Matlab Toolbox      Version 3.00
+* Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
+* Licensed under the Simplified BSD License [see external/bsd.txt]
+*******************************************************************************/
+#ifndef _WRAPPERS_HPP_
+#define _WRAPPERS_HPP_
+#ifdef MATLAB_MEX_FILE
+
+// wrapper functions if compiling from Matlab
+#include "mex.h"
+inline void wrError(const char *errormsg) { mexErrMsgTxt(errormsg); }
+inline void* wrCalloc( size_t num, size_t size ) { return mxCalloc(num,size); }
+inline void* wrMalloc( size_t size ) { return mxMalloc(size); }
+inline void wrFree( void * ptr ) { mxFree(ptr); }
+
+#else
+
+// wrapper functions if compiling from C/C++
+inline void wrError(const char *errormsg) { throw errormsg; }
+inline void* wrCalloc( size_t num, size_t size ) { return calloc(num,size); }
+inline void* wrMalloc( size_t size ) { return malloc(size); }
+inline void wrFree( void * ptr ) { free(ptr); }
+
+#endif
+
+// platform independent aligned memory allocation (see also alFree)
+void* alMalloc( size_t size, int alignment ) {
+  const size_t pSize = sizeof(void*), a = alignment-1;
+  void *raw = wrMalloc(size + a + pSize);
+  void *aligned = (void*) (((size_t) raw + pSize + a) & ~a);
+  *(void**) ((size_t) aligned-pSize) = raw;
+  return aligned;
+}
+
+// platform independent alignned memory de-allocation (see also alMalloc)
+void alFree(void* aligned) {
+  void* raw = *(void**)((char*)aligned-sizeof(void*));
+  wrFree(raw);
+}
+
+#endif