Kalman Filter笔记(2)

Kalman Filter分为两大部分，Predict和Correction。为了更好地理解Kalman Filter Tracking，我在Matlab File Exchange上找到了一个.m的仿真，看了半天发现作者取的是state_post，应该是将Kalman Filter做轨迹滤波用?

我将这个文件修改了一下，改成：

clear,clc
% compute the background image
Imzero = zeros(240,320,3);
for i = 1:5
Im{i} = double(imread(['DATA/',int2str(i),'.jpg']));
Imzero = Im{i}+Imzero;
end
Imback = Imzero/5;
[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
g = 0;
Bu = [0,0,0,g]';
kfinit=0;
x=zeros(100,4);

% loop over all images
for i = 1 : 60
  % load image
  Im = (imread(['DATA/',int2str(i), '.jpg']));
  imshow(Im)
  imshow(Im)
  Imwork = double(Im);

% Kalman predict

i
  if kfinit==0
    xp = [MC/2,MR/2,0,0]'
  else
    xp=A*x(i-1,:)' + Bu
  end
  kfinit=1;
  pred_pos = H*xp;
  recorded_pred_pos(i,1) = pred_pos(1);
  recorded_pred_pos(i,2) = pred_pos(2);
    %extract ball
  [cc(i),cr(i),radius,flag] = extractball(Imwork,Imback,i);
  if flag==0
    continue
  end
   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.')
      plot(pred_pos(1)+c,pred_pos(2)+r,'r.')
      plot(pred_pos(1)+c,pred_pos(2)-r,'r.')
    end
  hold on
    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
  % kalman update
  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

      pause(0.3)
end

% show positions
  figure
  plot(cc(10:60),'r*')
  hold on
  %plot(cr,'g*')
  plot(recorded_pred_pos(10:60,1), 'go');
  title('detected x position and predicted x position');
%end

  figure
  plot(cr(10:60),'r*')
  hold on
  %plot(cr,'g*')
  plot(recorded_pred_pos(10:60,2), 'go');
  title('detected y position and predicted y position');

在Matlab下运行，可以得到下面的结果：