upf_demo.m

无忌卡尔曼粒子滤波

    muPred_pfekf(t) = feval('ffun',xparticle_pfekf(t-1,i),t);
    Jx = 0.5;                                 % Jacobian for ffun.
    PPred_pfekf(t) = Q_pfekf + Jx*Pparticle_pfekf(t-1,i)*Jx'; 
    yPredTmp = feval('hfun',muPred_pfekf(t),t);
    if t<=30,
      Jy = 2*0.2*muPred_pfekf(t);                     % Jacobian for hfun.
    else
      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 benchmarking

for 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)]=ukf1(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)]=ukf1(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.