Home > src > demos > bot_demo > ekfs_bot_demo.m

ekfs_bot_demo

PURPOSE ^

Bearings Only Tracking (BOT) demonstration with EKF

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

 Bearings Only Tracking (BOT) demonstration with EKF

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 % Bearings Only Tracking (BOT) demonstration with EKF
0002 
0003 % Copyright (C) 2002 Simo S�rkk�
0004 %               2007 Jouni Hartikainen
0005 %
0006 % This software is distributed under the GNU General Public
0007 % Licence (version 2 or later); please refer to the file
0008 % Licence.txt, included with the software, for details.
0009 
0010   keep_trajectory = 0;
0011   silent = 0;
0012   
0013   % Number of steps to advance at a time in animations.
0014   steps = 2;
0015   
0016   % Handles to measurement model function and it's derivatives
0017   h_func = @bot_h;
0018   dh_dx_func = @bot_dh_dx;
0019   d2h_dx2_func = @bot_d2h_dx2;
0020 
0021   % Create a bit curved trajectory and angle
0022   % measurements from two sensors
0023   S1 = [-1;-2];
0024   S2 = [1;1];
0025   sd = 0.05;
0026   dt = 0.01;
0027   if ~keep_trajectory
0028     % Accelerations for the object
0029     a = zeros(1,500);
0030     a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
0031     a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
0032     a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
0033     x = [0;0;1;0];
0034     t = 0;
0035     X = [];
0036     Y = [];
0037     T = [];
0038     for i=1:500
0039       F = [0 0  1    0;...
0040            0 0  0    1;...
0041            0 0  0   a(i);...
0042            0 0 -a(i) 0];
0043       x = expm(F*dt)*x;
0044       y1 = atan2(x(2)-S1(2), x(1)-S1(1)) + sd * randn;
0045       y2 = atan2(x(2)-S2(2), x(1)-S2(1)) + sd * randn;
0046       t  = t + dt;
0047       X = [X x];
0048       T = [T t];
0049       Y = [Y [y1;y2]];
0050     end
0051   end
0052   
0053   % Uncomment if you want to plot and print the measurements
0054   %plot(1:size(Y,2),Y(1,:),'.',1:size(Y,2),Y(2,:),'.');
0055   %legend('Sensor 1', 'Sensor 2');
0056   %title('Measurements from sensors in radians');
0057   %print -dpsc bot_demo_measurements.ps
0058 
0059   %
0060   % Initialize EKF to values
0061   %
0062   %   x = 0
0063   %   y = 0,
0064   %   dx/dt = 0
0065   %   dy/dt = 0
0066   %
0067   % with great uncertainty in velocity
0068   %
0069   M = [0;0;0;0];
0070   P = diag([4 4 4 4]);
0071   R = sd^2;
0072   if ~silent
0073     der_check(h_func, dh_dx_func, 1, M, S1);
0074     der_check(h_func, dh_dx_func, 1, M, S2);
0075   end
0076   qx = 0.1;
0077   qy = 0.1;
0078   F = [0 0 1 0;
0079        0 0 0 1;
0080        0 0 0 0;
0081        0 0 0 0];
0082   [A,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt);
0083 
0084   clc;
0085   clf;
0086   disp(['In this demonstration we track a moving object with two sensors, ',...
0087         'which gives only bearings of the object with respect to sensors position.',...
0088        'The state of the system is estimated with EKF.'])
0089   disp(' ');
0090   fprintf('Running EKF...')
0091   
0092   %
0093   % Track and animate
0094   %
0095   MM = zeros(size(M,1),size(Y,2));
0096   PP = zeros(size(M,1),size(M,1),size(Y,2));
0097   ME = zeros(size(M,1),1);
0098   for k=1:size(Y,2)
0099     % Track with EKF
0100     [M,P] = ekf_predict1(M,P,A,Q);
0101     [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(2),h_func,[],[S1 S2]);
0102     MM(:,k)   = M;
0103     PP(:,:,k) = P;
0104     ME(k) = P(1,1) + P(2,2);
0105   end
0106   ekf_rmse = sqrt(mean((X(1,:)-MM(1,:)).^2+(X(2,:)-MM(2,:)).^2));
0107 
0108   fprintf('Done!\n')
0109   disp(' ');
0110   disp(['The filtering results are now displayed sequentially. ',...
0111        'Notice how the estimates gets more accurate when the filter gets on the right track. ',...
0112        'The green ellipse around the current estimate (little blue circle) reflects the filters ',...
0113        'confidence intervals of the position estimate.']);
0114   disp(' ');
0115   disp('<push any key to proceed>');
0116   
0117   % Plot the filtered estimates sequentially
0118   if ~silent
0119       M = MM(:,1);  
0120       P = PP(:,:,1);
0121       EST = M;
0122       tt = (0:0.01:1)*2*pi;
0123       cc = repmat(M(1:2),1,length(tt)) + ...
0124               2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0125       h = plot(X(1,:),X(2,:),'r-',...
0126                M(1),M(2),'bo',...
0127                EST(1,:),EST(2,:),'b--',...
0128                cc(1,:),cc(2,:),'g-',...
0129                S1(1),S1(2),'k--',...
0130                S1(1),S1(2),'k^',...
0131                S2(1),S2(2),'k--',...
0132                S2(1),S2(2),'k^');
0133       legend('Location','NorthWest','Real trajectory','Current estimate','Estimated trajectory',...
0134              'Confidence interval','Measurements from sensors','Positions of the sensors');
0135       title('Bearings Only Tracking with EKF.')
0136       axis([-1.5 1.5 -2.5 1.5]);
0137       
0138       EST = [];
0139       for k=1:steps:size(Y,2)
0140         M = MM(:,k);
0141         P = PP(:,:,k);
0142         EST = MM(:,1:k);
0143 
0144         % Confidence ellipse
0145         cc = repmat(M(1:2),1,length(tt)) + ...
0146          2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0147 
0148         % Measurement directions
0149         len = 2.5;
0150         dx1 = len*cos(Y(1,k));
0151         dy1 = len*sin(Y(1,k));
0152         dx2 = len*cos(Y(2,k));
0153         dy2 = len*sin(Y(2,k));
0154 
0155         % Update graphics
0156         set(h(2),'xdata',M(1)); set(h(2),'ydata',M(2));
0157         set(h(3),'xdata',EST(1,:)); set(h(3),'ydata',EST(2,:)); 
0158         set(h(4),'xdata',cc(1,:)); set(h(4),'ydata',cc(2,:)); 
0159         set(h(5),'xdata',[S1(1);S1(1)+dx1]); set(h(5),'ydata',[S1(2);S1(2)+dy1]); 
0160         set(h(7),'xdata',[S2(1);S2(1)+dx2]); set(h(7),'ydata',[S2(2);S2(2)+dy2]); 
0161       pause
0162     end
0163   end
0164   
0165   clc;
0166   disp(['In this demonstration we track a moving object with two sensors, ',...
0167         'which gives only bearings of the object with respect to sensors position.',...
0168        'The state of the system is estimated with EKF.'])
0169   disp(' ');
0170   fprintf('Running EKF...Done!\n')
0171   disp(' ');
0172   disp(['The filtering results are now displayed sequentially. ',...
0173        'Notice how the estimates gets more accurate when the filter gets on the right track. ',...
0174        'The green ellipse around the current estimate (little blue circle) reflects the filters ',...
0175        'confidence intervals of the position estimate.']);
0176   disp(' ');
0177   disp('<push any key to smooth the estimates with ERTS and ETF>');
0178   if (~silent) pause; end;
0179   clc;
0180   fprintf('Smoothing with ERTS and ETF...');
0181 
0182   %
0183   % Smoother 1
0184   %
0185   [SM1,SP1] = erts_smooth1(MM,PP,A,Q);
0186   eks_rmse1 = sqrt(mean((X(1,:)-SM1(1,:)).^2+(X(2,:)-SM1(2,:)).^2));
0187   ME1 = squeeze(SP1(1,1,:)+SP1(2,2,:));
0188 
0189 
0190   %
0191   % Smoother 2
0192   %
0193   [SM2,SP2] = etf_smooth1(MM,PP,Y,A,Q,[],[],[],...
0194              dh_dx_func,R*eye(2),h_func,[],[S1 S2]);
0195   eks_rmse2 = sqrt(mean((X(1,:)-SM2(1,:)).^2+(X(2,:)-SM2(2,:)).^2));
0196   ME2 = squeeze(SP2(1,1,:)+SP2(2,2,:));
0197 
0198   fprintf('Done!\n')
0199   disp(' ');
0200   disp(['Smoothing results of ERTS are now displayed sequentially. ',...
0201         'Notice how the confidence ellipse gets even smaller now.']);
0202   disp(' ');
0203   disp('<push any key to proceed>');
0204   
0205   % Plot the smoothed estimates sequentially
0206   if ~silent
0207     M = SM1(:,1);  
0208     P = SP1(:,:,1);
0209     EST = M;
0210     cc = repmat(M(1:2),1,length(tt)) + ...
0211      2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0212     h = plot(X(1,:),X(2,:),'r-',...
0213              M(1),M(2),'o',...
0214              EST(1,:),EST(2,:),'--',...
0215              cc(1,:),cc(2,:),'g-',...
0216              MM(1,:),MM(2,:),'b--',...
0217              S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0218     legend('Location','NorthWest','Real trajectory','Current estimate','Smoothed trajectory',...
0219            'Confidence interval','Filter estimate','Positions of the sensors');
0220     title('Bearings Only Tracking with ERTS.')
0221     axis([-1.5 1.5 -2.5 1.5]);
0222     EST = [];
0223     for k=size(Y,2):-steps:1
0224       M = SM1(:,k);  
0225       P = SP1(:,:,k);
0226       EST = SM1(:,end:-1:k);
0227 
0228       % Confidence ellipse
0229       cc = repmat(M(1:2),1,length(tt)) + ...
0230      2*chol(P(1:2,1:2))'*[cos(tt);sin(tt)];
0231       
0232       set(h(2),'xdata',M(1)); set(h(2),'ydata',M(2));
0233       set(h(3),'xdata',EST(1,:)); set(h(3),'ydata',EST(2,:)); 
0234       set(h(4),'xdata',cc(1,:)); set(h(4),'ydata',cc(2,:)); 
0235       pause;
0236      end
0237   end
0238   
0239   disp(' ')
0240   disp('<push any key to display all estimates together>')
0241   if (~silent) pause; end;
0242   clc;
0243   disp('All estimates are now displayed.')
0244 
0245   %
0246   % Plot all the estimates together
0247   %
0248   if ~silent
0249     plot(X(1,:),X(2,:),'k-',...
0250          MM(1,:),MM(2,:),'b--',...
0251          SM1(1,:),SM1(2,:),'r-.',...
0252          SM2(1,:),SM2(2,:),'g-.',...
0253          S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0254     axis([-1.5 1.5 -2.5 1]);
0255     legend('Real trajectory',...
0256            'EKF1 estimate',...
0257            'ERTS estimate',...
0258            'ETF estimate',...
0259            'Positions of sensors',...
0260            'Location', 'NorthWest');
0261     title('Filtering and smoothing result with 1st order EKF');
0262     axis([-1.5 1.5 -2.5 1.5]);
0263     % Uncomment if you want to save an image
0264     %print -dpsc bot_demo_ekf1.ps
0265   end
0266   
0267   % Print RMSE
0268   disp(' ');
0269   disp('RMS errors:');
0270   fprintf('EKF-RMSE  = %.3f  [%.3f]\n',ekf_rmse,sqrt(mean(ME)));
0271   fprintf('EKS-RMSE1 = %.4f [%.4f]\n',eks_rmse1,sqrt(mean(ME1)));
0272   fprintf('EKS-RMSE2 = %.4f [%.4f]\n',eks_rmse2,sqrt(mean(ME2)));

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