Home > src > demos > bot_demo > bot_demo_all.m

bot_demo_all

PURPOSE ^

% Bearings Only Tracking (BOT) demonstration with various filters

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

% Bearings Only Tracking (BOT) demonstration with various filters

  Description:
    In this example various Kalman filters and Rauch-Tung-Striebel
    smoothers are used to estimate the position and velocity of a
    moving object on a plane. Two sensors track the position of the 
    object by returning noisy measurements of angular direction of the 
    target. The methods used are:
      * Extended Kalman filter (1st and 2nd degree)
      * Unscented Kalman filter
      * Gauss-Hermite Kalman filter (degree 3)
      * Cubature Kalman filter
    Additionally, the corresponding Rauch-Tung-Striebel smoother results 
    are also presented.

  References:
    Refer to the Toolbox documentation for details on the model.

  See also:
    ukf_predict1, ukf_update1, urts_smooth1,
    ekf_predict1, ekf_update1, erts_smooth1, ekf_predict2, ekf_update2,
    ghkf_predict, ghkf_update, ghrts_smooth,
    ckf_predict, ckf_update, crts_smooth

  Author:
    Copyright (C) 2002, 2003 Simo Särkkä
                  2007       Jouni Hartikainen
                  2010       Arno Solin

  Licence:
    This software is distributed under the GNU General Public
    Licence (version 2 or later); please refer to the file
    Licence.txt, included with the software, for details.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %% Bearings Only Tracking (BOT) demonstration with various filters
0002 %
0003 %  Description:
0004 %    In this example various Kalman filters and Rauch-Tung-Striebel
0005 %    smoothers are used to estimate the position and velocity of a
0006 %    moving object on a plane. Two sensors track the position of the
0007 %    object by returning noisy measurements of angular direction of the
0008 %    target. The methods used are:
0009 %      * Extended Kalman filter (1st and 2nd degree)
0010 %      * Unscented Kalman filter
0011 %      * Gauss-Hermite Kalman filter (degree 3)
0012 %      * Cubature Kalman filter
0013 %    Additionally, the corresponding Rauch-Tung-Striebel smoother results
0014 %    are also presented.
0015 %
0016 %  References:
0017 %    Refer to the Toolbox documentation for details on the model.
0018 %
0019 %  See also:
0020 %    ukf_predict1, ukf_update1, urts_smooth1,
0021 %    ekf_predict1, ekf_update1, erts_smooth1, ekf_predict2, ekf_update2,
0022 %    ghkf_predict, ghkf_update, ghrts_smooth,
0023 %    ckf_predict, ckf_update, crts_smooth
0024 %
0025 %  Author:
0026 %    Copyright (C) 2002, 2003 Simo Särkkä
0027 %                  2007       Jouni Hartikainen
0028 %                  2010       Arno Solin
0029 %
0030 %  Licence:
0031 %    This software is distributed under the GNU General Public
0032 %    Licence (version 2 or later); please refer to the file
0033 %    Licence.txt, included with the software, for details.
0034 
0035 %% Set parameters
0036 
0037   silent = 0;
0038   save_plots = 0;
0039   
0040   
0041 %% Simulate trajectory
0042   
0043   % Measurement mean and derivative
0044   %
0045   %  h = atan((y-sy) / (x-sx))
0046   h_func = @bot_h;
0047   dh_dx_func = @bot_dh_dx;
0048   d2h_dx2_func = @bot_d2h_dx2;
0049   
0050   % Create a bit curved trajectory and angle
0051   % measurements from two sensors
0052   S1 = [-1;-2];
0053   S2 = [1;1];
0054   sd = 0.05;
0055   dt = 0.01;
0056 
0057   % Acceleration for the target to have a curved trajectory
0058   a = zeros(1,500);
0059   a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
0060   a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
0061   a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
0062     
0063   % Starting state
0064   x = [0;0;1;0];
0065   t = 0;
0066   X = [];
0067   Y = [];
0068   T = [];
0069   for i=1:500
0070     % Dynamics in continous case
0071     F = [0 0  1    0;...
0072          0 0  0    1;...
0073          0 0  0   a(i);...
0074          0 0 -a(i) 0];
0075     % Discretization for the data generation
0076     x = expm(F*dt)*x;
0077     % Angular measurements for both sensors.
0078     y1 = atan2(x(2)-S1(2), x(1)-S1(1)) + sd * randn;
0079     y2 = atan2(x(2)-S2(2), x(1)-S2(1)) + sd * randn;
0080       
0081     % Save the data
0082     t  = t + dt;
0083     X = [X x];
0084     T = [T t];
0085     Y = [Y [y1;y2]];
0086   end
0087 
0088   % Prior for position and velocity
0089   M_0 = [0;0;0;0];
0090   P_0 = diag([0.1 0.1 10 10]);
0091 
0092   % Discretize the continous model
0093   qx = 0.1;
0094   qy = 0.1;
0095   F = [0 0 1 0;
0096        0 0 0 1;
0097        0 0 0 0;
0098        0 0 0 0];
0099   [A,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt);
0100 
0101   
0102 %% EKF1 and EKF2
0103   
0104   clc;  clf;
0105   disp(['In this demonstration we track a moving object with two sensors, ',...
0106         'which gives only bearings of the object with respect to sensors position. ',...
0107        'The state of the system is estimated with 1st and 2nd order EKF, and UKF.'])
0108   disp(' ');
0109   fprintf('Running 1st order EKF...')
0110   %
0111   % Initialize EKF1
0112   %
0113   M = M_0;
0114   P = P_0;
0115   R = sd^2;
0116   %if ~silent
0117   %  der_check(h_func, dh_dx_func, 1, M, S1);
0118   %  der_check(h_func, dh_dx_func, 1, M, S2);
0119   %end
0120   
0121   MM_EKF1 = zeros(size(M,1),size(Y,2));
0122   PP_EKF1 = zeros(size(M,1),size(M,1),size(Y,2));
0123   ME_EKF1 = zeros(size(M,1),1);
0124 
0125   % Filter with EKF1
0126   for k = 1:size(Y,2) 
0127     [M,P] = ekf_predict1(M,P,A,Q);
0128     [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(2),h_func,[],[S1 S2]);
0129     MM_EKF1(:,k)   = M;
0130     PP_EKF1(:,:,k) = P;
0131     ME_EKF1(k) = P(1,1) + P(2,2);
0132   end
0133   
0134   % RMSE for EKF1
0135   ekf1_rmse = sqrt(mean((X(1,:)-MM_EKF1(1,:)).^2+(X(2,:)-MM_EKF1(2,:)).^2));
0136 
0137   fprintf('Done!\n')
0138   fprintf('Running smoothers...');
0139   % ERTS smoother
0140   [SM1_EKF1,SP1_EKF1] = erts_smooth1(MM_EKF1,PP_EKF1,A,Q);
0141   eks1_rmse1 = sqrt(mean((X(1,:)-SM1_EKF1(1,:)).^2+(X(2,:)-SM1_EKF1(2,:)).^2));
0142   ME1_EKF1 = squeeze(SP1_EKF1(1,1,:)+SP1_EKF1(2,2,:));
0143 
0144   % ETF smoother
0145   [SM2_EKF1,SP2_EKF1] = etf_smooth1(MM_EKF1,PP_EKF1,Y,A,Q,[],[],[],...
0146                 dh_dx_func,R*eye(2),h_func,[],[S1 S2]);
0147   eks1_rmse2 = sqrt(mean((X(1,:)-SM2_EKF1(1,:)).^2+(X(2,:)-SM2_EKF1(2,:)).^2));
0148   ME2_EKF1 = squeeze(SP2_EKF1(1,1,:)+SP2_EKF1(2,2,:));
0149   fprintf('Done!\n');
0150   
0151   % Plot the results
0152   if ~silent
0153     plot(X(1,:),X(2,:),'k-',...
0154          MM_EKF1(1,:),MM_EKF1(2,:),'b--',...
0155          SM1_EKF1(1,:),SM1_EKF1(2,:),'r-.',...
0156          SM2_EKF1(1,:),SM2_EKF1(2,:),'g-.',...
0157          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0158     axis([-1.5 1.5 -2.5 1.5]);
0159 
0160     legend('Real trajectory',...
0161            'EKF1 estimate',...
0162            'ERTS estimate',...
0163            'ETF estimate',...
0164            'Positions of sensors',...
0165            'Location', 'NorthWest');
0166     title('Filtering and smoothing result with 1st order EKF');
0167     if save_plots
0168       print -dpsc bot_demo_ekf1.ps
0169     end
0170   end
0171   
0172   disp(' ');
0173   disp('1st order filtering and smoothing results are now displayed');
0174   disp(' ');
0175   disp('<push any key to filter and smooth with 2nd order EKF>');
0176   pause
0177   
0178   %
0179   % Initialize EKF2
0180   %
0181   M = M_0; 
0182   P = P_0;
0183   R = sd^2;
0184   if ~silent
0185       %der_check(dh_dx_func, d2h_dx2_func, 1, M, S1);
0186       %der_check(dh_dx_func, d2h_dx2_func, 1, M, S2);
0187   end
0188   
0189   MM_EKF2 = zeros(size(M,1),size(Y,2));
0190   PP_EKF2 = zeros(size(M,1),size(M,1),size(Y,2));
0191   ME_EKF2 = zeros(size(M,1),1);
0192   
0193   clc; 
0194   fprintf('Running 2nd order EKF...');
0195   % Filter with EKF2
0196   for k = 1:size(Y,2) 
0197     [M,P] = ekf_predict1(M,P,A,Q);
0198     [M,P] = ekf_update2(M,P,Y(:,k),dh_dx_func,d2h_dx2_func,R*eye(2),h_func,[],[S1 S2]);
0199     MM_EKF2(:,k)   = M;
0200     PP_EKF2(:,:,k) = P;
0201     ME_EKF2(k) = P(1,1) + P(2,2);
0202   end
0203   ekf2_rmse = sqrt(mean((X(1,:)-MM_EKF2(1,:)).^2+(X(2,:)-MM_EKF2(2,:)).^2));
0204   fprintf('Done!\n');
0205   
0206   fprintf('Running smoothers...');
0207   %
0208   % Smoother 1
0209   %
0210   [SM1_EKF2,SP1_EKF2] = erts_smooth1(MM_EKF2,PP_EKF2,A,Q);
0211   eks2_rmse1 = sqrt(mean((X(1,:)-SM1_EKF2(1,:)).^2+(X(2,:)-SM1_EKF2(2,:)).^2));
0212   ME1_EKF2 = squeeze(SP1_EKF2(1,1,:)+SP1_EKF2(2,2,:));
0213 
0214   %
0215   % Smoother 2
0216   %
0217   [SM2_EKF2,SP2_EKF2] = etf_smooth1(MM_EKF2,PP_EKF2,Y,A,Q,[],[],[],...
0218                 dh_dx_func,R*eye(2),h_func,[],[S1 S2]);
0219   eks2_rmse2 = sqrt(mean((X(1,:)-SM2_EKF2(1,:)).^2+(X(2,:)-SM2_EKF2(2,:)).^2));
0220   ME2_EKF2 = squeeze(SP2_EKF2(1,1,:)+SP2_EKF2(2,2,:));
0221   fprintf('Done!\n');
0222   
0223   if ~silent
0224     plot(X(1,:),X(2,:),'k-',...
0225          MM_EKF2(1,:),MM_EKF2(2,:),'b--',...
0226          SM1_EKF2(1,:),SM1_EKF2(2,:),'r-.',...
0227          SM2_EKF2(1,:),SM2_EKF2(2,:),'g-.',...
0228          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0229     axis([-1.5 1.5 -2.5 1.5]);
0230     legend('Real trajectory',...
0231            'EKF2 estimate',...
0232            'ERTS estimate',...
0233            'ETF estimate',...
0234            'Positions of sensors',...
0235            'Location', 'NorthWest');
0236     title('Filtering and smoothing result with 2nd order EKF');
0237     if save_plots
0238       print -dpsc bot_demo_ekf2.ps
0239     end
0240   end
0241   
0242   disp(' ');
0243   disp('2nd order filtering and smoothing results are now displayed.');
0244   disp(' ');
0245 
0246   
0247 %% UKF
0248   
0249   disp('<push any key to filter and smooth with unscented filter and smoothers>')
0250   pause
0251   
0252   clc;
0253   fprintf('Running UKF...');
0254   % Initialize UKF
0255   M = M_0;
0256   P = P_0;
0257   MM_UKF = zeros(size(M,1),size(Y,2));
0258   PP_UKF = zeros(size(M,1),size(M,1),size(Y,2));
0259   ME_UKF = zeros(size(M,1),1);
0260   
0261   % Filter with UKF
0262   for k=1:size(Y,2)
0263     [M,P] = ukf_predict1(M,P,A,Q);
0264     [M,P] = ukf_update1(M,P,Y(:,k),h_func,R*eye(2),[S1 S2]);
0265     MM_UKF(:,k)   = M;
0266     PP_UKF(:,:,k) = P;
0267     ME_UKF(k) = P(1,1) + P(2,2);
0268   end
0269   
0270   % Calculate RMSE of UKF
0271   ukf_rmse = sqrt(mean((X(1,:)-MM_UKF(1,:)).^2+(X(2,:)-MM_UKF(2,:)).^2));
0272 
0273   fprintf('Done!\n');
0274 
0275   fprintf('Running smoothers...');  
0276   % URTS Smoother
0277   [SM1_UKF,SP1_UKF] = urts_smooth1(MM_UKF,PP_UKF,A,Q);
0278   uks_rmse1 = sqrt(mean((X(1,:)-SM1_UKF(1,:)).^2+(X(2,:)-SM1_UKF(2,:)).^2));
0279   ME1_UKF = squeeze(SP1_UKF(1,1,:)+SP1_UKF(2,2,:));
0280   
0281   % UTF Smoother
0282   IAW = inv(A)*[eye(size(A,1)) eye(size(A,1))];
0283   [SM2_UKF,SP2_UKF] = utf_smooth1(MM_UKF,PP_UKF,Y,IAW,Q,[],...
0284                            h_func,R*eye(2),[S1 S2]);
0285   uks_rmse2 = sqrt(mean((X(1,:)-SM2_UKF(1,:)).^2+(X(2,:)-SM2_UKF(2,:)).^2));
0286   ME2_UKF = squeeze(SP2_UKF(1,1,:)+SP2_UKF(2,2,:));
0287 
0288   fprintf('Done!\n\n');
0289 
0290   % Plot UKF, URTS, UTF
0291   if ~silent
0292     plot(X(1,:),X(2,:),'k-',...
0293          MM_UKF(1,:),MM_UKF(2,:),'b--',...
0294          SM1_UKF(1,:),SM1_UKF(2,:),'r-.',...
0295          SM2_UKF(1,:),SM2_UKF(2,:),'g-.',...
0296          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0297     axis([-1.5 1.5 -2.5 1.5]);
0298     legend('Real trajectory',...
0299            'UKF estimate',...
0300            'URTS estimate',...
0301            'UTF estimate',...
0302            'Positions of sensors',...
0303            'Location', 'NorthWest');
0304     title('Filtering and smoothing result with UKF');
0305     if save_plots
0306       print -dpsc bot_demo_ukf.ps
0307     end
0308   end
0309   
0310   disp(' ');
0311   disp('Unscented filtering and smoothing results are now displayed.')
0312   disp(' ');
0313   
0314   
0315   %% GHKF
0316   
0317   disp('<push any key to filter and smooth with Gauss-Hermite Kalman filter and smoother>')
0318   pause
0319   
0320   clc;
0321   fprintf('Running GHKF...');
0322   % Initialize GHKF
0323   M = M_0;
0324   P = P_0;
0325   MM_GHKF = zeros(size(M,1),size(Y,2));
0326   PP_GHKF = zeros(size(M,1),size(M,1),size(Y,2));
0327   ME_GHKF = zeros(size(M,1),1);
0328   
0329   % Filter with GHKF
0330   for k=1:size(Y,2)
0331     [M,P] = ghkf_predict(M,P,A,Q,[],3);
0332     [M,P] = ghkf_update(M,P,Y(:,k),h_func,R*eye(2),[S1 S2],3);
0333     MM_GHKF(:,k)   = M;
0334     PP_GHKF(:,:,k) = P;
0335     ME_GHKF(k) = P(1,1) + P(2,2);
0336   end
0337   
0338   % Calculate RMSE of GHKF
0339   ghkf_rmse = sqrt(mean((X(1,:)-MM_GHKF(1,:)).^2+(X(2,:)-MM_GHKF(2,:)).^2));
0340 
0341   fprintf('Done!\n');
0342 
0343   fprintf('Running smoothers...');  
0344   % GHRTS Smoother
0345   [SM1_GHKF,SP1_GHKF] = ghrts_smooth(MM_GHKF,PP_GHKF,A,Q,[],3);
0346   ghks_rmse1 = sqrt(mean((X(1,:)-SM1_GHKF(1,:)).^2+(X(2,:)-SM1_GHKF(2,:)).^2));
0347   ME1_GHKF = squeeze(SP1_GHKF(1,1,:)+SP1_GHKF(2,2,:));
0348   
0349   fprintf('Done!\n\n');
0350   
0351   % Plot GHKF, GHRTS
0352   if ~silent
0353     plot(X(1,:),X(2,:),'k-',...
0354          MM_GHKF(1,:),MM_GHKF(2,:),'b--',...
0355          SM1_GHKF(1,:),SM1_GHKF(2,:),'r-.',...
0356          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0357     axis([-1.5 1.5 -2.5 1.5]);
0358     legend('Real trajectory',...
0359            'GHKF estimate',...
0360            'GHRTS estimate',...
0361            'Positions of sensors',...
0362            'Location', 'NorthWest');
0363     title('Filtering and smoothing result with GHKF');
0364     if save_plots
0365       print -dpsc bot_demo_ghkf.ps
0366     end
0367   end
0368   
0369   disp(' ');
0370   disp('Gauss-Hermite filtering and smoothing results are now displayed.')
0371   disp(' ');
0372   
0373   
0374 
0375   %% CKF
0376   
0377   disp('<push any key to filter and smooth with Cubature Kalman filter and smoother>')
0378   pause
0379   
0380   clc;
0381   fprintf('Running CKF...');
0382   % Initialize CKF
0383   M = M_0;
0384   P = P_0;
0385   MM_CKF = zeros(size(M,1),size(Y,2));
0386   PP_CKF = zeros(size(M,1),size(M,1),size(Y,2));
0387   ME_CKF = zeros(size(M,1),1);
0388   
0389   % Filter with CKF
0390   for k=1:size(Y,2)
0391     [M,P] = ckf_predict(M,P,A,Q,[]);
0392     [M,P] = ckf_update(M,P,Y(:,k),h_func,R*eye(2),[S1 S2]);
0393     MM_CKF(:,k)   = M;
0394     PP_CKF(:,:,k) = P;
0395     ME_CKF(k) = P(1,1) + P(2,2);
0396   end
0397   
0398   % Calculate RMSE of CKF
0399   ckf_rmse = sqrt(mean((X(1,:)-MM_CKF(1,:)).^2+(X(2,:)-MM_CKF(2,:)).^2));
0400 
0401   fprintf('Done!\n');
0402 
0403   fprintf('Running smoothers...');  
0404   % CRTS Smoother
0405   [SM1_CKF,SP1_CKF] = crts_smooth(MM_CKF,PP_CKF,A,Q,[]);
0406   cks_rmse1 = sqrt(mean((X(1,:)-SM1_CKF(1,:)).^2+(X(2,:)-SM1_CKF(2,:)).^2));
0407   ME1_CKF = squeeze(SP1_CKF(1,1,:)+SP1_CKF(2,2,:));
0408   
0409   fprintf('Done!\n\n');
0410 
0411   % Plot CKF, CRTS
0412   if ~silent
0413     plot(X(1,:),X(2,:),'k-',...
0414          MM_CKF(1,:),MM_CKF(2,:),'b--',...
0415          SM1_CKF(1,:),SM1_CKF(2,:),'r-.',...
0416          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0417     axis([-1.5 1.5 -2.5 1.5]);
0418     legend('Real trajectory',...
0419            'CKF estimate',...
0420            'CRTS estimate',...
0421            'Positions of sensors',...
0422            'Location', 'NorthWest');
0423     title('Filtering and smoothing result with CKF');
0424     if save_plots
0425       print -dpsc bot_demo_ckf.ps
0426     end
0427   end
0428   
0429   disp(' ');
0430   disp('Cubature filtering and smoothing results are now displayed.')
0431   disp(' ');
0432   
0433   
0434 %% Show RMSE values for all methods
0435   
0436   % Print errors
0437   disp('RMS errors:')
0438   fprintf('EKF1-RMSE  = %.4f [%.4f]\n',ekf1_rmse,  sqrt(mean(ME_EKF1)));
0439   fprintf('ERTS1-RMSE = %.4f [%.4f]\n',eks1_rmse1, sqrt(mean(ME1_EKF1)));
0440   fprintf('ETF1-RMSE  = %.4f [%.4f]\n',eks1_rmse2, sqrt(mean(ME2_EKF1)));
0441   fprintf('EKF2-RMSE  = %.4f [%.4f]\n',ekf2_rmse,  sqrt(mean(ME_EKF2)));
0442   fprintf('ERTS2-RMSE = %.4f [%.4f]\n',eks2_rmse1, sqrt(mean(ME1_EKF2)));
0443   fprintf('ETF2-RMSE  = %.4f [%.4f]\n',eks2_rmse2, sqrt(mean(ME2_EKF2)));  
0444   fprintf('UKF-RMSE   = %.4f [%.4f]\n',ukf_rmse,   sqrt(mean(ME_UKF)));
0445   fprintf('URTS-RMSE  = %.4f [%.4f]\n',uks_rmse1,  sqrt(mean(ME1_UKF)));    
0446   fprintf('UTF-RMSE   = %.4f [%.4f]\n',uks_rmse2,  sqrt(mean(ME2_UKF)));
0447   fprintf('GHKF-RMSE  = %.4f [%.4f]\n',ghkf_rmse,   sqrt(mean(ME_GHKF)));
0448   fprintf('GHRTS-RMSE = %.4f [%.4f]\n',ghks_rmse1,  sqrt(mean(ME1_GHKF)));    
0449   fprintf('CKF-RMSE   = %.4f [%.4f]\n',ckf_rmse,   sqrt(mean(ME_CKF)));
0450   fprintf('CRTS-RMSE  = %.4f [%.4f]\n',cks_rmse1,  sqrt(mean(ME1_CKF))); 
0451

Generated on Fri 12-Aug-2011 15:15:16 by m2html © 2005