一、简介

1 卡尔曼滤波是什么
卡尔曼滤波适用于估计一个动态系统的最优状态。即便是观测到的系统状态参数含有噪声，观测值不准确，卡尔曼滤波也能够完成对状态真实值的最优估计。网上大多数的教程讲到卡尔曼的数学公式推导，会让人很头疼，难以把握其中的主线和思想。所以我参考了国外一位学者的文章，讲述卡尔曼滤波的工作原理，然后编写了一个基于OpenCV的小程序给大家做一下说明。

2 卡尔曼滤波能做什么
假设我们手头有一辆DIY的移动小车。这辆车的外形是这样的：

这辆车可以在荒野移动，为了便于对它进行控制，需要知道它的位置以及移动速度。所以，建立一个向量，用来存储小车的位置和速度

其实，一个系统的状态有很多，选择最关心的状态来建立这个状态向量是很重要的。例如，状态还有水库里面水位的高低、炼钢厂高炉内的温度、平板电脑上面指尖触碰屏幕的位置等等这些需要持续跟踪的物理量。好了，回归到正题，小车上面安装了GPS传感器，这个传感器的精度是10米。但是如果小车行驶的荒野上面有河流和悬崖的话，10米的范围就太大，很容易掉进去进而无法继续工作。所以，单纯靠GPS的定位是无法满足需求的。另外，如果有人说小车本身接收操控着发送的运动指令，根据车轮所转动过的圈数时能够知道它走了多远，但是方向未知，并且在路上小车打滑车轮空转的现象绝对是不可避免。所以，GPS以及车轮上面电机的码盘等传感器是间接地为我们提供了小车的信息，这些信息包含了很多的和不确定性。如果将所有这些信息综合起来，是否能够通过计算得到我们更想要的准确信息呢？答案是可以的！

3 卡尔曼滤波的工作原理
1.先验状态估计
以之前我们创建的状态变量为例，

下图表示的是一个状态空间图，为了研究方便，假如小车在一条绝对笔直的线路上面行驶，其位置和速度的方向是确定的，不确定的是大小。

二、源代码

% compute the background image
Im0 = double(imread('ball00000100.jpg','jpg'));
Im1 = double(imread('ball00000101.jpg','jpg'));
Im2 = double(imread('ball00000102.jpg','jpg'));
Im3 = double(imread('ball00000103.jpg','jpg'));
Im4 = double(imread('ball00000104.jpg','jpg'));
Imback = (Im0 + Im1 + Im2 + Im3 + Im4)/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 = 9.8; % pixels^2/time step
Bu = [0,0,0,g]'; 
kfinit=0;
x=zeros(100,4);

% loop over all images
fig1=1;
fig2=0;
fig15=0;
fig3=0;
fig4=4;
for i = 1 : 60
  % load image
  if i < 11
    Im = (imread(['ball0000010',int2str(i-1), '.jpg'],'jpg')); 
  else
    Im = (imread(['ball000001',int2str(i-1), '.jpg'],'jpg')); 
  end
  if fig1 > 0
    figure(fig1)%creat figure with handle fig1
    clf%clear current figure
    imshow(Im)
  end
  Imwork = double(Im);

  %extract ball
  [cc(i),cr(i),radius,flag]=extractball(Imwork,Imback,fig1,fig2,fig3,fig15,i);
  if flag==0
    continue
  end

  if fig1 > 0
    figure(fig1)
    hold on
    
%plot the figure of measure value
%     for c = -0.97*radius: radius/20 : 0.97*radius
%       r = sqrt(radius^2-c^2);
%       plot(cc(i)+c,cr(i)+r,'g.')
%       plot(cc(i)+c,cr(i)-r,'g.')
%     end


    %eval(['saveas(gcf,''TRACK/trk',int2str(i-1),'.jpg'',''jpg'')']);  
  end

  % Kalman update
i
  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
  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

  if fig1 > 0
    figure(fig1)
    hold on
    for c = -0.97*radius: radius/20 : 0.97*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
%    eval(['saveas(gcf,''KFILT/kflt',int2str(i-1),'.jpg'',''jpg'')']);  
  end
  % compute the background image
Im0 = double(imread('ball00000100.jpg','jpg'));
Im1 = double(imread('ball00000101.jpg','jpg'));
Im2 = double(imread('ball00000102.jpg','jpg'));
Im3 = double(imread('ball00000103.jpg','jpg'));
Im4 = double(imread('ball00000104.jpg','jpg'));
Imback = (Im0 + Im1 + Im2 + Im3 + Im4)/5;
[MR,MC,Dim] = size(Imback);

% Kalman filter static initializations
R=[[0.2845,0.0045]',[0.0045,0.0455]'];
H=[[1,0]',[0,1]',[0,0]',[0,0]'];
Q=0.01*eye(4);
dt=1;
A1=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,0,0,0]'];  % on table, no vertical velocity
A2=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]']; % bounce
A3=[[1,0,0,0]',[0,1,0,0]',[dt,0,1,0]',[0,dt,0,1]']; % normal motion
g = 6.0;              % gravity=pixels^2/time step
Bu1 = [0,0,0,0]';   % on table, no gravity
Bu2 = [0,0,0,g]';   % bounce
Bu3 = [0,0,0,g]';   % normal motion
loss=0.7;

% multiple condensation states
NCON=100;          % number of condensation samples
MAXTIME=60;        % number of time frames
x=zeros(NCON,MAXTIME,4);      % state vectors
weights=zeros(NCON,MAXTIME);  % est. probability of state
trackstate=zeros(NCON,MAXTIME);         % state=1,2,3;
P=zeros(NCON,MAXTIME,4,4);    % est. covariance of state vec.
for i = 1 : NCON              % initialize estimated covariance
for j = 1 : MAXTIME
  P(i,j,1,1) = 100;
  P(i,j,2,2) = 100;
  P(i,j,3,3) = 100;
  P(i,j,4,4) = 100;
end
end
pstop=0.05;      % probability of stopping vertical motion
pbounce=0.30;    % probability of bouncing at current state (overestimated)
xc=zeros(4,1);   % selected state
TP=zeros(4,4);   % predicted covariance

% loop over all images
fig1=1;
fig2=0;
fig15=0;
fig3=0;
for i = 1 : MAXTIME
i
  % load image
  if i < 11
    Im = (imread(['ball0000010',int2str(i-1), '.jpg'],'jpg')); 
  else
    Im = (imread(['ball000001',int2str(i-1), '.jpg'],'jpg')); 
  end
  if fig1 > 0
    figure(fig1)
    clf
    imshow(Im)
  end
  Imwork = double(Im);

  % extract ball
  [cc(i),cr(i),radius,flag]=extractball(Imwork,Imback,fig1,fig2,fig3,fig15,i);
  if flag==0
    for k = 1 : NCON
      x(k,i,:) = [floor(MC*rand(1)),floor(MR*rand(1)),0,0]';
      weights(k,i)=1/NCON;
    end
    continue
  end

  % display green estimated ball circle over original image
  if fig1 > 0
    figure(fig1)
    hold on
    for c = -0.99*radius: radius/10 : 0.99*radius
      r = sqrt(radius^2-c^2);
      plot(cc(i)+c,cr(i)+r,'g.')
      plot(cc(i)+c,cr(i)-r,'g.')
    end
  end

  % condensation tracking
  % generate NCON new hypotheses from current NCON hypotheses
  % first create an auxiliary array ident() containing state vector j
  % SAMPLE*p_k times, where p is the estimated probability of j
  if i ~= 1
    SAMPLE=10;
    ident=zeros(100*SAMPLE,1);
    idcount=0;
    for j = 1 : NCON    % generate sampling distribution
      num=floor(SAMPLE*100*weights(j,i-1));  % number of samples to generate
      if num > 0
        ident(idcount+1:idcount+num) = j*ones(1,num);
        idcount=idcount+num;
      end
    end
  end

  % generate NCON new state vectors
  for j = 1 : NCON

    % sample randomly from the auxiliary array ident()
    if i==1 % make a random vector
      xc = [floor(MC*rand(1)),floor(MR*rand(1)),0,0]';
    else
      k = ident(ceil(idcount*rand(1))); % select which old sample
      xc(1) = x(k,i-1,1);  % get its state vector
      xc(2) = x(k,i-1,2);
      xc(3) = x(k,i-1,3);
      xc(4) = x(k,i-1,4);

      % sample about this vector from the distribution (assume no covariance)
      for n = 1 : 4
        xc(n) = xc(n) + 5*sqrt(P(j,i-1,n,n))*randn(1);
      end
    end

三、运行结果

四、备注

完整代码或者代写添加QQ 912100926