pf_epf_upf_demo.asv

PF,EPF,UPF的对比仿真代码

  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_pfekf(t,:) = xparticlePred_pfekf(t,outIndex); % Keep particles with
                                              % resampled indices.
  Pparticle_pfekf(t,:) = PparticlePred_pfekf(t,outIndex);  
  
end;   % End of t loop.

time_pfekf(j) = toc;



%%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ======== UKF proposal ========  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
xparticle_pfukf = 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_pfukf = P0*ones(T,N);     % Particles for the covariance of x.
xparticlePred_pfukf = ones(T,N);    % One-step-ahead predicted values of the states.
PparticlePred_pfukf = ones(T,N);    % One-step-ahead predicted values of P.
yPred_pfukf = ones(T,N);            % One-step-ahead predicted values of y.
w = ones(T,N);                      % Importance weights.
mu_pfukf = ones(T,1);               % EKF estimate of the mean of the states.

error=0;

disp(' ');

tic;

for t=2:T,    
  fprintf('run = %i / %i :  PF-UKF : t = %i / %i  \r',j,no_of_runs,t,T);
  fprintf('\n')
  
  % PREDICTION STEP:
  % ================ 
  % We use the UKF as proposal.
  for i=1:N,
    % Call Unscented Kalman Filter
    [mu_pfukf(t,i),PparticlePred_pfukf(t,i)]=ukf(xparticle_pfukf(t-1,i),Pparticle_pfukf(t-1,i),[],Q_pfukf,'ukf_ffun',y(t),R_pfukf,'ukf_hfun',t,alpha,beta,kappa);
    xparticlePred_pfukf(t,i) = mu_pfukf(t,i) + sqrtm(PparticlePred_pfukf(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_pfukf(t,i) = feval('hfun',xparticlePred_pfukf(t,i),t);        
    lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfukf(t,i))^(2)))+1e-99;
    prior = ((xparticlePred_pfukf(t,i)-xparticle_pfukf(t-1,i))^(g1-1)) ...
		 * exp(-g2*(xparticlePred_pfukf(t,i)-xparticle_pfukf(t-1,i)));
    proposal = inv(sqrt(PparticlePred_pfukf(t,i))) * ...
	       exp(-0.5*inv(PparticlePred_pfukf(t,i)) *((xparticlePred_pfukf(t,i)-mu_pfukf(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!
  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_pfukf(t,:) = xparticlePred_pfukf(t,outIndex); % Keep particles with
                                              % resampled indices.
  Pparticle_pfukf(t,:) = PparticlePred_pfukf(t,outIndex);  
  
end;   % End of t loop.

time_pfukf(j) = toc;








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

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




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


rmsError_pf(j)      = sqrt(inv(T)*sum((x'-mean(xparticle_pf')).^(2)));
rmsError_pfekf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfekf')).^(2)));
rmsError_pfukf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfukf')).^(2)));


disp(' ');
disp('Root mean square (RMS) errors');
disp('-----------------------------');
disp(' ');

disp(['PF           = ' num2str(rmsError_pf(j))]);
disp(['PF-EKF       = ' num2str(rmsError_pfekf(j))]);
disp(['PF-UKF       = ' num2str(rmsError_pfukf(j))]);


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

drawnow;

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

end    % Main loop (for j...)

% calculate mean of RMSE errors
mean_RMSE_pf      = mean(rmsError_pf);
mean_RMSE_pfekf   = mean(rmsError_pfekf);
mean_RMSE_pfukf   = mean(rmsError_pfukf);

% calculate variance of RMSE errors
var_RMSE_pf      = var(rmsError_pf);
var_RMSE_pfekf   = var(rmsError_pfekf);
var_RMSE_pfukf   = var(rmsError_pfukf);

% calculate mean of execution time
mean_time_pf      = mean(time_pf);
mean_time_pfekf   = mean(time_pfekf);
mean_time_pfukf   = mean(time_pfukf);
% display final results

disp(' ');
disp(' ');
disp('************* FINAL RESULTS *****************');
disp(' ');
disp('RMSE : mean and variance');
disp('---------');
disp(' ');
disp(['PF           = ' num2str(mean_RMSE_pf) ' (' num2str(var_RMSE_pf) ')']);
disp(['PF-EKF       = ' num2str(mean_RMSE_pfekf) ' (' num2str(var_RMSE_pfekf) ')']);
disp(['PF-UKF       = ' num2str(mean_RMSE_pfukf) ' (' num2str(var_RMSE_pfukf) ')']);

disp(' ');
disp(' ');
disp('Execution time  (seconds)');
disp('-------------------------');
disp(' ');
disp(['PF           = ' num2str(mean_time_pf)]);
disp(['PF-EKF       = ' num2str(mean_time_pfekf)]);
disp(['PF-UKF       = ' num2str(mean_time_pfukf)]);
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;