upf_demos粒子滤波

% PURPOSE : Demonstrate the differences between the following filters on the same problem: % % 1) Extended Kalman Filter (EKF) % 2) Unscented Kalman Filter (UKF) % 3) Particle Filter (PF) % 4) PF with EKF proposal (PFEKF) % 5) PF with UKF proposal (PFUKF) % For more details refer to: % AUTHORS : Nando de Freitas (jfgf@cs.berkeley.edu) % Rudolph van der Merwe (rvdmerwe@ece.ogi.edu) % DATE : 17 August 2000 clear all; clc; echo off; path('./ukf',path); % INITIALISATION AND PARAMETERS: % ============================== no_of_runs = 100; % number of experiments to generate statistical % averages doPlot = 0; % 1 plot online. 0 = only plot at the end. sigma = 1e-5; % Variance of the Gaussian measurement noise. g1 = 3; % Paramater of Gamma transition prior. g2 = 2; % Parameter of Gamman transition prior. % Thus mean = 3/2 and var = 3/4. T = 60; % Number of time steps. R = 1e-5; % EKF's measurement noise variance. Q = 3/4; % EKF's process noise variance. P0 = 3/4; % EKF's initial variance of the states. N = 200; % Number of particles. resamplingScheme = 1; % The possible choices are % systematic sampling (2), % residual (1) % and multinomial (3). % They're all O(N) algorithms. Q_pfekf = 10*3/4; R_pfekf = 1e-1; Q_pfukf = 2*3/4; R_pfukf = 1e-1; alpha = 1; % UKF : point scaling parameter beta = 0; % UKF : scaling parameter for higher order terms of Taylor series expansion kappa = 2; % UKF : sigma point selection scaling parameter (best to leave this = 0) %************************************************************************************** % SETUP BUFFERS TO STORE PERFORMANCE RESULTS % ========================================== rmsError_ekf = zeros(1,no_of_runs); rmsError_ukf = zeros(1,no_of_runs); rmsError_pf = zeros(1,no_of_runs); rmsError_pfMC = zeros(1,no_of_runs); rmsError_pfekf = zeros(1,no_of_runs); rmsError_pfekfMC = zeros(1,no_of_runs); rmsError_pfukf = zeros(1,no_of_runs); rmsError_pfukfMC = zeros(1,no_of_runs); time_pf = zeros(1,no_of_runs); time_pfMC = zeros(1,no_of_runs); time_pfekf = zeros(1,no_of_runs); time_pfekfMC = zeros(1,no_of_runs); time_pfukf = zeros(1,no_of_runs); time_pfukfMC = zeros(1,no_of_runs); %************************************************************************************** % MAIN LOOP for j=1:no_of_runs, rand('state',sum(100*clock)); % Shuffle the pack! randn('state',sum(100*clock)); % Shuffle the pack! % GENERATE THE DATA: % ================== x = zeros(T,1); y = zeros(T,1); processNoise = zeros(T,1); measureNoise = zeros(T,1); x(1) = 1; % Initial state. for t=2:T processNoise(t) = gengamma(g1,g2); measureNoise(t) = sqrt(sigma)*randn(1,1); x(t) = feval('ffun',x(t-1),t) +processNoise(t); % Gamma transition prior. y(t) = feval('hfun',x(t),t) + measureNoise(t); % Gaussian likelihood. end; % PLOT THE GENERATED DATA: % ======================== figure(1) clf; plot(1:T,x,'r',1:T,y,'b'); ylabel('Data','fontsize',15); xlabel('Time','fontsize',15); legend('States (x)','Observations(y)'); %%%%%%%%%%%%%%% PERFORM EKF and UKF ESTIMATION %%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% ============================== %%%%%%%%%%%%%%%%%%%%% % INITIALISATION: % ============== mu_ekf = ones(T,1); % EKF estimate of the mean of the states. P_ekf = P0*ones(T,1); % EKF estimate of the variance of the states. mu_ukf = mu_ekf; % UKF estimate of the mean of the states. P_ukf = P_ekf; % UKF estimate of the variance of the states. yPred = ones(T,1); % One-step-ahead predicted values of y. mu_ekfPred = ones(T,1); % EKF O-s-a estimate of the mean of the states. PPred = ones(T,1); % EKF O-s-a estimate of the variance of the states. disp(' '); for t=2:T, fprintf('run = %i / %i : EKF & UKF : t = %i / %i \r',j,no_of_runs,t,T); fprintf('\n') % PREDICTION STEP: % ================ mu_ekfPred(t) = feval('ffun',mu_ekf(t-1),t); Jx = 0.5; % Jacobian for ffun. PPred(t) = Q + Jx*P_ekf(t-1)*Jx'; % CORRECTION STEP: % ================ yPred(t) = feval('hfun',mu_ekfPred(t),t); if t<=30, Jy = 2*0.2*mu_ekfPred(t); % Jacobian for hfun. else Jy = 0.5; % Jy = cos(mu_ekfPred(t))/2; % Jy = 2*mu_ekfPred(t)/4; % Jacobian for hfun. end; M = R + Jy*PPred(t)*Jy'; % Innovations covariance. K = PPred(t)*Jy'*inv(M); % Kalman gain. mu_ekf(t) = mu_ekfPred(t) + K*(y(t)-yPred(t)); P_ekf(t) = PPred(t) - K*Jy*PPred(t); % Full Unscented Kalman Filter step % ================================= [mu_ukf(t),P_ukf(t)]=ukf(mu_ukf(t-1),P_ukf(t-1),[],Q,'ukf_ffun',y(t),R,'ukf_hfun',t,alpha,beta,kappa); end; % End of t loop. %%%%%%%%%%%%%%% PERFORM SEQUENTIAL MONTE CARLO %%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%% ============================== %%%%%%%%%%%%%%%%%%%%% % INITIALISATION: % ============== xparticle_pf = 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. xparticlePred_pf = ones(T,N); % One-step-ahead predicted values of the states. yPred_pf = ones(T,N); % One-step-ahead predicted values of y. w = ones(T,N); % Importance weights. disp(' '); tic; % Initialize timer for benchmarking for t=2:T, fprintf('run = %i / %i : PF : t = %i / %i \r',j,no_of_runs,t,T); fprintf('\n') % PREDICTION STEP: % ================ % We use the transition prior as proposal. for i=1:N, xparticlePred_pf(t,i) = feval('ffun',xparticle_pf(t-1,i),t) + gengamma(g1,g2); end; % EVALUATE IMPORTANCE WEIGHTS: % ============================ % For our choice of proposal, the importance weights are give by: for i=1:N, yPred_pf(t,i) = feval('hfun',xparticlePred_pf(t,i),t); lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pf(t,i))^(2))) ... + 1e-99; % Deal with ill-conditioning. w(t,i) = lik; 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_pf(t,:) = xparticlePred_pf(t,outIndex); % Keep particles with % resampled indices. end; % End of t loop. time_pf(j) = toc; % How long did this take? %%%%%%%%%%%%%% PERFORM SEQUENTIAL MONTE CARLO WITH MCMC %%%%%%%%%%%%%%%% %%%%%%%%%%%%%% ======================================== %%%%%%%%%%%%%%%% % INITIALISATION: % ============== xparticle_pfMC = 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