ekf_ukf_pfdemo.asv

该算法详细仿真了EKF、UKF以及PF三种方法

% 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)
      
% 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;


% 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 = 50;                     % Number of time steps.
R = 1e-2;                   % EKF's measurement noise variance. 
Q = 3/4;                    % EKF's process noise variance.
P0 = 3/4;                   % EKF's initial variance of the states.

N =500;                     % 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);

time_ekf      = zeros(1,no_of_runs); 
time_ukf      = 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 EKF and UKF ESTIMATION  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
mu_ekf = ones(T,1);     % EKF estimate of the mean of the states.
P_ekf = P0*ones(T,1);   % 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(T,1);      % One-step-ahead predicted values of y.
mu_ekfPred = ones(T,1); % EKF O-s-a estimate of the mean of the states.
PPred = ones(T,1);      % EKF O-s-a estimate of the variance of the states.

disp(' ');
tic;
for t=2:T,    
  fprintf('run = %i / %i :  EKF  : t = %i / %i  \r',j,no_of_runs,t,T);
  fprintf('\n')
  
  % PREDICTION STEP:
  % ================ 
  mu_ekfPred(t) = feval('ffun',mu_ekf(t-1),t);
  Jx = 0.5;                             % Jacobian for ffun.
  PPred(t) = Q + Jx*P_ekf(t-1)*Jx'; 
  
  % CORRECTION STEP:
  % ================
  yPred(t) = feval('hfun',mu_ekfPred(t),t);
  if t<=30,
    Jy = 2*0.2*mu_ekfPred(t);                 % Jacobian for hfun.
  else
    Jy = 0.5;
  %  Jy = cos(mu_ekfPred(t))/2;
  %   Jy = 2*mu_ekfPred(t)/4;                 % Jacobian for hfun. 
  end;
  M = R + 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);
end;
time_ekf(j)=toc;
  
  % Full Unscented Kalman Filter step
  % =================================
  tic;
  for t=2:T,    
  fprintf('run = %i / %i :  UKF : t = %i / %i  \r',j,no_of_runs,t,T);
  fprintf('\n')
  [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.
time_ukf(j)=toc;



%%%%%%%%%%%%%%%  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?










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

figure(1)
clf;
% p0=plot(1:T,y,'k+','lineWidth',2); hold on;
p2=plot(1:T,mu_ekf,'r:','lineWidth',2); hold on;
p3=plot(1:T,mu_ukf,'b:','lineWidth',2);
p4=plot(1:T,mean(xparticle_pf(:,:)'),'g','lineWidth',2);
p1=plot(1:T,x,'k.','lineWidth',2); hold off;
legend([p1 p2 p3 p4],'True x','EKF estimate','UKF estimate','PF estimate');
xlabel('Time','fontsize',15)
zoom on;
title('Filter estimates (posterior means) vs. True state','fontsize',15)

figure(2)
clf
subplot(211);
semilogy(1:T,P_ekf,'r--',1:T,P_ukf,'b','lineWidth',2);
legend('EKF','UKF');
title('Estimates of state covariance','fontsize',14);
xlabel('time','fontsize',12);
ylabel('var(x)','fontsize',12);
zoom on;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-- CALCULATE PERFORMANCE --%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rmsError_ekf(j)     = sqrt(inv(T)*sum((x-mu_ekf).^(2)));
rmsError_ukf(j)     = sqrt(inv(T)*sum((x-mu_ukf).^(2)));
rmsError_pf(j)      = sqrt(inv(T)*sum((x'-mean(xparticle_pf')).^(2)));

disp(' ');
disp('Root mean square (RMS) errors');
disp('-----------------------------');
disp(' ');
disp(['EKF          = ' num2str(rmsError_ekf(j))]);
disp(['UKF          = ' num2str(rmsError_ukf(j))]);
disp(['PF           = ' num2str(rmsError_pf(j))]);


disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['EKF           = ' num2str(time_ekf(j))]);
disp(['UKF           = ' num2str(time_ukf(j))]);
disp(['PF           = ' num2str(time_pf(j))]);
disp(' ');

drawnow;

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

end    % Main loop (for j...)

% calculate mean of RMSE errors
mean_RMSE_ekf     = mean(rmsError_ekf);
mean_RMSE_ukf     = mean(rmsError_ukf);
mean_RMSE_pf      = mean(rmsError_pf);


% calculate variance of RMSE errors
var_RMSE_ekf     = var(rmsError_ekf);
var_RMSE_ukf     = var(rmsError_ukf);
var_RMSE_pf      = var(rmsError_pf);


% calculate mean of execution time
mean_time_ekf      = mean(time_ekf);
mean_time_ukf      = mean(time_ukf);
mean_time_pf      = mean(time_pf);

% display final results

disp(' ');
disp(' ');
disp('************* FINAL RESULTS *****************');
disp(' ');
disp('RMSE : mean and variance');
disp('---------');
disp(' ');
disp(['EKF          = ' num2str(mean_RMSE_ekf) ' (' num2str(var_RMSE_ekf) ')']);
disp(['UKF          = ' num2str(mean_RMSE_ukf) ' (' num2str(var_RMSE_ukf) ')']);
disp(['PF           = ' num2str(mean_RMSE_pf) ' (' num2str(var_RMSE_pf) ')']);


disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['EKF           = ' num2str(mean_time_ekf)]);
disp(['UKF           = ' num2str(mean_time_ukf)]);
disp(['PF           = ' num2str(mean_time_pf)]);
disp(' ');

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

break;

% This is an alternative way of plotting the 3D stuff:
% Somewhere in between lies the best way!
figure(3)
clf;
support=[-1:.1:2];
NN=50;
extPlot=zeros(10*61,1);
for t=6:5:T,
  [r,d]=hist(yPred_pf(t,:),support);
  r=r/sum(r);
  for i=1:1:61,
    for j=1:1:NN,
      extPlot(NN*i-NN+1:i*NN) = r(i);
    end;
  end;
  d= linspace(-5,25,length(extPlot));
  plot3(d,t*ones(size(extPlot)),extPlot,'r')
  hold on;
end;