% The iterated extended kalman particle filter%writed by Li Liangqun% DATE:  August 2005clear all;clc;% INITIALISATION AND PARAMETERS:% ==============================no_of_runs =1;            % number of experiments to generate statistical                            % averagesdoPlot = 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 parameterbeta  = 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 LOOPfor 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 errorsmean_RMSE_pfiekf = mean(rmsError_pfiekf);% calculate variance of RMSE errorsvar_RMSE_pfiekf = var(rmsError_pfiekf);% calculate mean of execution timemean_time_pfiekf = mean(time_pfiekf);% display final resultsdisp(' ');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);figureplot(1:T,rms4,'-*','lineWidth',1)