previousPResukfMC(t,:) = previousPukfMC(t,outIndex);  % Resample particles  at t-1.
   
  % METROPOLIS-HASTINGS STEP:
  % ========================
  u=rand(N,1); 
  accepted=0;
  rejected=0;
  for i=1:N,   
     % Call Unscented Kalman Filter
    [muProp,PProp]=ukf1(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)
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;

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 on
ylabel('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 on
ylabel('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 on
ylabel('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 on
ylabel('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 on
ylabel('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 on
ylabel('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 errors
mean_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 errors
var_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 time
mean_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 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(['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;