Home > src > demos > bot_demo > ukfs_bot_demo.m

ukfs_bot_demo

PURPOSE ^

Bearings Only Tracking (BOT) demonstration with UKF.

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

 Bearings Only Tracking (BOT) demonstration with UKF.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

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