Jy = 0.5;    end;    M = R_pfekf + Jy*PPred_pfekf(t)*Jy';                  % Innovations covariance.    K = PPred_pfekf(t)*Jy'*inv(M);                  % Kalman gain.    mu_pfekf(t,i) = muPred_pfekf(t) + K*(y(t)-yPredTmp); % Mean of proposal.    P_pfekf(t) = PPred_pfekf(t) - K*Jy*PPred_pfekf(t);          % Variance of proposal.    xparticlePred_pfekf(t,i) = mu_pfekf(t,i) + sqrtm(P_pfekf(t))*randn(1,1);    PparticlePred_pfekf(t,i) = P_pfekf(t);  end;  % EVALUATE IMPORTANCE WEIGHTS:  % ============================  % For our choice of proposal, the importance weights are give by:    for i=1:N,    yPred_pfekf(t,i) = feval('hfun',xparticlePred_pfekf(t,i),t);            lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfekf(t,i))^(2)))+1e-99;    prior = ((xparticlePred_pfekf(t,i)-xparticle_pfekf(t-1,i))^(g1-1)) ...		 * exp(-g2*(xparticlePred_pfekf(t,i)-xparticle_pfekf(t-1,i)));    proposal = inv(sqrt(PparticlePred_pfekf(t,i))) * ...	       exp(-0.5*inv(PparticlePred_pfekf(t,i)) *((xparticlePred_pfekf(t,i)-mu_pfekf(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_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 WITH MCMC  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  ======== EKF proposal ==================  %%%%%%%%%%%%%%%%% INITIALISATION:% ==============xparticle_pfekfMC = 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_pfekfMC = P0*ones(T,N);     % Particles for the covariance of x.xparticlePred_pfekfMC = ones(T,N);    % One-step-ahead predicted values of the states.PparticlePred_pfekfMC = ones(T,N);    % One-step-ahead predicted values of P.yPred_pfekfMC = ones(T,N);            % One-step-ahead predicted values of y.w = ones(T,N);                        % Importance weights.muPred_pfekfMC = ones(T,1);           % EKF O-s-a estimate of the mean of the states.PPred_pfekfMC = ones(T,1);            % EKF O-s-a estimate of the variance of the states.mu_pfekfMC = ones(T,1);               % EKF estimate of the mean of the states.P_pfekfMC = P0*ones(T,1);             % EKF estimate of the variance of the states.previousXekfMC = ones(T,N);           % Particles at the previous time step. previousXResekfMC = ones(T,N);        % Resampled previousX.previousPekfMC = ones(T,N);           % Covariance particles at the previous time step. previousPResekfMC = ones(T,N);        % Resampled previousP.disp(' ');tic;                                % Initialize timer for benchmarkingfor t=2:T,      fprintf('run = %i / %i :  PF-EKF-MCMC : t = %i / %i  \r',j,no_of_runs,t,T);  fprintf('\n')    % PREDICTION STEP:  % ================   % We use the EKF as proposal.  for i=1:N,    muPred_pfekfMC(t) = feval('ffun',xparticle_pfekfMC(t-1,i),t);    Jx = 0.5;                                 % Jacobian for ffun.    PPred_pfekfMC(t) = Q_pfekf + Jx*Pparticle_pfekfMC(t-1,i)*Jx';     yPredTmp = feval('hfun',muPred_pfekfMC(t),t);    if t<=30,      Jy = 2*0.2*muPred_pfekfMC(t);                     % Jacobian for hfun.    else      Jy = 0.5;    end;    M = R_pfekf + Jy*PPred_pfekfMC(t)*Jy';                  % Innovations covariance.    K = PPred_pfekfMC(t)*Jy'*inv(M);                  % Kalman gain.    mu_pfekfMC(t,i) = muPred_pfekfMC(t) + K*(y(t)-yPredTmp); % Mean of proposal.    P_pfekfMC(t) = PPred_pfekfMC(t) - K*Jy*PPred_pfekfMC(t);          % Variance of proposal.    xparticlePred_pfekfMC(t,i) = mu_pfekfMC(t,i) + sqrtm(P_pfekfMC(t))*randn(1,1);    PparticlePred_pfekfMC(t,i) = P_pfekfMC(t);  end;  previousXekfMC(t,:) = xparticle_pfekfMC(t-1,:);  % Store the particles at t-1.   previousPekfMC(t,:) = Pparticle_pfekfMC(t-1,:);  % Store the particles at t-1.       % EVALUATE IMPORTANCE WEIGHTS:  % ============================  % For our choice of proposal, the importance weights are give by:    for i=1:N,    yPred_pfekfMC(t,i) = feval('hfun',xparticlePred_pfekfMC(t,i),t);            lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfekfMC(t,i))^(2)))+1e-99;    prior = ((xparticlePred_pfekfMC(t,i)-xparticle_pfekfMC(t-1,i))^(g1-1)) ...		 * exp(-g2*(xparticlePred_pfekfMC(t,i)-xparticle_pfekfMC(t-1,i)));    proposal = inv(sqrt(PparticlePred_pfekfMC(t,i))) * ...	       exp(-0.5*inv(PparticlePred_pfekfMC(t,i)) *((xparticlePred_pfekfMC(t,i)-mu_pfekfMC(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_pfekfMC(t,:) = xparticlePred_pfekfMC(t,outIndex); % Keep particles with                                                         % resampled indices.  Pparticle_pfekfMC(t,:) = PparticlePred_pfekfMC(t,outIndex);    previousXResekfMC(t,:) = previousXekfMC(t,outIndex);  % Resample particles                                                        % at t-1.  previousPResekfMC(t,:) = previousPekfMC(t,outIndex);  % Resample particles                                                        % at t-1.     % METROPOLIS-HASTINGS STEP:  % ========================  u=rand(N,1);   accepted=0;  rejected=0;  for i=1:N,       muPred_ekfMCMC = feval('ffun',previousXResekfMC(t,i),t);    Jx = 0.5;                                     % Jacobian for ffun.    PPred_ekfMCMC = Q_pfekf + Jx*previousPResekfMC(t,i)*Jx';     yPredTmp = feval('hfun',muPred_ekfMCMC,t);    if t<=30,      Jy = 2*0.2*muPred_ekfMCMC;                     % Jacobian for hfun.    else      Jy = 0.5;    end;    M = R_pfekf + Jy*PPred_ekfMCMC*Jy';                  % Innovations covariance.    K = PPred_ekfMCMC*Jy'*inv(M);                  % Kalman gain.    muProp = muPred_ekfMCMC + K*(y(t)-yPredTmp);   % Mean of proposal.    PProp = PPred_ekfMCMC - K*Jy*PPred_ekfMCMC;          % Variance of proposal.    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-previousXResekfMC(t,i))^(g1-1)) ...		 * exp(-g2*(xparticleProp-previousXResekfMC(t,i)));    proposalProp = inv(sqrt(PparticleProp)) * ...	       exp(-0.5*inv(PparticleProp) *( ...					      (xparticleProp-muProp)^(2)));    m = feval('hfun',xparticle_pfekfMC(t,i),t);            lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-m)^(2)))+1e-99;    prior = ((xparticle_pfekfMC(t,i)-previousXResekfMC(t,i))^(g1-1)) ...		 * exp(-g2*(xparticle_pfekfMC(t,i)-previousXResekfMC(t,i)));    proposal = inv(sqrt(Pparticle_pfekfMC(t,i))) * ...	       exp(-0.5*inv(Pparticle_pfekfMC(t,i)) *((xparticle_pfekfMC(t,i)-muProp)^(2)));    ratio = (likProp*priorProp*proposal)/(lik*prior*proposalProp);    acceptance = min(1,ratio);    if u(i,1) <= acceptance       xparticle_pfekfMC(t,i) = xparticleProp;      Pparticle_pfekfMC(t,i) = PparticleProp;      accepted=accepted+1;    else      xparticle_pfekfMC(t,i) = xparticle_pfekfMC(t,i);       Pparticle_pfekfMC(t,i) = Pparticle_pfekfMC(t,i);        rejected=rejected+1;    end;  end;   % End of MCMC loop.end;   % End of t loop.time_pfekfMC(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;%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO WITH MCMC  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%  ============= UKF proposal =============  %%%%%%%%%%%%%%%%% INITIALISATION:% ==============xparticle_pfukfMC = 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_pfukfMC = P0*ones(T,N);     % Particles for the covariance of x.xparticlePred_pfukfMC = ones(T,N);    % One-step-ahead predicted values of the states.PparticlePred_pfukfMC = ones(T,N);    % One-step-ahead predicted values of P.yPred_pfukfMC = ones(T,N);            % One-step-ahead predicted values of y.w = ones(T,N);                        % Importance weights.mu_pfukfMC = ones(T,1);               % EKF estimate of the mean of the states.previousXukfMC = ones(T,N);           % Particles at the previous time step. previousXResukfMC = ones(T,N);        % Resampled previousX.previousPukfMC = ones(T,N);           % Covariance particles at the previous time step. previousPResukfMC = ones(T,N);        % Resampled previousP.error=0;disp(' ');tic;for t=2:T,      fprintf('run = %i / %i :  PF-UKF-MCMC : 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_pfukfMC(t,i),PparticlePred_pfukfMC(t,i)]=ukf(xparticle_pfukfMC(t-1,i),Pparticle_pfukfMC(t-1,i),[],Q_pfukf,'ukf_ffun',y(t),R_pfukf,'ukf_hfun',t,alpha,beta,kappa);    xparticlePred_pfukfMC(t,i) = mu_pfukfMC(t,i) + sqrtm(PparticlePred_pfukfMC(t,i))*randn(1,1);  end;    previousXukfMC(t,:) = xparticle_pfukfMC(t-1,:);  % Store the particles at t-1.   previousPukfMC(t,:) = Pparticle_pfukfMC(t-1,:);  % Store the particles at t-1.        % EVALUATE IMPORTANCE WEIGHTS:  % ============================  % For our choice of proposal, the importance weights are give by:    for i=1:N,    yPred_pfukfMC(t,i) = feval('hfun',xparticlePred_pfukfMC(t,i),t);            lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfukfMC(t,i))^(2)))+1e-99;    prior = ((xparticlePred_pfukfMC(t,i)-xparticle_pfukfMC(t-1,i))^(g1-1)) ...		 * exp(-g2*(xparticlePred_pfukfMC(t,i)-xparticle_pfukfMC(t-1,i)));    proposal = inv(sqrt(PparticlePred_pfukfMC(t,i))) * ...	       exp(-0.5*inv(PparticlePred_pfukfMC(t,i)) *((xparticlePred_pfukfMC(t,i)-mu_pfukfMC(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_pfukfMC(t,:) = xparticlePred_pfukfMC(t,outIndex); % Keep particles with                                              % resampled indices.  Pparticle_pfukfMC(t,:) = PparticlePred_pfukfMC(t,outIndex);     previousXResukfMC(t,:) = previousXukfMC(t,outIndex);  % Resample particles                                                        % at t-1.  previousPResukfMC(t,:) = previousPukfMC(t,outIndex);  % Resample particles                                                        % at t-1.