开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 文章/文档 > 行业发展研究 > 查看源码
demo_MC.asv
资源名称：particle-filter.rar　[点击查看]
上传用户：hfch80
上传日期：2007-10-25
资源大小：3637k
文件大小：19k
源码类别：

行业发展研究

开发平台：

Matlab

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