【MATLAB】基于卡尔曼滤波跟踪移动中的目标

最新推荐文章于 2024-05-20 16:39:58 发布

大自然的搬运工'

最新推荐文章于 2024-05-20 16:39:58 发布

阅读量352

点赞数

文章标签： matlab 开发语言目标跟踪

原文链接：http://t.csdn.cn/QUQrt

版权

效果图：

源码如下

·extract.m

% extracts the center (cc,cr) and radius of the largest blob
function [x2,y2,width_x,width_y,cc,cr,flag]=extract(Imwork,Imback,index)%,fig1,fig2,fig3,fig15,index)
  
  x2 = 0;
  y2 = 0;
  width_x = 0;
  width_y = 0;
  cc = 0;
  cr = 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)) > 20) ...
     | (abs(Imwork(:,:,2) - Imback(:,:,2)) > 20) ...
     | (abs(Imwork(:,:,3) - Imback(:,:,3)) > 20);  

  % Morphology Operation  erode to remove small noise
  %foremm = bwmorph(fore,'erode',2); %2 time

  % select largest object
  labeled = bwlabel(fore,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;
  boundingbox = stats(1).BoundingBox;
  x2=boundingbox(1);
  y2=boundingbox(2);
  width_x=boundingbox(3);
  width_y=boundingbox(4);
  cc = centroid(1);
  cr = centroid(2);
  flag = 1;
  return

·kalman.m

clear,clc
% compute the background image
Imzero = zeros(240,360,3);
% video=aviread('SampleVideo.avi');
% Im = double(cat(4,video(1:2:end).cdata))/255;
video=VideoReader('SampleVideo.avi');
Im = double(cat(4,read(video(1:2:end))))/255;
    clear video

% Convert to RGB to GRAY SCALE image.
nFrames = size(Im,4);
for i = 1:5
%Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
Imzero = Im(:,:,:,i)+Imzero;
end
Imback = Imzero/5*255;

[MR,MC,Dim] = size(Imback);
% Kalman filter initialization
R=[[0.2845,0.0045]',[0.0045,0.0455]'];         %观测噪声协方差矩阵
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(100,4);     %状态矩阵

% loop over all images
for i = 1 : nFrames
  % load image
  imshow(Im(:,:,:,i));
  hold on
  %imshow(Im)

  Imwork = Im(:,:,:,i)*255;

  %extract ball
  [x2(i),y2(i),width_x(i),width_y(i),cc(i),cr(i),flag] = extract(Imwork,Imback,i);
  if flag==0
    continue
  end

  
    %for c = -1*radius: radius/20 : 1*radius
      %r = sqrt(radius^2-c^2);
      %plot(cc(i)+c,cr(i)+r,'g.')
      %plot(cc(i)+c,cr(i)-r,'g.')
    %end
    
    if (width_x(i)~=0) & (width_y(i)~=0) 
       rectangle('Position',[x2(i) y2(i) width_x(i) width_y(i)],'EdgeColor','r');
    end
    if (width_x(i)~=0) & (width_y(i)~=0) 
       plot(cc(i),cr(i), 'r+');
    end
    hwidth_x(i)=cc(i)-x2(i);
    hwidth_y(i)=cr(i)-y2(i);
  % Kalman update
  % i;
  if kfinit==0
    xp = [cc(i),cr(i),0,0]';            %预测状态矩阵
  else
    xp=A*x(i-1,:)';
  end
  kfinit=1;
  PP = A*P*A' + Q;                      %预测误差协方差
  K = PP*H'*inv(H*PP*H'+R);
  x(i,:) = (xp + K*([cc(i),cr(i)]' - H*xp))';
  x(i,:);
  [cc(i),cr(i)];
  P = (eye(4)-K*H)*PP;

  hold on
    %for c = -1*radius: radius/20 : 1*radius
      %r = sqrt(radius^2-c^2);
      %plot(x(i,1)+c,x(i,2)+r,'r.')
      %plot(x(i,1)+c,x(i,2)-r,'r.')
    %end
    
    if (width_x(i)~=0) & (width_y(i)~=0) 
       rectangle('Position',[(x(i,1)-hwidth_x(i)) (x(i,2)-hwidth_y(i)) width_x(i) width_y(i)],'EdgeColor','g');
    end
    
     % pause(0.3)
    x1(i)=x(i,1)-cc(i);
    y1(i)=x(i,2)-cr(i);
    hold on
    if (width_x(i)~=0) & (width_y(i)~=0) 
       plot(x(i,1),x(i,2), 'g+');
    end
    drawnow
end

% show positions
  figure
  plot(x1,'r*');
  hold on
  plot(y1,'g*');
%end

%estimate image noise (R) from stationary ball
  posn = [cc(55:60)',cr(55:60)'];
  mp = mean(posn);
  diffp = posn - ones(6,1)*mp;
  Rnew = (diffp'*diffp)/5

注意：原版视频avi格式报错要编码器，转为wmv格式可行