% METROPOLIS-HASTINGS STEP:  % ========================  u=rand(N,1);   accepted=0;  rejected=0;  for i=1:N,        % Call Unscented Kalman Filter    [muProp,PProp]=ukf(previousXResukfMC(t,i),previousPResukfMC(t,i),[],Q_pfukf,'ukf_ffun',y(t),R_pfukf,'ukf_hfun',t,alpha,beta,kappa);        xparticleProp = muProp + sqrtm(PProp)*randn(1,1);    PparticleProp = PProp;       mProp = feval('hfun',xparticleProp,t);            likProp = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-mProp)^(2)))+1e-99;    priorProp = ((xparticleProp-previousXResukfMC(t,i))^(g1-1)) ...		 * exp(-g2*(xparticleProp-previousXResukfMC(t,i)));    proposalProp = inv(sqrt(PparticleProp)) * ...	       exp(-0.5*inv(PparticleProp) *( ...					      (xparticleProp-muProp)^(2)));    m = feval('hfun',xparticle_pfukfMC(t,i),t);            lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-m)^(2)))+1e-99;    prior = ((xparticle_pfukfMC(t,i)-previousXResukfMC(t,i))^(g1-1)) ...		 * exp(-g2*(xparticle_pfukfMC(t,i)-previousXResukfMC(t,i)));    proposal = inv(sqrt(Pparticle_pfukfMC(t,i))) * ...	       exp(-0.5*inv(Pparticle_pfukfMC(t,i)) *((xparticle_pfukfMC(t,i)-muProp)^(2)));    ratio = (likProp*priorProp*proposal)/(lik*prior*proposalProp);    acceptance = min(1,ratio);    if u(i,1) <= acceptance       xparticle_pfukfMC(t,i) = xparticleProp;      Pparticle_pfukfMC(t,i) = PparticleProp;      accepted=accepted+1;    else      xparticle_pfukfMC(t,i) = xparticle_pfukfMC(t,i);       Pparticle_pfukfMC(t,i) = Pparticle_pfukfMC(t,i);        rejected=rejected+1;    end;  end;   % End of MCMC loop. end;   % End of t loop.time_pfukfMC(j) = toc;%%%%%%%%%%%%%%%%%%%%%  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);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:o','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([p0 p1 p4 p5 p6],'Noisy observations','True x','PF estimate','PF-EKF estimate','PF-UKF estimate');xlabel('Time','fontsize',15)zoom on;title('Filter estimates (posterior means) vs. True state','fontsize',15)figure(2)clfsubplot(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;if (1),figure(3)clf;% Plot predictive distribution of y:subplot(231);domain = zeros(T,1);range = zeros(T,1);thex=[-3:.1:15];hold onylabel('Time (t)','fontsize',15)xlabel('y_t','fontsize',15)zlabel('p(y_t|y_{t-1})','fontsize',15)title('Particle Filter','fontsize',15);%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(yPred_pf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');% Plot posterior distribution of x:subplot(234);domain = zeros(T,1);range = zeros(T,1);thex=[0:.1:10];hold onylabel('Time (t)','fontsize',15)xlabel('x_t','fontsize',15)zlabel('p(x_t|y_t)','fontsize',15)%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(xparticle_pf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');% Plot predictive distribution of y:subplot(232);domain = zeros(T,1);range = zeros(T,1);thex=[-3:.1:15];hold onylabel('Time (t)','fontsize',15)xlabel('y_t','fontsize',15)zlabel('p(y_t|y_{t-1})','fontsize',15)title('Particle Filter (EKF proposal)','fontsize',15);%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(yPred_pfekf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');% Plot posterior distribution of x:subplot(235);domain = zeros(T,1);range = zeros(T,1);thex=[0:.1:10];hold onylabel('Time (t)','fontsize',15)xlabel('x_t','fontsize',15)zlabel('p(x_t|y_t)','fontsize',15)%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(xparticle_pfekf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');% Plot predictive distribution of y:subplot(233);domain = zeros(T,1);range = zeros(T,1);thex=[-3:.1:15];hold onylabel('Time (t)','fontsize',15)xlabel('y_t','fontsize',15)zlabel('p(y_t|y_{t-1})','fontsize',15)title('Particle Filter (UKF proposal)','fontsize',15);%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(yPred_pfukf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');% Plot posterior distribution of x:subplot(236);domain = zeros(T,1);range = zeros(T,1);thex=[0:.1:10];hold onylabel('Time (t)','fontsize',15)xlabel('x_t','fontsize',15)zlabel('p(x_t|y_t)','fontsize',15)%v=[0 1];%caxis(v);for t=6:5:T,  [range,domain]=hist(xparticle_pfukf(t,:),thex);  waterfall(domain,t,range/sum(range));end;view(-30,80);rotate3d on;a=get(gca);set(gca,'ygrid','off');end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%-- 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)));rmsError_pfMC(j)    = sqrt(inv(T)*sum((x'-mean(xparticle_pfMC')).^(2)));rmsError_pfekf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfekf')).^(2)));rmsError_pfekfMC(j) = sqrt(inv(T)*sum((x'-mean(xparticle_pfekfMC')).^(2)));rmsError_pfukf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfukf')).^(2)));rmsError_pfukfMC(j) = sqrt(inv(T)*sum((x'-mean(xparticle_pfukfMC')).^(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(['PF-MCMC      = ' num2str(rmsError_pfMC(j))]);disp(['PF-EKF       = ' num2str(rmsError_pfekf(j))]);disp(['PF-EKF-MCMC  = ' num2str(rmsError_pfekfMC(j))]);disp(['PF-UKF       = ' num2str(rmsError_pfukf(j))]);disp(['PF-UKF-MCMC  = ' num2str(rmsError_pfukfMC(j))]);disp(' ');disp(' ');disp('Execution time  (seconds)');disp('-------------------------');disp(' ');disp(['PF           = ' num2str(time_pf(j))]);disp(['PF-MCMC      = ' num2str(time_pfMC(j))]);disp(['PF-EKF       = ' num2str(time_pfekf(j))]);disp(['PF-EKF-MCMC  = ' num2str(time_pfekfMC(j))]);disp(['PF-UKF       = ' num2str(time_pfukf(j))]);disp(['PF-UKF-MCMC  = ' num2str(time_pfukfMC(j))]);disp(' ');drawnow;%*************************************************************************end    % Main loop (for j...)% calculate mean of RMSE errorsmean_RMSE_ekf     = mean(rmsError_ekf);mean_RMSE_ukf     = mean(rmsError_ukf);mean_RMSE_pf      = mean(rmsError_pf);mean_RMSE_pfMC    = mean(rmsError_pfMC);mean_RMSE_pfekf   = mean(rmsError_pfekf);mean_RMSE_pfekfMC = mean(rmsError_pfekfMC);mean_RMSE_pfukf   = mean(rmsError_pfukf);mean_RMSE_pfukfMC = mean(rmsError_pfukfMC);% calculate variance of RMSE errorsvar_RMSE_ekf     = var(rmsError_ekf);var_RMSE_ukf     = var(rmsError_ukf);var_RMSE_pf      = var(rmsError_pf);var_RMSE_pfMC    = var(rmsError_pfMC);var_RMSE_pfekf   = var(rmsError_pfekf);var_RMSE_pfekfMC = var(rmsError_pfekfMC);var_RMSE_pfukf   = var(rmsError_pfukf);var_RMSE_pfukfMC = var(rmsError_pfukfMC);% calculate mean of execution timemean_time_pf      = mean(time_pf);mean_time_pfMC    = mean(time_pfMC);mean_time_pfekf   = mean(time_pfekf);mean_time_pfekfMC = mean(time_pfekfMC);mean_time_pfukf   = mean(time_pfukf);mean_time_pfukfMC = mean(time_pfukfMC);% display final resultsdisp(' ');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(['PF-MCMC      = ' num2str(mean_RMSE_pfMC) ' (' num2str(var_RMSE_pfMC) ')']);disp(['PF-EKF       = ' num2str(mean_RMSE_pfekf) ' (' num2str(var_RMSE_pfekf) ')']);disp(['PF-EKF-MCMC  = ' num2str(mean_RMSE_pfekfMC) ' (' num2str(var_RMSE_pfekfMC) ')']);disp(['PF-UKF       = ' num2str(mean_RMSE_pfukf) ' (' num2str(var_RMSE_pfukf) ')']);disp(['PF-UKF-MCMC  = ' num2str(mean_RMSE_pfukfMC) ' (' num2str(var_RMSE_pfukfMC) ')']);disp(' ');disp(' ');disp('Execution time  (seconds)');disp('-------------------------');disp(' ');disp(['PF           = ' num2str(mean_time_pf)]);disp(['PF-MCMC      = ' num2str(mean_time_pfMC)]);disp(['PF-EKF       = ' num2str(mean_time_pfekf)]);disp(['PF-EKF-MCMC  = ' num2str(mean_time_pfekfMC)]);disp(['PF-UKF       = ' num2str(mean_time_pfukf)]);disp(['PF-UKF-MCMC  = ' num2str(mean_time_pfukfMC)]);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;