A list,

What is Kalman filter Kalman filter is suitable for estimating the optimal state of a dynamic system. Even if the observed system state parameters contain noise and the observed value is not accurate, kalman filter can also complete the optimal estimation of the true value of the state. Most of the tutorials on the web are about The derivation of Kalman’s mathematical formulas, and it’s hard to keep track of the main lines and ideas. So I referred to a foreign scholar’s article, on the working principle of Kalman filter, and then wrote a small program based on OpenCV to explain.

2 What can Kalman filter do

Let’s say we have a DIY mobile car. The car looks like this:



The car can move in the wilderness, and in order to control it, you need to know where it is and how fast it is moving. So, build a vector to store the position and speed of the car



In fact, there are many states of a system, and it is important to choose the state you care most about to build this state vector. States, for example, include the level of water in a reservoir, the temperature of a blast furnace in a steel mill, the position of a fingertip on a tablet screen, and so on, all of which require constant tracking. All right, back to business, the car has a GPS sensor on it, and this sensor has an accuracy of 10 meters. But if there are rivers and cliffs in the wilderness where the car is driving, 10 meters is too wide to fall into and not be able to continue working. Therefore, GPS positioning alone is unable to meet the demand. In addition, if someone says that the car itself receives the movement instructions sent by the controller, according to the number of laps the wheel has turned, it can know how far it has gone, but the direction is unknown, and the phenomenon of the car slipping and wheel idling on the road is absolutely inevitable. Therefore, GPS and the code plate of the motor on the wheel and other sensors indirectly provide us with the information of the car, which contains a lot of and uncertainty. If you put all this information together, can you compute the exact information we want? The answer is yes!



3. Working principle of Kalman filter

1. Prior state estimation

Take the state variable we created earlier,



The figure below shows a state space graph. For the convenience of study, if the car is running on an absolutely straight line, the direction of its position and speed is determined, while the size is uncertain.





Ii. Source code

% 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]',,1,0,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]',,1,0,0 [0] ',[dt,0.1.0]',,0,0,0 [0] '];  % on table, no vertical velocity
A2=[[1.0.0.0]',,1,0,0 [0] ',[dt,0.1.0]'[0, dt, 0, 1]']; % bounce
A3=[[1.0.0.0]',,1,0,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 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 endCopy the code

3. Operation results



Fourth, note

Version: 2014 a