fpfeke.m

upf滤波算法源程序

PPred_pfekf = ones(T,1);            % EKF O-s-a estimate of the variance of the states.
mu_pfekf = ones(T,1);               % EKF estimate of the mean of the states.
P_pfekf = P0*ones(T,1);             % EKF estimate of the variance of the states.

disp(' ');

tic;                                % Initialize timer for benchmarking

for t=2:T,    

  
  % PREDICTION STEP:
  % ================ 
  % We use the EKF as proposal.
  for i=1:N,
    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  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%  ======== FEKF proposal ========  %%%%%%%%%%%%%%%%%%%%%

% INITIALISATION:
% ==============
Fxparticle_pfekf = 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.
FPparticle_pfekf = P0*ones(T,N);     % Particles for the covariance of x.
Fpointest_pfekf=ones(T,N);
FxparticlePred_pfekf = ones(T,N);    % One-step-ahead predicted values of the states.
FPparticlePred_pfekf = ones(T,N);    % One-step-ahead predicted values of P.
FyPred_pfekf = ones(T,N);            % One-step-ahead predicted values of y.
w = ones(T,N);                      % Importance weights.
FmuPred_pfekf = ones(T,1);           % EKF O-s-a estimate of the mean of the states.
FPPred_pfekf = ones(T,1);            % EKF O-s-a estimate of the variance of the states.
Fmu_pfekf = ones(T,1);               % EKF estimate of the mean of the states.
FP_pfekf = P0*ones(T,1);             % EKF estimate of the variance of the states.
Fmean_pfekf=ones(T,1);               % EKF estimate of the mean of the states
e=0.96;
HH=1-((3*e-1)/2*e)^2;
a=sqrt(1-HH^2);
disp(' ');

tic;                                % Initialize timer for benchmarking

for t=2:T,    

  Fmean_pfekf(t-1)=mean(Fxparticle_pfekf(t-1,:));
  
  % PREDICTION STEP:
  % ================ 
  % We use the EKF as proposal.
  for i=1:N,
      a1=a*Fxparticle_pfekf(t-1,i)+(1-a)*Fmean_pfekf(t-1);
      a2=feval('ffun',a1,t);
      Fpointest_pfekf(t,i)=feval('hfun',a2,t);
      %begin the ekf
    FmuPred_pfekf(t) = feval('ffun',Fxparticle_pfekf(t-1,i),t);
    Jx = 0.5;                                 % Jacobian for ffun.
    FPPred_pfekf(t) = Q_pfekf + Jx*FPparticle_pfekf(t-1,i)*Jx'; 
    yPredTmp = feval('hfun',FmuPred_pfekf(t),t);
    if t<=30,
      Jy = 2*0.2*FmuPred_pfekf(t);                     % Jacobian for hfun.
    else
      Jy = 0.5;
    end;
    M = R_pfekf + Jy*FPPred_pfekf(t)*Jy';                  % Innovations covariance.
    K = FPPred_pfekf(t)*Jy'*inv(M);                  % Kalman gain.
    Fmu_pfekf(t,i) = FmuPred_pfekf(t) + K*(y(t)-yPredTmp); % Mean of proposal.
    FP_pfekf(t) = FPPred_pfekf(t) - K*Jy*FPPred_pfekf(t);          % Variance of proposal.
    FxparticlePred_pfekf(t,i) = Fmu_pfekf(t,i) + sqrtm(FP_pfekf(t))*randn(1,1);
    FPparticlePred_pfekf(t,i) = FP_pfekf(t);
  end;

  % EVALUATE IMPORTANCE WEIGHTS:
  % ============================
  % For our choice of proposal, the importance weights are give by:  
  for i=1:N,
    FyPred_pfekf(t,i) = feval('hfun',FxparticlePred_pfekf(t,i),t);        
    lik =  exp(-0.5*inv(sigma)*((y(t)-FyPred_pfekf(t,i))^(2)))+1e-99;
    prior = exp(-0.5*inv(sigma)*((y(t)-Fpointest_pfekf(t,i))^(2)))+1e-99;
    w(t,i) = lik*prior;      
  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;
  Fxparticle_pfekf(t,:) = FxparticlePred_pfekf(t,outIndex); % Keep particles with
                                              % resampled indices.
  FPparticle_pfekf(t,:) = FPparticlePred_pfekf(t,outIndex);  
  
end;   % End of t loop.

Ftime_pfekf(j) = toc;
%%%%%%%%%%%%%%%%%%%%%  PLOT THE RESULTS  %%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%  ================  %%%%%%%%%%%%%%%%%%%%%



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-- 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_pfekf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfekf')).^(2)));
FrmsError_pfekf(j)   = sqrt(inv(T)*sum((x'-mean(Fxparticle_pfekf')).^(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-EKF       = ' num2str(rmsError_pfekf(j))]);
disp(['FPF-EKF       = ' num2str(FrmsError_pfekf(j))]);

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

disp(['PF-EKF       = ' num2str(time_pfekf(j))]);
disp(['fPF-EKF       = ' num2str(Ftime_pfekf(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_pfekf   = mean(rmsError_pfekf);
Fmean_RMSE_pfekf   = mean(FrmsError_pfekf);

% 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_pfekf   = var(rmsError_pfekf);
Fvar_RMSE_pfekf   = var(FrmsError_pfekf);

% 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-EKF       = ' num2str(mean_RMSE_pfekf) ' (' num2str(var_RMSE_pfekf) ')']);
disp(['FPF-EKF       = ' num2str(Fmean_RMSE_pfekf) ' (' num2str(Fvar_RMSE_pfekf) ')']);