demo_mc.asv

matlab UPF

% PURPOSE : Demonstrate the differences between the following filters on the same problem:
%           
%           1) Extended Kalman Filter  (EKF)
%           2) Unscented Kalman Filter (UKF)
%           3) Particle Filter         (PF)
%           4) PF with EKF proposal    (PFEKF)
%           5) PF with UKF proposal    (PFUKF)


% For more details refer to:

% AUTHORS  : Nando de Freitas      ([e-mail])
%            Rudolph van der Merwe ([e-mail])
% DATE     : 17 August 2000

clear all;
clc;
echo off;
path('./ukf',path);
tax=1.0;
tx=0.5;
T=0.01; 
% A=[1 T (T/tax-1+exp(-T/tax)) 0;0 1 (1-exp(-T/tax))*tax 0;0 0 exp(-T/tax) 0;0 0 0 exp(-T/tx)];
% A_pred =[1 T (T^2)/2 0;0 1 T 0;0 0 1 0;0 0 0 exp(-T/tx)];
A=[1 T T^2/2 0;0 1 T 0;0 0 1 0;0 0 0 exp(-T/tx)];
H=[1 0 0 1];
% 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.
R = 1e-5;                   % EKF's measurement noise variance. 
Q = 3/4;                    % EKF's process noise variance.
P0 = 3/4;                   % EKF's initial variance of the states.

N = 200;                     % Number of particles.
resamplingScheme = 1;       % The possible choices are
                            % systematic sampling (2),
                            % residual (1)
                            % and multinomial (3). 
                            % They're all O(N) algorithms. 

Q_pfekf = 10*3/4;
R_pfekf = 1e-1;

Q_pfukf = 2*3/4;
R_pfukf = 1e-1;
			    
alpha = 1;                  % UKF : point scaling parameter
beta  = 0;                  % UKF : scaling parameter for higher order terms of Taylor series expansion 
kappa = 2;                  % UKF : sigma point selection scaling parameter (best to leave this = 0)

%**************************************************************************************

% SETUP BUFFERS TO STORE PERFORMANCE RESULTS
% ==========================================

rmsError_ekf      = zeros(1,no_of_runs);
rmsError_ukf      = zeros(1,no_of_runs);
rmsError_pf       = zeros(1,no_of_runs);
rmsError_pfMC     = zeros(1,no_of_runs);
rmsError_pfekf    = zeros(1,no_of_runs);
rmsError_pfekfMC  = zeros(1,no_of_runs);
rmsError_pfukf    = zeros(1,no_of_runs);
rmsError_pfukfMC  = zeros(1,no_of_runs);

time_pf       = zeros(1,no_of_runs);     
time_pfMC     = zeros(1,no_of_runs);
time_pfekf    = zeros(1,no_of_runs);
time_pfekfMC  = zeros(1,no_of_runs);
time_pfukf    = zeros(1,no_of_runs);
time_pfukfMC  = 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(4,T);
y = zeros(4,T);
processNoise = zeros(4,T);
measureNoise = zeros(4,T);
x(:,1) = [1 0 1 0]';                        % Initial state
 
for t=2:T
  processNoise(:,t) = [0 0 gengamma(g1,g2) gengamma(g1,g2)];  
  measureNoise(t) = sqrt(sigma)*randn(4,1);    
  x(:,t) = A*x(:,t-1)+ processNoise(t);      % Gamma transition prior.  
  y(:,t) = (x(:,t)).^2 + measureNoise(t);      % Gaussian likelihood.
end;  

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

%%%%%%%%%%%%%%%  PERFORM EKF and UKF ESTIMATION  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
mu_ekf = ones(4,T);     % EKF estimate of the mean of the states.
P_ekf = P0*ones(4,T);   % EKF estimate of the variance of the states.

mu_ukf = mu_ekf;        % UKF estimate of the mean of the states.
P_ukf = P_ekf;          % UKF estimate of the variance of the states.

yPred = ones(4,T);      % One-step-ahead predicted values of y.
mu_ekfPred = ones(4,T); % EKF O-s-a estimate of the mean of the states.
PPred = ones(4,T);      % EKF O-s-a estimate of the variance of the states.

disp(' ');

for t=2:T,    
  fprintf('run = %i / %i :  EKF & UKF : t = %i / %i  \r',j,no_of_runs,t,T);
  fprintf('\n')
  
  % PREDICTION STEP:
  % ================ 
  mu_ekfPred(:,t) = A*mu_ekf(:,t-1);
  Jx = 0.5;                             % Jacobian for ffun.
  PPred(t) = Q*eye(4) + Jx*P_ekf(t-1)*Jx'; 
  
  % CORRECTION STEP:
  % ================
  yPred(:,t) = mu_ekfPred(:,t).^2;

  M = R*eye(4) + Jy*PPred(t)*Jy';                 % Innovations covariance.
  K = PPred(t)*Jy'*inv(M);                 % Kalman gain.
  mu_ekf(:,t) = mu_ekfPred(:,t) + K*(y(:,t)-yPred(:,t));
  P_ekf(t) = PPred(t) - K*Jy*PPred(t);
  
  % Full Unscented Kalman Filter step
  % =================================
  [mu_ukf(t),P_ukf(t)]=ukf(mu_ukf(t-1),P_ukf(t-1),[],Q,'ukf_ffun',y(t),R,'ukf_hfun',t,alpha,beta,kappa);  
  
end;   % End of t loop.



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

% INITIALISATION:
% ==============
xparticle_pf = ones(4,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(4,T,N);    % One-step-ahead predicted values of the states.
yPred_pf = ones(4,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) = A*xparticlePred_pf(:,t-1)+ processNoise(t);   
  end;

  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(:,t)-yPred_pf(:,t,i))^(2)))+ 1e-99;
	  
  for i=1:N,
    yPred_pf(:,t,i) = (xparticlePred_pf(:,t)).^2;        
    lik = (det(sigmaM)^(-0.5))*exp(-0.5*(y(:,t)-y_pred(:,t,i))'* ...
			   pinv(sigmaM)*(y(:,t)-y_pred(:,t,i)))+ 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
    xx_pf(3,t)=mean(x_pf(3,t,:));                                                % resampled indices.
end;   % End of t loop.

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


%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO WITH MCMC  %%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%  ========================================  %%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
xparticle_pfMC = ones(4,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_pfMC = ones(4,T,N);  % One-step-ahead predicted values of the states.
yPred_pfMC = ones(4,T,N);          % One-step-ahead predicted values of y.
w = ones(T,N);                   % Importance weights.
previousXMC = ones(4,T,N);         % Particles at the previous time step. 
previousXResMC = ones(4,T,N);      % Resampled previousX.

disp(' ');
 
tic;                             % Initialize timer for benchmarking

for t=2:T,    
  fprintf('run = %i / %i :  PF-MCMC : 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_pfMC(:,t,i) = A*xparticle_pfMC(:,t-1,i)+ processNoise(t);   
  end;
  previousXMC(:,t,:) = xparticle_pfMC(:,t-1,:);  % Store the particles at t-1. 


  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  
  for i=1:N,
    yPred_pfMC(:,t,i) = xparticlePred_pfMC(:,t,i).^2;        
    lik = (det(sigmaM)^(-0.5))*exp(-0.5*(y(:,t)-yPred_pfMC(:,t,i))'* ...
			   pinv(sigmaM)*(y(:,t)-yPred_pfMC(:,t,i)))+ 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_pfMC(:,t,:) = xparticlePred_pfMC(:,t,outIndex); % Keep particles with
                                                        % resampled
                                                        % indices.
  previousXResMC(:,t,:) = previousXMC(:,t,outIndex);  % Resample particles
                                                  % at t-1.
  xparticle_pfMC(3,t)=mean(xparticle_pfMC(3,t,:));
  % METROPOLIS-HASTINGS STEP:
  % ========================
  u=rand(N,1); 
  accepted=0;
  rejected=0;
  for i=1:N,   
    xProp(:,1) =A*previousXResMC(:,t,i) + processNoise(t);   
    mProp(:,1) = xProp(:,1).^2;        
    likProp = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(:,t)-mProp(:,1))^(2))) + 1e-99;     
    m(:,1) = xparticle_pfMC(:,t,i).^2;        
    lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(:,t)-m(:,1))^(2))) + 1e-99;     
    acceptance = min(1,likProp/lik);
    if u(i,1) <= acceptance 
      xparticle_pfMC(:,t,i) = xProp(:,1);
      accepted=accepted+1;
    else
      xparticle_pfMC(:,t,i) = xparticle_pfMC(:,t,i); 
      rejected=rejected+1;
    end;
  end;
  
  
end;   % End of t loop.

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


%%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ======== EKF proposal ========  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
xparticle_pfekf = 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.
Pparticle_pfekf = P0*ones(T,N);     % Particles for the covariance of x.
xparticlePred_pfekf = ones(T,N);    % One-step-ahead predicted values of the states.
PparticlePred_pfekf = ones(T,N);    % One-step-ahead predicted values of P.
yPred_pfekf = ones(T,N);            % One-step-ahead predicted values of y.
w = ones(T,N);                      % Importance weights.
muPred_pfekf = ones(T,1);           % EKF O-s-a estimate of the mean of the states.
PPred_pfekf = ones(T,1);            % EKF O-s-a estimate of the variance of the states.
mu_pfekf = ones(T,1);               % EKF estimate of the mean of the states.
P_pfekf = P0*ones(T,1);             % EKF estimate of the variance of the states.

disp(' ');

tic;                                % Initialize timer for benchmarking

for t=2:T,    
  fprintf('run = %i / %i :  PF-EKF : t = %i / %i  \r',j,no_of_runs,t,T);
  fprintf('\n')
  
  % PREDICTION STEP:
  % ================ 
  % We use the EKF as proposal.
  for i=1:N,
    muPred_pfekf(t) = feval('ffun',xparticle_pfekf(t-1,i),t);
    Jx = 0.5;                                 % Jacobian for ffun.
    PPred_pfekf(t) = Q_pfekf + Jx*Pparticle_pfekf(t-1,i)*Jx'; 
    yPredTmp = feval('hfun',muPred_pfekf(t),t);
    if t<=30,
      Jy = 2*0.2*muPred_pfekf(t);                     % Jacobian for hfun.