Kalman filter and visual tracking

以下是matlab code:

function neo()
clear,clc
width = 320;
height = 240;
background_image_number = 5;
total_image_number = 60;
image_folder = 'DATA/';
extendfile = '.jpg';
% compute the background image
Imzero = zeros(height,width,3);
for i = 1:background_image_number
Im{i} = double(imread([image_folder,int2str(i),extendfile]));
Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[MR,MC,Dim] = size(Imback);
%設定參數
R=[[0.2845,0.0045]',[0.0045,0.0455]'];   
%R=[[0,0]',[0,0]']; %R趨近0表示越相信測量值zk
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
P = 100*eye(4);
dt=1;
A=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]'];
g = 6; % pixels^2/time step
Bu = [0,0,0,g]';
kfinit=0;
x=zeros(total_image_number,4);    %x[position_X,position_Y,velocity_X,velocity_Y]

for i = 1 : total_image_number
    Im = (imread([image_folder,int2str(i),extendfile])); 
  imshow(Im)
   Imwork = double(Im);
   [cc(i),cr(i),radius,flag] = measure(Imwork,Imback,i);
  hold on
  plot(cc(i),cr(i),'g.')
  
%-----------------------prediction state-----------------------------------  
if kfinit==0
    xp = [MC/2,MR/2,0,0]'; %起始先驗估計座標點為圖中心
  else
    xp=A*x(i-1,:)' + Bu;   %先驗估計座標點
  end
  kfinit=1;
  PP = A*P*A' + Q; %先驗共變數矩陣
%--------------------------------------------------------------------------
%-----------------------correction state-----------------------------------  
 K = PP*H'*inv(H*PP*H'+R); %由先驗共變數矩陣求卡曼增益矩陣
 x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))'; %由測量座標點更新估計座標點
 P = (eye(4)-K*H)*PP; %更新共變數矩陣
 hold on
 plot(x(i,1),x(i,2),'r.')
 pause(0.03)
end
  %X
  figure 
  hold on
  plot(cc,'b')
  plot(x(1:total_image_number,1),'r')
  %Y
  figure
  hold on
  plot(cr,'b')
  plot(x(1:total_image_number,2),'r')
  
  function [cc,cr,radius,flag]=measure(Imwork,Imback,index)
  cc = 0;
  cr = 0;
  radius = 0;
  flag = 0;
  [MR,MC,Dim] = size(Imback);
  % subtract background & select pixels with a big difference
  fore = zeros(MR,MC);          %image subtracktion
  fore = (abs(Imwork(:,:,1)-Imback(:,:,1)) > 10) ...
     | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 10) ...
     | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 10);  
  % Morphology Operation  erode to remove small noise
  foremm = bwmorph(fore,'erode',2); %2 time
  % select largest object
  labeled = bwlabel(foremm,4);
  stats = regionprops(labeled,['basic']);%basic mohem nist
  [N,W] = size(stats);
  if N < 1
    return   
  end
  % do bubble sort (large to small) on regions in case there are more than 1
  id = zeros(N);
  for i = 1 : N
    id(i) = i;
  end
  for i = 1 : N-1
    for j = i+1 : N
      if stats(i).Area < stats(j).Area
        tmp = stats(i);
        stats(i) = stats(j);
        stats(j) = tmp;
        tmp = id(i);
        id(i) = id(j);
        id(j) = tmp;
      end
    end
  end
  % make sure that there is at least 1 big region
  if stats(1).Area < 100 
    return
  end
  selected = (labeled==id(1));
  % get center of mass and radius of largest
  centroid = stats(1).Centroid;
  radius = sqrt(stats(1).Area/pi);
  cc = centroid(1);
  cr = centroid(2);
  flag = 1;
  return