pf.m

this particle filter code can be used for target tracking

clear all;
clc;
echo off;

%% INITIALISATION AND PARAMETERS:
% ==============================
no_of_runs = 1;            % number of experiments to generate statistical
                            % averages
doPlot = 0;                 % 1 plot online. 0 = only plot at the end.
sigma =  1e-5;              % Variance of the Gaussian measurement noise.
g1 = 3;                     % Paramater of Gamma transition prior.
g2 = 2;                     % Parameter of Gamman transition prior.
                            % Thus mean = 3/2 and var = 3/4.
T = 60;                     % Number of time steps.
N = 200;                    % Number of particles.
resamplingScheme = 3;       

%% SETUP BUFFERS TO STORE PERFORMANCE RESULTS
%==========================================
rmsError_pf   = zeros(1,no_of_runs);
time_pf       = zeros(1,no_of_runs);

%% MAIN LOOP
%==========================================
for j=1:no_of_runs,
    rand('state',sum(100*clock));   % Shuffle the pack!
    randn('state',sum(100*clock));  % Shuffle the pack!

    % GENERATE THE DATA:
    % ==================
    x = zeros(T,1); y = zeros(T,1); processNoise = zeros(T,1); measureNoise = zeros(T,1);
    x(1) = 1; % Initial state.
    for t=2:T
      processNoise(t) = gengamma(g1,g2);  
      measureNoise(t) = sqrt(sigma)*randn(1,1);    
      x(t) = feval('ffun',x(t-1),t) +processNoise(t);     % Gamma transition prior.  
      y(t) = feval('hfun',x(t),t) + measureNoise(t);      % Gaussian likelihood.
    end;  

    % PLOT THE GENERATED DATA:
    % ========================
    figure(1)
    clf;
    plot(1:T,x,'r',1:T,y,'b');
    ylabel('Data','fontsize',15);
    xlabel('Time','fontsize',15);
    legend('States (x)','Observations(y)');

%%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%

    % INITIALISATION:
    % ==============
    xparticle_pf = ones(T,N);        % These are the particles for the estimate
                                     % of x. Note that there's no need to store
                                     % them for all t. We're only doing this to
                                     % show you all the nice plots at the end.
    xparticlePred_pf = ones(T,N);    % One-step-ahead predicted values of the states.
    yPred_pf = ones(T,N);            % One-step-ahead predicted values of y.
    w = ones(T,N);                   % Importance weights.

    disp(' ');
    tic;                             % Initialize timer for benchmarking
    for t=2:T,    
%       fprintf('run = %i / %i :  PF : t = %i / %i  \r',j,no_of_runs,t,T);
%       fprintf('\n')

      % PREDICTION STEP:
      % ================ 
      % We use the transition prior as proposal.
      for i=1:N,
        xparticlePred_pf(t,i) = feval('ffun',xparticle_pf(t-1,i),t) + gengamma(g1,g2);   
      end;

      % EVALUATE IMPORTANCE WEIGHTS:
      % ============================
      % For our choice of proposal, the importance weights are give by:  
      for i=1:N,
        yPred_pf(t,i) = feval('hfun',xparticlePred_pf(t,i),t);        
        lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pf(t,i))^(2))) ...
          + 1e-99; % Deal with ill-conditioning.
        w(t,i) = lik;    
      end;  
      w(t,:) = w(t,:)./sum(w(t,:));                % Normalise the weights.

      % SELECTION STEP:
      % ===============
      % Here, we give you the choice to try three different types of
      % resampling algorithms. Note that the code for these algorithms
      % applies to any problem!
      if resamplingScheme == 1
        outIndex = residualR(1:N,w(t,:)');        % Residual resampling.
      elseif resamplingScheme == 2
        outIndex = systematicR(1:N,w(t,:)');      % Systematic resampling.
      else  
        outIndex = multinomialR(1:N,w(t,:)');     % Multinomial resampling.  
      end;
      xparticle_pf(t,:) = xparticlePred_pf(t,outIndex); % Keep particles with
                                                        % resampled indices.
    end;   % End of t loop.

    time_pf(j) = toc;    % How long did this take?
end

%%%%%%%%%%%%%%%%%%%%%  PLOT THE RESULTS  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%  ================  %%%%%%%%%%%%%%%%%%%%%

figure(2)
clf;
p0=plot(1:T,y,'k+','lineWidth',2); hold on;
p4=plot(1:T,mean(xparticle_pf,2),'g','lineWidth',2);
p1=plot(1:T,x,'k:o','lineWidth',2); hold off;
legend(p4,'PF estimate');
xlabel('Time','fontsize',15)
zoom on;
title('Filter estimates (posterior means) vs. True state','fontsize',15)