Home > src > demos > bot_demo > ghkfs_bot_demo.m

ghkfs_bot_demo

PURPOSE ^

% Bearings Only Tracking (BOT) demonstration with GHKF

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

% Bearings Only Tracking (BOT) demonstration with GHKF

  Description:
    In this example the Gauss-Hermite Kalman filter and Gauss-Hermite 
    Rauch-Tung-Striebel smoother 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. 

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

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

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