main.m

迭代扩展卡尔曼滤波

% The iterated extended kalman particle filter
%writed by Li Liangqun
% DATE:  August 2005

clear all;
clc;
% 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-4;              % 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-4;                   % EKF's measurement noise variance. 
Q = 3/4;                    % EKF's process noise variance.
P0 = 3/4;                   % EKF's initial variance of the states.

N = 20;                     % Number of particles.

Q_pfekf = 10*3/4;
R_pfekf = 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_pfiekf  = zeros(1,no_of_runs);

time_pfiekf  = zeros(1,no_of_runs);

xparticle_pfiekf1 =zeros(T,no_of_runs);

x1=zeros(T,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;  
x1(:,j)=x;

% INITIALISATION:
% ==============
xparticle_pfiekf = 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_pfiekf = P0*ones(T,N);     % Particles for the covariance of x.
xparticlePred_pfiekf = ones(T,N);    % One-step-ahead predicted values of the states.
PparticlePred_pfiekf = ones(T,N);    % One-step-ahead predicted values of P.
yPred_pfiekf = ones(T,N);            % One-step-ahead predicted values of y.
w = ones(T,N);                       % Importance weights.
mu_pfiekf = ones(T,1);               % EKF estimate of the mean of the states.

error=0;

disp(' ');

tic;

for t=2:T,    

  % PREDICTION STEP:
  % ================ 
  % We use the UKF as proposal.
  for i=1:N,
    % Call Unscented Kalman Filter
    [mu_pfiekf(t,i),PparticlePred_pfiekf(t,i)]=iekf(xparticle_pfiekf(t-1,i),Pparticle_pfiekf(t-1,i),Q,'ffun',y(t),R,'hfun',t);
    xparticlePred_pfiekf(t,i) = mu_pfiekf(t,i) + sqrtm(PparticlePred_pfiekf(t,i))*randn(1,1);
  end;

  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  
  for i=1:N,
    yPred_pfiekf(t,i) = feval('hfun',xparticlePred_pfiekf(t,i),t);        
    lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfiekf(t,i))^(2)))+1e-99;
    prior = ((xparticlePred_pfiekf(t,i)-xparticle_pfiekf(t-1,i))^(g1-1)) ...
		 * exp(-g2*(xparticlePred_pfiekf(t,i)-xparticle_pfiekf(t-1,i)));
    proposal = inv(sqrt(PparticlePred_pfiekf(t,i))) * ...
	       exp(-0.5*inv(PparticlePred_pfiekf(t,i)) *((xparticlePred_pfiekf(t,i)-mu_pfiekf(t,i))^(2)));
    w(t,i) = lik*prior/proposal;      
  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!
    outIndex = residualR(1:N,w(t,:)');        % Residual resampling.
    
  xparticle_pfiekf(t,:) = xparticlePred_pfiekf(t,outIndex); % Keep particles with
                                              % resampled indices.
  Pparticle_pfiekf(t,:) = PparticlePred_pfiekf(t,outIndex);  
  
end;   % End of t loop.

time_pfiekf(j) = toc;
%%%%%%%%%%%%%%%%%%%%  PLOT THE RESULTS  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%  ================  %%%%%%%%%%%%%%%%%%%%%

xparticle_pfiekf1(:,j)=mean(xparticle_pfiekf(:,:)')';

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

rmsError_pfiekf(j)  = sqrt(inv(T)*sum((x'-mean(xparticle_pfiekf')).^(2)));

disp(' ');
disp('Root mean square (RMS) errors');
disp('-----------------------------');
disp(' ');
disp(['PF-IEKF      = ' num2str(rmsError_pfiekf(j))]);

disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['PF-IEKF-     = ' num2str(time_pfiekf(j))]);
disp(' ');

drawnow;

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

end    % Main loop (for j...)

% calculate mean of RMSE errors
mean_RMSE_pfiekf = mean(rmsError_pfiekf);

% calculate variance of RMSE errors
var_RMSE_pfiekf = var(rmsError_pfiekf);

% calculate mean of execution time
mean_time_pfiekf = mean(time_pfiekf);

% display final results

disp(' ');
disp(' ');
disp('************* FINAL RESULTS *****************');
disp(' ');
disp('RMSE : mean and variance');
disp('---------');
disp(' ');
disp(['PF-IEKF      = ' num2str(mean_RMSE_pfiekf) ' (' num2str(var_RMSE_pfiekf) ')']);

disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['PF-IEKF      = ' num2str(mean_time_pfiekf)]);
disp(' ');

%*************************************************************************
rms4=zeros(60,1);



    for j=1:no_of_runs
        rms4=rms4+(x1(:,j)-xparticle_pfiekf1(:,j)).^(2);
    end
    

rms4=sqrt(rms4/no_of_runs);
figure
plot(1:T,rms4,'-*','lineWidth',1)