demo_MC.m
上传用户:hfch80
上传日期:2007-10-25
资源大小:3637k
文件大小:19k
源码类别:

行业发展研究

开发平台:

Matlab

  1. % PURPOSE : Demonstrate the differences between the following filters on the same problem:
  2. %           
  3. %           1) Extended Kalman Filter  (EKF)
  4. %           2) Unscented Kalman Filter (UKF)
  5. %           3) Particle Filter         (PF)
  6. %           4) PF with EKF proposal    (PFEKF)
  7. %           5) PF with UKF proposal    (PFUKF)
  8. % For more details refer to:
  9. % AUTHORS  : Nando de Freitas      (jfgf@cs.berkeley.edu)
  10. %            Rudolph van der Merwe (rvdmerwe@ece.ogi.edu)
  11. % DATE     : 17 August 2000
  12. clear all;
  13. clc;
  14. echo off;
  15. path('./ukf',path);
  16. % INITIALISATION AND PARAMETERS:
  17. % ==============================
  18. no_of_runs = 10;            % number of experiments to generate statistical
  19.                             % averages
  20. doPlot = 0;                 % 1 plot online. 0 = only plot at the end.
  21. sigma =  1e-5;              % Variance of the Gaussian measurement noise.
  22. g1 = 3;                     % Paramater of Gamma transition prior.
  23. g2 = 2;                     % Parameter of Gamman transition prior.
  24.                             % Thus mean = 3/2 and var = 3/4.
  25. T = 60;                     % Number of time steps.
  26. R = 1e-5;                   % EKF's measurement noise variance. 
  27. Q = 3/4;                    % EKF's process noise variance.
  28. P0 = 3/4;                   % EKF's initial variance of the states.
  29. N = 200;                     % Number of particles.
  30. resamplingScheme = 1;       % The possible choices are
  31.                             % systematic sampling (2),
  32.                             % residual (1)
  33.                             % and multinomial (3). 
  34.                             % They're all O(N) algorithms. 
  35. Q_pfekf = 10*3/4;
  36. R_pfekf = 1e-1;
  37. Q_pfukf = 2*3/4;
  38. R_pfukf = 1e-1;
  39.     
  40. alpha = 1;                  % UKF : point scaling parameter
  41. beta  = 0;                  % UKF : scaling parameter for higher order terms of Taylor series expansion 
  42. kappa = 2;                  % UKF : sigma point selection scaling parameter (best to leave this = 0)
  43. %**************************************************************************************
  44. % SETUP BUFFERS TO STORE PERFORMANCE RESULTS
  45. % ==========================================
  46. rmsError_ekf      = zeros(1,no_of_runs);
  47. rmsError_ukf      = zeros(1,no_of_runs);
  48. rmsError_pf       = zeros(1,no_of_runs);
  49. rmsError_pfMC     = zeros(1,no_of_runs);
  50. rmsError_pfekf    = zeros(1,no_of_runs);
  51. rmsError_pfekfMC  = zeros(1,no_of_runs);
  52. rmsError_pfukf    = zeros(1,no_of_runs);
  53. rmsError_pfukfMC  = zeros(1,no_of_runs);
  54. time_pf       = zeros(1,no_of_runs);     
  55. time_pfMC     = zeros(1,no_of_runs);
  56. time_pfekf    = zeros(1,no_of_runs);
  57. time_pfekfMC  = zeros(1,no_of_runs);
  58. time_pfukf    = zeros(1,no_of_runs);
  59. time_pfukfMC  = zeros(1,no_of_runs);
  60. %**************************************************************************************
  61. % MAIN LOOP
  62. for j=1:no_of_runs,
  63.   rand('state',sum(100*clock));   % Shuffle the pack!
  64.   randn('state',sum(100*clock));   % Shuffle the pack!
  65.   
  66. % GENERATE THE DATA:
  67. % ==================
  68. x = zeros(T,1);
  69. y = zeros(T,1);
  70. processNoise = zeros(T,1);
  71. measureNoise = zeros(T,1);
  72. x(1) = 1;                         % Initial state.
  73. for t=2:T
  74.   processNoise(t) = gengamma(g1,g2);  
  75.   measureNoise(t) = sqrt(sigma)*randn(1,1);    
  76.   x(t) = feval('ffun',x(t-1),t) +processNoise(t);     % Gamma transition prior.  
  77.   y(t) = feval('hfun',x(t),t) + measureNoise(t);      % Gaussian likelihood.
  78. end;  
  79. % PLOT THE GENERATED DATA:
  80. % ========================
  81. figure(1)
  82. clf;
  83. plot(1:T,x,'r',1:T,y,'b');
  84. ylabel('Data','fontsize',15);
  85. xlabel('Time','fontsize',15);
  86. legend('States (x)','Observations(y)');
  87. %%%%%%%%%%%%%%%  PERFORM EKF and UKF ESTIMATION  %%%%%%%%%%%%%%%%%%%%%
  88. %%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%
  89. % INITIALISATION:
  90. % ==============
  91. mu_ekf = ones(T,1);     % EKF estimate of the mean of the states.
  92. P_ekf = P0*ones(T,1);   % EKF estimate of the variance of the states.
  93. mu_ukf = mu_ekf;        % UKF estimate of the mean of the states.
  94. P_ukf = P_ekf;          % UKF estimate of the variance of the states.
  95. yPred = ones(T,1);      % One-step-ahead predicted values of y.
  96. mu_ekfPred = ones(T,1); % EKF O-s-a estimate of the mean of the states.
  97. PPred = ones(T,1);      % EKF O-s-a estimate of the variance of the states.
  98. disp(' ');
  99. for t=2:T,    
  100.   fprintf('run = %i / %i :  EKF & UKF : t = %i / %i  r',j,no_of_runs,t,T);
  101.   fprintf('n')
  102.   
  103.   % PREDICTION STEP:
  104.   % ================ 
  105.   mu_ekfPred(t) = feval('ffun',mu_ekf(t-1),t);
  106.   Jx = 0.5;                             % Jacobian for ffun.
  107.   PPred(t) = Q + Jx*P_ekf(t-1)*Jx'; 
  108.   
  109.   % CORRECTION STEP:
  110.   % ================
  111.   yPred(t) = feval('hfun',mu_ekfPred(t),t);
  112.   if t<=30,
  113.     Jy = 2*0.2*mu_ekfPred(t);                 % Jacobian for hfun.
  114.   else
  115.     Jy = 0.5;
  116.   %  Jy = cos(mu_ekfPred(t))/2;
  117.   %   Jy = 2*mu_ekfPred(t)/4;                 % Jacobian for hfun. 
  118.   end;
  119.   M = R + Jy*PPred(t)*Jy';                 % Innovations covariance.
  120.   K = PPred(t)*Jy'*inv(M);                 % Kalman gain.
  121.   mu_ekf(t) = mu_ekfPred(t) + K*(y(t)-yPred(t));
  122.   P_ekf(t) = PPred(t) - K*Jy*PPred(t);
  123.   
  124.   % Full Unscented Kalman Filter step
  125.   % =================================
  126.   [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);  
  127.   
  128. end;   % End of t loop.
  129. %%%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO  %%%%%%%%%%%%%%%%%%%%%
  130. %%%%%%%%%%%%%%%  ==============================  %%%%%%%%%%%%%%%%%%%%%
  131. % INITIALISATION:
  132. % ==============
  133. xparticle_pf = ones(T,N);        % These are the particles for the estimate
  134.                                  % of x. Note that there's no need to store
  135.                                  % them for all t. We're only doing this to
  136.                                  % show you all the nice plots at the end.
  137. xparticlePred_pf = ones(T,N);    % One-step-ahead predicted values of the states.
  138. yPred_pf = ones(T,N);            % One-step-ahead predicted values of y.
  139. w = ones(T,N);                   % Importance weights.
  140. disp(' ');
  141.  
  142. tic;                             % Initialize timer for benchmarking
  143. for t=2:T,    
  144.   fprintf('run = %i / %i :  PF : t = %i / %i  r',j,no_of_runs,t,T);
  145.   fprintf('n')
  146.   
  147.   % PREDICTION STEP:
  148.   % ================ 
  149.   % We use the transition prior as proposal.
  150.   for i=1:N,
  151.     xparticlePred_pf(t,i) = feval('ffun',xparticle_pf(t-1,i),t) + gengamma(g1,g2);   
  152.   end;
  153.   % EVALUATE IMPORTANCE WEIGHTS:
  154.   % ============================
  155.   % For our choice of proposal, the importance weights are give by:  
  156.   for i=1:N,
  157.     yPred_pf(t,i) = feval('hfun',xparticlePred_pf(t,i),t);        
  158.     lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pf(t,i))^(2))) ...
  159.   + 1e-99; % Deal with ill-conditioning.
  160.     w(t,i) = lik;    
  161.   end;  
  162.   w(t,:) = w(t,:)./sum(w(t,:));                % Normalise the weights.
  163.   % if resamplingScheme == 1
  164.   % outIndex = residualR(1:N,w(t,:)'); 
  165.     
  166.     
  167.   % SELECTION STEP:
  168.   % ===============
  169.   % Here, we give you the choice to try three different types of
  170.   % resampling algorithms. Note that the code for these algorithms
  171.   % applies to any problem!
  172.   if resamplingScheme == 1
  173.     outIndex = residualR(1:N,w(t,:)');        % Residual resampling.
  174.   elseif resamplingScheme == 2
  175.     outIndex = systematicR(1:N,w(t,:)');      % Systematic resampling.
  176.   else  
  177.     outIndex = multinomialR(1:N,w(t,:)');     % Multinomial resampling.  
  178.   end;
  179.   xparticle_pf(t,:) = xparticlePred_pf(t,outIndex); % Keep particles with
  180.                                                     % resampled indices.
  181. end;   % End of t loop.
  182. time_pf(j) = toc;    % How long did this take?
  183. %%%%%%%%%%%%%%  PERFORM SEQUENTIAL MONTE CARLO WITH MCMC  %%%%%%%%%%%%%%%%
  184. %%%%%%%%%%%%%%  ========================================  %%%%%%%%%%%%%%%%
  185. % INITIALISATION:
  186. % ==============
  187. xparticle_pfMC = ones(T,N);      % These are the particles for the estimate
  188.                                  % of x. Note that there's no need to store
  189.                                  % them for all t. We're only doing this to
  190.                                  % show you all the nice plots at the end.
  191. xparticlePred_pfMC = ones(T,N);  % One-step-ahead predicted values of the states.
  192. yPred_pfMC = ones(T,N);          % One-step-ahead predicted values of y.
  193. w = ones(T,N);                   % Importance weights.
  194. previousXMC = ones(T,N);         % Particles at the previous time step. 
  195. previousXResMC = ones(T,N);      % Resampled previousX.
  196. disp(' ');
  197.  
  198. tic;                             % Initialize timer for benchmarking
  199. for t=2:T,    
  200.   fprintf('run = %i / %i :  PF-MCMC : t = %i / %i  r',j,no_of_runs,t,T);
  201.   fprintf('n')
  202.   
  203.   % PREDICTION STEP:
  204.   % ================ 
  205.   % We use the transition prior as proposal.
  206.   for i=1:N,
  207.     xparticlePred_pfMC(t,i) = feval('ffun',xparticle_pfMC(t-1,i),t) + gengamma(g1,g2);   
  208.   end;
  209.   previousXMC(t,:) = xparticle_pfMC(t-1,:);  % Store the particles at t-1. 
  210.   % EVALUATE IMPORTANCE WEIGHTS:
  211.   % ============================
  212.   % For our choice of proposal, the importance weights are give by:  
  213.   for i=1:N,
  214.     yPred_pfMC(t,i) = feval('hfun',xparticlePred_pfMC(t,i),t);        
  215.     lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-yPred_pfMC(t,i))^(2))) ...
  216.   + 1e-99; % Deal with ill-conditioning.
  217.     w(t,i) = lik;    
  218.   end;  
  219.   w(t,:) = w(t,:)./sum(w(t,:));                % Normalise the weights.
  220.   
  221.   % SELECTION STEP:
  222.   % ===============
  223.   % Here, we give you the choice to try three different types of
  224.   % resampling algorithms. Note that the code for these algorithms
  225.   % applies to any problem!
  226.   if resamplingScheme == 1
  227.     outIndex = residualR(1:N,w(t,:)');        % Residual resampling.
  228.   elseif resamplingScheme == 2
  229.     outIndex = systematicR(1:N,w(t,:)');      % Systematic resampling.
  230.   else  
  231.     outIndex = multinomialR(1:N,w(t,:)');     % Multinomial resampling.  
  232.   end;
  233.   xparticle_pfMC(t,:) = xparticlePred_pfMC(t,outIndex); % Keep particles with
  234.                                                         % resampled
  235.                                                         % indices.
  236.   previousXResMC(t,:) = previousXMC(t,outIndex);  % Resample particles
  237.                                                   % at t-1.
  238.   
  239.   % METROPOLIS-HASTINGS STEP:
  240.   % ========================
  241.   u=rand(N,1); 
  242.   accepted=0;
  243.   rejected=0;
  244.   for i=1:N,   
  245.     xProp = feval('ffun',previousXResMC(t,i),t) + gengamma(g1,g2);   
  246.     mProp = feval('hfun',xProp,t);        
  247.     likProp = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-mProp)^(2))) + 1e-99;     
  248.     m = feval('hfun',xparticle_pfMC(t,i),t);        
  249.     lik = inv(sqrt(sigma)) * exp(-0.5*inv(sigma)*((y(t)-m)^(2))) + 1e-99;     
  250.     acceptance = min(1,likProp/lik);
  251.     if u(i,1) <= acceptance 
  252.       xparticle_pfMC(t,i) = xProp;
  253.       accepted=accepted+1;
  254.     else
  255.       xparticle_pfMC(t,i) = xparticle_pfMC(t,i); 
  256.       rejected=rejected+1;
  257.     end;
  258.   end;
  259.   
  260.   
  261. end;   % End of t loop.
  262. time_pfMC(j) = toc;    % How long did this take?
  263. %%%%%%%%%%%%%%%%%%%%%  PLOT THE RESULTS  %%%%%%%%%%%%%%%%%%%%%
  264. %%%%%%%%%%%%%%%%%%%%%  ================  %%%%%%%%%%%%%%%%%%%%%
  265. figure(1)
  266. clf;
  267. p0=plot(1:T,y,'k+','lineWidth',2); hold on;
  268. %p2=plot(1:T,mu_ekf,'r:','lineWidth',2); hold on;
  269. %p3=plot(1:T,mu_ukf,'b:','lineWidth',2);
  270. p4=plot(1:T,mean(xparticle_pf(:,:)'),'g','lineWidth',2);
  271. p1=plot(1:T,x,'k:o','lineWidth',2); hold off;
  272. %legend([p1 p2 p3 p4 p5 p6],'True x','EKF estimate','UKF estimate','PF estimate','PF-EKF estimate','PF-UKF estimate');
  273. legend([p0 p1 p4],'Noisy observations','True x','PF estimate','PF-EKF estimate','PF-UKF estimate');
  274. xlabel('Time','fontsize',15)
  275. zoom on;
  276. title('Filter estimates (posterior means) vs. True state','fontsize',15)
  277. figure(2)
  278. clf
  279. subplot(211);
  280. semilogy(1:T,P_ekf,'r--',1:T,P_ukf,'b','lineWidth',2);
  281. legend('EKF','UKF');
  282. title('Estimates of state covariance','fontsize',14);
  283. xlabel('time','fontsize',12);
  284. ylabel('var(x)','fontsize',12);
  285. zoom on;
  286. if (1),
  287. figure(3)
  288. clf;
  289. % Plot predictive distribution of y:
  290. subplot(231);
  291. domain = zeros(T,1);
  292. range = zeros(T,1);
  293. thex=[-3:.1:15];
  294. hold on
  295. ylabel('Time (t)','fontsize',15)
  296. xlabel('y_t','fontsize',15)
  297. zlabel('p(y_t|y_{t-1})','fontsize',15)
  298. title('Particle Filter','fontsize',15);
  299. %v=[0 1];
  300. %caxis(v);
  301. for t=6:5:T,
  302.   [range,domain]=hist(yPred_pf(t,:),thex);
  303.   waterfall(domain,t,range/sum(range));
  304. end;
  305. view(-30,80);
  306. rotate3d on;
  307. a=get(gca);
  308. set(gca,'ygrid','off');
  309. % Plot posterior distribution of x:
  310. subplot(234);
  311. domain = zeros(T,1);
  312. range = zeros(T,1);
  313. thex=[0:.1:10];
  314. hold on
  315. ylabel('Time (t)','fontsize',15)
  316. xlabel('x_t','fontsize',15)
  317. zlabel('p(x_t|y_t)','fontsize',15)
  318. %v=[0 1];
  319. %caxis(v);
  320. for t=6:5:T,
  321.   [range,domain]=hist(xparticle_pf(t,:),thex);
  322.   waterfall(domain,t,range/sum(range));
  323. end;
  324. view(-30,80);
  325. rotate3d on;
  326. a=get(gca);
  327. set(gca,'ygrid','off');
  328. % Plot predictive distribution of y:
  329. subplot(232);
  330. domain = zeros(T,1);
  331. range = zeros(T,1);
  332. thex=[-3:.1:15];
  333. hold on
  334. ylabel('Time (t)','fontsize',15)
  335. xlabel('y_t','fontsize',15)
  336. zlabel('p(y_t|y_{t-1})','fontsize',15)
  337. title('Particle Filter (EKF proposal)','fontsize',15);
  338. %v=[0 1];
  339. %caxis(v);
  340. for t=6:5:T,
  341.   [range,domain]=hist(yPred_pfekf(t,:),thex);
  342.   waterfall(domain,t,range/sum(range));
  343. end;
  344. view(-30,80);
  345. rotate3d on;
  346. a=get(gca);
  347. set(gca,'ygrid','off');
  348. % Plot posterior distribution of x:
  349. subplot(235);
  350. domain = zeros(T,1);
  351. range = zeros(T,1);
  352. thex=[0:.1:10];
  353. hold on
  354. ylabel('Time (t)','fontsize',15)
  355. xlabel('x_t','fontsize',15)
  356. zlabel('p(x_t|y_t)','fontsize',15)
  357. %v=[0 1];
  358. %caxis(v);
  359. for t=6:5:T,
  360.   [range,domain]=hist(xparticle_pfekf(t,:),thex);
  361.   waterfall(domain,t,range/sum(range));
  362. end;
  363. view(-30,80);
  364. rotate3d on;
  365. a=get(gca);
  366. set(gca,'ygrid','off');
  367. % Plot predictive distribution of y:
  368. subplot(233);
  369. domain = zeros(T,1);
  370. range = zeros(T,1);
  371. thex=[-3:.1:15];
  372. hold on
  373. ylabel('Time (t)','fontsize',15)
  374. xlabel('y_t','fontsize',15)
  375. zlabel('p(y_t|y_{t-1})','fontsize',15)
  376. title('Particle Filter (UKF proposal)','fontsize',15);
  377. %v=[0 1];
  378. %caxis(v);
  379. for t=6:5:T,
  380.   [range,domain]=hist(yPred_pfukf(t,:),thex);
  381.   waterfall(domain,t,range/sum(range));
  382. end;
  383. view(-30,80);
  384. rotate3d on;
  385. a=get(gca);
  386. set(gca,'ygrid','off');
  387. % Plot posterior distribution of x:
  388. subplot(236);
  389. domain = zeros(T,1);
  390. range = zeros(T,1);
  391. thex=[0:.1:10];
  392. hold on
  393. ylabel('Time (t)','fontsize',15)
  394. xlabel('x_t','fontsize',15)
  395. zlabel('p(x_t|y_t)','fontsize',15)
  396. %v=[0 1];
  397. %caxis(v);
  398. for t=6:5:T,
  399.   [range,domain]=hist(xparticle_pfukf(t,:),thex);
  400.   waterfall(domain,t,range/sum(range));
  401. end;
  402. view(-30,80);
  403. rotate3d on;
  404. a=get(gca);
  405. set(gca,'ygrid','off');
  406. end
  407. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  408. %-- CALCULATE PERFORMANCE --%
  409. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  410. rmsError_ekf(j)     = sqrt(inv(T)*sum((x-mu_ekf).^(2)));
  411. rmsError_ukf(j)     = sqrt(inv(T)*sum((x-mu_ukf).^(2)));
  412. rmsError_pf(j)      = sqrt(inv(T)*sum((x'-mean(xparticle_pf')).^(2)));
  413. rmsError_pfMC(j)    = sqrt(inv(T)*sum((x'-mean(xparticle_pfMC')).^(2)));
  414. rmsError_pfekf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfekf')).^(2)));
  415. rmsError_pfekfMC(j) = sqrt(inv(T)*sum((x'-mean(xparticle_pfekfMC')).^(2)));
  416. rmsError_pfukf(j)   = sqrt(inv(T)*sum((x'-mean(xparticle_pfukf')).^(2)));
  417. rmsError_pfukfMC(j) = sqrt(inv(T)*sum((x'-mean(xparticle_pfukfMC')).^(2)));
  418. disp(' ');
  419. disp('Root mean square (RMS) errors');
  420. disp('-----------------------------');
  421. disp(' ');
  422. disp(['EKF          = ' num2str(rmsError_ekf(j))]);
  423. disp(['UKF          = ' num2str(rmsError_ukf(j))]);
  424. disp(['PF           = ' num2str(rmsError_pf(j))]);
  425. disp(['PF-MCMC      = ' num2str(rmsError_pfMC(j))]);
  426. disp(['PF-EKF       = ' num2str(rmsError_pfekf(j))]);
  427. disp(['PF-EKF-MCMC  = ' num2str(rmsError_pfekfMC(j))]);
  428. disp(['PF-UKF       = ' num2str(rmsError_pfukf(j))]);
  429. disp(['PF-UKF-MCMC  = ' num2str(rmsError_pfukfMC(j))]);
  430. disp(' ');
  431. disp(' ');
  432. disp('Execution time  (seconds)');
  433. disp('-------------------------');
  434. disp(' ');
  435. disp(['PF           = ' num2str(time_pf(j))]);
  436. disp(['PF-MCMC      = ' num2str(time_pfMC(j))]);
  437. disp(['PF-EKF       = ' num2str(time_pfekf(j))]);
  438. disp(['PF-EKF-MCMC  = ' num2str(time_pfekfMC(j))]);
  439. disp(['PF-UKF       = ' num2str(time_pfukf(j))]);
  440. disp(['PF-UKF-MCMC  = ' num2str(time_pfukfMC(j))]);
  441. disp(' ');
  442. drawnow;
  443. %*************************************************************************
  444. end    % Main loop (for j...)
  445. % calculate mean of RMSE errors
  446. mean_RMSE_ekf     = mean(rmsError_ekf);
  447. mean_RMSE_ukf     = mean(rmsError_ukf);
  448. mean_RMSE_pf      = mean(rmsError_pf);
  449. mean_RMSE_pfMC    = mean(rmsError_pfMC);
  450. mean_RMSE_pfekf   = mean(rmsError_pfekf);
  451. mean_RMSE_pfekfMC = mean(rmsError_pfekfMC);
  452. mean_RMSE_pfukf   = mean(rmsError_pfukf);
  453. mean_RMSE_pfukfMC = mean(rmsError_pfukfMC);
  454. % calculate variance of RMSE errors
  455. var_RMSE_ekf     = var(rmsError_ekf);
  456. var_RMSE_ukf     = var(rmsError_ukf);
  457. var_RMSE_pf      = var(rmsError_pf);
  458. var_RMSE_pfMC    = var(rmsError_pfMC);
  459. var_RMSE_pfekf   = var(rmsError_pfekf);
  460. var_RMSE_pfekfMC = var(rmsError_pfekfMC);
  461. var_RMSE_pfukf   = var(rmsError_pfukf);
  462. var_RMSE_pfukfMC = var(rmsError_pfukfMC);
  463. % calculate mean of execution time
  464. mean_time_pf      = mean(time_pf);
  465. mean_time_pfMC    = mean(time_pfMC);
  466. mean_time_pfekf   = mean(time_pfekf);
  467. mean_time_pfekfMC = mean(time_pfekfMC);
  468. mean_time_pfukf   = mean(time_pfukf);
  469. mean_time_pfukfMC = mean(time_pfukfMC);
  470. % display final results
  471. disp(' ');
  472. disp(' ');
  473. disp('************* FINAL RESULTS *****************');
  474. disp(' ');
  475. disp('RMSE : mean and variance');
  476. disp('---------');
  477. disp(' ');
  478. disp(['EKF          = ' num2str(mean_RMSE_ekf) ' (' num2str(var_RMSE_ekf) ')']);
  479. disp(['UKF          = ' num2str(mean_RMSE_ukf) ' (' num2str(var_RMSE_ukf) ')']);
  480. disp(['PF           = ' num2str(mean_RMSE_pf) ' (' num2str(var_RMSE_pf) ')']);
  481. disp(['PF-MCMC      = ' num2str(mean_RMSE_pfMC) ' (' num2str(var_RMSE_pfMC) ')']);
  482. disp(['PF-EKF       = ' num2str(mean_RMSE_pfekf) ' (' num2str(var_RMSE_pfekf) ')']);
  483. disp(['PF-EKF-MCMC  = ' num2str(mean_RMSE_pfekfMC) ' (' num2str(var_RMSE_pfekfMC) ')']);
  484. disp(['PF-UKF       = ' num2str(mean_RMSE_pfukf) ' (' num2str(var_RMSE_pfukf) ')']);
  485. disp(['PF-UKF-MCMC  = ' num2str(mean_RMSE_pfukfMC) ' (' num2str(var_RMSE_pfukfMC) ')']);
  486. disp(' ');
  487. disp(' ');
  488. disp('Execution time  (seconds)');
  489. disp('-------------------------');
  490. disp(' ');
  491. disp(['PF           = ' num2str(mean_time_pf)]);
  492. disp(['PF-MCMC      = ' num2str(mean_time_pfMC)]);
  493. disp(['PF-EKF       = ' num2str(mean_time_pfekf)]);
  494. disp(['PF-EKF-MCMC  = ' num2str(mean_time_pfekfMC)]);
  495. disp(['PF-UKF       = ' num2str(mean_time_pfukf)]);
  496. disp(['PF-UKF-MCMC  = ' num2str(mean_time_pfukfMC)]);
  497. disp(' ');
  498. %*************************************************************************
  499. break;
  500. % This is an alternative way of plotting the 3D stuff:
  501. % Somewhere in between lies the best way!
  502. figure(3)
  503. clf;
  504. support=[-1:.1:2];
  505. NN=50;
  506. extPlot=zeros(10*61,1);
  507. for t=6:5:T,
  508.   [r,d]=hist(yPred_pf(t,:),support);
  509.   r=r/sum(r);
  510.   for i=1:1:61,
  511.     for j=1:1:NN,
  512.       extPlot(NN*i-NN+1:i*NN) = r(i);
  513.     end;
  514.   end;
  515.   d= linspace(-5,25,length(extPlot));
  516.   plot3(d,t*ones(size(extPlot)),extPlot,'r')
  517.   hold on;
  518. end;