ekf_bot_tt_demo

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

   EKF Monte Carlo Data Association demo 3

  Track two objects using azimuth measurements
  in heavy clutter. Allow only one data assocation
  per target on single time instance.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %
0003 %   EKF Monte Carlo Data Association demo 3
0004 %
0005 %  Track two objects using azimuth measurements
0006 %  in heavy clutter. Allow only one data assocation
0007 %  per target on single time instance.
0008 %
0009 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0010 
0011 % History:
0012 %    17.3.2005 SS  The first implementation
0013 %
0014 % Copyright (C) 2005 Simo Särkkä
0015 %               2008 Jouni Hartikainen
0016 %
0017 % $Id: $
0018 %
0019 % This software is distributed under the GNU General Public
0020 % Licence (version 2 or later); please refer to the file
0021 % Licence.txt, included with the software, for details.
0022 
0023   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0024   %
0025   % Generic parameters
0026   %
0027   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0028 
0029   % Plotting flags
0030   print_figures = 1;
0031   save_figures = 1;
0032   
0033   
0034   %
0035   % Time is running from t0 to t1 on dt steps
0036   %
0037   t0 = 0;
0038   t1 = 2;
0039   dt = 0.01;
0040 
0041   % Dimensionality of state space
0042   m = 4;
0043 
0044   %
0045   % Sensor positions and precisions
0046   %
0047   S1 = [0.2;1.5];
0048   S2 = [1.0;-0.5];
0049   % Standard
0050   sd = 0.02; 
0051   % Covariance matrix of the measurement model
0052   R = sd^2;
0053 
0054   %
0055   % Detection probability and clutter parameters
0056   %
0057   pd = 0.8;    % Probability of detection
0058   mc = 5;      % Mean number of clutter measurements per time instance
0059   CD = 1/2/pi; % Density of clutter measurements
0060   
0061   %
0062   % Measurement mean and derivative
0063   %
0064 
0065   h_func     = @az_h;
0066   dh_dx_func = @az_dh_dx;
0067 
0068   der_check(h_func, dh_dx_func, 1, randn(4,1), S1);
0069   der_check(h_func, dh_dx_func, 1, randn(4,1), S2);
0070 
0071   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0072   %
0073   % Data generation
0074   %
0075   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0076   
0077   %
0078   % Create curved trajectory A and angle
0079   % measurements from the two sensors
0080   %
0081   w = 1;
0082   F = [0 0 1 0;
0083        0 0 0 1;
0084        0 0 0 -w;
0085        0 0 w 0];
0086   T  = (t0:dt:t1);
0087   X1 = lti_int([0;0;1;0],[],F,[],[],T);
0088   y1 = atan2(X1(2,:)-S1(2), X1(1,:)-S1(1)) + sd * randn(1,size(X1,2));
0089   y2 = atan2(X1(2,:)-S2(2), X1(1,:)-S2(1)) + sd * randn(1,size(X1,2));
0090   Y1 = [y1;y2];
0091 
0092   %
0093   % Create curved trajectory B and angle
0094   % measurements from the same two sensors
0095   %
0096   w = 1;
0097   F = [0 0 1 0;
0098        0 0 0 1;
0099        0 0 0 -w;
0100        0 0 w 0];
0101   T = (t0:dt:t1);
0102   X2 = lti_int([0;1;0;-1],[],F,[],[],T);
0103 
0104   %
0105   % Simulate non-cluttered measurements
0106   %
0107   y1 = atan2(X2(2,:)-S1(2), X2(1,:)-S1(1)) + sd * randn(1,size(X2,2));
0108   y2 = atan2(X2(2,:)-S2(2), X2(1,:)-S2(1)) + sd * randn(1,size(X2,2));
0109   Y2 = [y1;y2];
0110   
0111   %
0112   % Simulate cluttering, permute measurements to Y
0113   % and store the correct associations to DA, sensor
0114   % positions to S and times to TIme.
0115   %
0116   Y = {};
0117   DA = {};
0118   S = {};
0119   Time = {};
0120   for k=1:size(Y1,2)
0121     Y{k} = [];
0122     DA{k} = [];
0123     S{k} = [];
0124     Time{k} = [];
0125 
0126     if rand < pd
0127       Y{k} = [Y{k} Y1(1,k)];
0128       DA{k} = [DA{k} 1];
0129       S{k} = [S{k} S1];
0130       Time{k} = [Time{k} T(k)];
0131     end
0132   
0133     if rand < pd
0134       Y{k} = [Y{k} Y1(2,k)];
0135       DA{k} = [DA{k} 1];
0136       S{k} = [S{k} S2];
0137       Time{k} = [Time{k} T(k)];
0138     end
0139   
0140     if rand < pd
0141       Y{k} = [Y{k} Y2(1,k)];
0142       DA{k} = [DA{k} 2];
0143       S{k} = [S{k} S1];
0144       Time{k} = [Time{k} T(k)];
0145     end
0146   
0147     if rand < pd
0148       Y{k} = [Y{k} Y2(2,k)];
0149       DA{k} = [DA{k} 2];
0150       S{k} = [S{k} S2];
0151       Time{k} = [Time{k} T(k)];
0152     end
0153     
0154     nc = poissrnd(mc);
0155     for i=1:nc
0156       Y{k} = [Y{k} 2*pi*rand-pi];
0157       DA{k} = [DA{k} 0];
0158       S{k} = [S{k} [0;0]];
0159       Time{k} = [Time{k} T(k)];
0160     end
0161   
0162     if length(Y{k}) > 0
0163       ind = randperm(length(Y{k}));
0164       Y{k} = Y{k}(:,ind);
0165       DA{k} = DA{k}(:,ind);
0166       S{k} = S{k}(:,ind);
0167       Time{k} = Time{k}(ind);
0168     end
0169   end
0170 
0171   n = length(Y);
0172 
0173   %
0174   % Initialize EKF1 to values
0175   %
0176   %   x = 0
0177   %   y = 0,
0178   %   dx/dt = 1
0179   %   dy/dt = 0
0180 
0181   M0_1a = [0;0;0;0];
0182   P0_1a = diag([0.001 0.001 1 1]);
0183   M0_1b = [-0.4009;0.0702;0;0];
0184   P0_1b = P0_1a;
0185 
0186   % Initialize EKF2 to values
0187   %
0188   %   x = 0
0189   %   y = 1,
0190   %   dx/dt = 0
0191   %   dy/dt = -1
0192 
0193   M0_2a = [0;1;0;0];
0194   P0_2a = diag([0.001 0.001 1 1]);
0195   M0_2b = [0.1336;0.8480;0;0];
0196   P0_2b = P0_2a;
0197 
0198   %
0199   % Dynamic model is common for now
0200   %
0201   qx = 0.1;
0202   qy = 0.1;
0203   F = [0 0 1 0;
0204        0 0 0 1;
0205        0 0 0 0;
0206        0 0 0 0];
0207   [A,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt);
0208 
0209   %
0210   % EKF MCDA parameters
0211   %
0212   N = 100;
0213   NP = N;
0214   NT = 2;
0215   M = cell(2,N);
0216   P = cell(2,N);
0217   TP = [0.5;0.5];
0218 
0219   % Bimodal prior for both targets
0220   for i=1:floor(N/2)
0221     M{1,i} = M0_1a + 0.1*chol(P0_1a)'*randn(4,1);
0222     M{2,i} = M0_2a + 0.1*chol(P0_2a)'*randn(4,1);
0223     P{1,i} = P0_1a;
0224     P{2,i} = P0_2a;
0225   end
0226   for i=floor(N/2)+1:N
0227     M{1,i} = M0_1b + 0.1*chol(P0_1b)'*randn(4,1);
0228     M{2,i} = M0_2b + 0.1*chol(P0_2b)'*randn(4,1);
0229     P{1,i} = P0_1b;
0230     P{2,i} = P0_2b;
0231   end
0232 
0233   % Initialize particles
0234   PS = mcda_init(N,M,P);
0235   
0236   if print_figures
0237       % Plot the measurement data
0238       set(gcf,'PaperType','a4');
0239       set(gcf,'PaperPosition',[0.25 2.5 3.8 3]);
0240       set(gca,'Position',[0.25 2.5 3.8 3]);
0241       
0242       clf;  
0243       h=plot([Time{:}],[Y{:}],'x');
0244       set(h,'color',[0.0 0.0 0.0]);
0245       set(h,'markersize',0.5);
0246       set(h,'linewidth',2);
0247       set(gca,'FontSize',8);
0248       xlabel('Time [s]');
0249       ylabel('Measurement [rad]');
0250       ylim([-pi pi])
0251       if save_figures
0252           print('-dpsc','mcda_measurement.ps');          
0253       end
0254       fprintf('Measurement data, press enter\n');
0255       
0256       pause;
0257       
0258       % Plot initial estimate
0259       EST_M = zeros(m,NT);
0260       for i = 1:N
0261           EST_M = EST_M + PS{i}.W*[PS{i}.M{:}];
0262       end      
0263 
0264       % Plot trajectories
0265       hold off;
0266       h = plot(X1(1,1:end),X1(2,1:end),'-',...
0267                X2(1,1:end),X2(2,1:end),'-');
0268       set(h(1),'color',[0.0 0.0 0.0]);
0269       set(h(2),'color',[0.5 0.5 0.5]);            
0270       legend([h],'Target 1','Target 2');
0271       hold on;
0272       s = (0.05:0.05:1)';
0273 
0274       % Plot prior particles
0275       samp1 = zeros(4,500);
0276       samp2 = zeros(4,500);
0277       for j=1:size(samp1,2)
0278           ind = categ_rnd(PS);
0279           samp1(:,j) = gauss_rnd(M{1,ind},P{1,ind},1);
0280           samp2(:,j) = gauss_rnd(M{2,ind},P{2,ind},1);
0281       end
0282       tmp1 = samp1;
0283       tmp2 = samp2;
0284       
0285       h=plot(tmp1(1,:),tmp1(2,:),'.',tmp2(1,:),tmp2(2,:),'.');
0286       set(h,'markersize',2);
0287       set(h(1),'color',[0.0 0.0 0.0]);
0288       set(h(2),'color',[0.5 0.5 0.5]);      
0289 
0290       h=plot(EST_M(1,1),EST_M(2,1),'k*',EST_M(1,2),EST_M(2,2),'k*');
0291       h=plot(S1(1),S1(2),'k^',S2(1),S2(2),'k^');
0292       
0293       set(h,'markersize',4);
0294       axis([-0.5 1.5 -0.7 1.7]);
0295 
0296       if save_figures
0297           print('-dpsc','mcda_trajectory.ps');          
0298       end
0299       
0300       fprintf('Initial estimate, press enter\n');
0301       pause;
0302   end
0303   %
0304   % Track and animate
0305   %
0306   EST1 = zeros(4,size(Y1,2));
0307   EST2 = zeros(4,size(Y1,2));
0308 
0309   % Space for the particles
0310   SS1 = cell(n,NP);
0311   
0312   clf;
0313   fprintf('Tracking...');
0314   for k=1:length(Y)
0315     % Predict all targets
0316     PS = ekf_mcda_predict(PS,A,Q);
0317     
0318     % Space for data assosiations in the current time step
0319     % for both targets in each particle
0320     D1 = zeros(1,NP);
0321     D2 = zeros(1,NP);
0322     
0323     % Space for clutter and target priors
0324     CP = zeros(1,NP);
0325     TP = zeros(2,NP);
0326 
0327     % Loop over all measurement in the current time step
0328     for kk=1:length(Y{k})
0329       % Number of measurements not processed yet
0330       r = length(Y{k}) - kk + 1;
0331       
0332       for j=1:size(M,2)
0333           % Construct priors for each particle separately
0334 
0335           % Associated to targets 1 and 2 already
0336           if (D1(j)~=0) & (D2(j)~=0)
0337               CP(j) = 1;
0338               TP(:,j) = [0.5;0.5]; % Can be anything
0339           % Associated to target 1 already
0340           elseif (D1(j)~=0) & (D2(j)==0)
0341               CP(j) = (1-pd) + pd*(r-1)/r;
0342               tp = [0;pd/r];
0343               TP(:,j) = tp ./ sum(tp);
0344           % Associated to target 1 already
0345           elseif (D1(j)==0) & (D2(j)~=0)
0346               CP(j) = (1-pd) + pd*(r-1)/r;
0347               tp = [pd/r;0];
0348               TP(:,j) = tp ./ sum(tp);
0349           % Not associated to any target yet
0350           elseif (D1(j)==0) & (D2(j)==0)
0351               if r == 1
0352                   CP(j) = 1 - 2*pd*(1-pd);
0353                   tp = [pd*(1-pd);pd*(1-pd)];
0354                   TP(:,j) = tp ./ sum(tp);
0355               else
0356                   CP(j) = (1-pd)*(1-pd)+pd^2*(r-2)/r + 2*pd*(1-pd)*(r-1)/r;
0357                   tp = [pd*(1-pd)/r+pd^2/r;pd*(1-pd)/r+pd^2/r];
0358                   TP(:,j) = tp ./ sum(tp);
0359               end
0360           end
0361       end
0362       [PS,C] = ekf_mcda_update(PS,Y{k}(:,kk),dh_dx_func,R,h_func,...
0363                                [],TP,CP,CD,S{k}(:,kk));
0364       
0365       % Store the data associations
0366       ind = find(C==1);
0367       D1(ind) = 1;
0368     
0369       ind = find(C==2);
0370       D2(ind) = 1;
0371 
0372     end
0373     
0374 
0375 % $$$     EST_M = zeros(m,NT);
0376 % $$$     for i = 1:N
0377 % $$$         EST_M = EST_M + PS{i}.W*[PS{i}.M{:}];
0378 % $$$     end
0379 % $$$     EST1(:,k) = EST_M(:,1);
0380 % $$$     EST2(:,k) = EST_M(:,2);
0381     
0382     EST1(:,k) = calculate_mean(PS,1);
0383     EST2(:,k) = calculate_mean(PS,2);
0384 
0385     %
0386     % Store for smoother
0387     %
0388     SS1(k,:) = PS;
0389 % $$$     for i=1:N
0390 % $$$       MM1{i}(:,k)   = M{1,i};
0391 % $$$       PP1{i}(:,:,k) = P{1,i};
0392 % $$$       MM2{i}(:,k)   = M{2,i};
0393 % $$$       PP2{i}(:,:,k) = P{2,i};
0394 % $$$     end
0395 
0396     %
0397     % Resample if needed
0398     %
0399     n_eff = eff_particles(PS);
0400     fprintf('n_eff = %d\n',round(n_eff));
0401     if n_eff < N/4
0402       ind = resample(PS);
0403       PS = PS(ind);
0404       C = C(ind);
0405 
0406       PS = normalize_weights(PS);
0407       fprintf('Resampling done on time step %d\n',k);
0408     end
0409 
0410     % Animate
0411     if print_figures
0412         len = 1.5;
0413         dx11 = len*cos(Y1(1,k));
0414         dy11 = len*sin(Y1(1,k));
0415         dx21 = len*cos(Y1(2,k));
0416         dy21 = len*sin(Y1(2,k));
0417         dx12 = len*cos(Y2(1,k));
0418         dy12 = len*sin(Y2(1,k));
0419         dx22 = len*cos(Y2(2,k));
0420         dy22 = len*sin(Y2(2,k));
0421         
0422         hold off;
0423         plot(X1(1,1:k),X1(2,1:k),'-',...
0424              X2(1,1:k),X2(2,1:k),'-',...
0425              EST1(1,1:k),EST1(2,1:k),'--',...
0426              EST2(1,1:k),EST2(2,1:k),'--');
0427         legend('Actual 1','Actual 2','Estimated 1','Estimated 2');
0428         hold on;
0429         s = (0.05:0.05:1)';
0430         
0431         nvis = 500;
0432         MV1 = zeros(m,NP);
0433         PV1 = zeros(m,m,NP);
0434         MV2 = zeros(m,NP);
0435         PV2 = zeros(m,m,NP);
0436         for j=1:NP
0437             MV1(:,j)   = SS1{k,j}.M{1};
0438             PV1(:,:,j) = SS1{k,j}.P{1};
0439             MV2(:,j)   = SS1{k,j}.M{2};
0440             PV2(:,:,j) = SS1{k,j}.P{2};
0441         end
0442         tmp = [PS{:}];
0443         W = [tmp.W];
0444         samp1 = gmm_rnd(MV1,PV1,W,nvis);
0445         samp2 = gmm_rnd(MV2,PV2,W,nvis);
0446         
0447         H=plot(samp1(1,:),samp1(2,:),'r.',samp2(1,:),samp2(2,:),'g.');
0448         set(H,'markersize',2);
0449         plot(EST_M(1,1),EST_M(2,1),'k*',EST_M(1,2),EST_M(2,2),'k*',...
0450              [S1(1);S1(1)+s*dx11],[S1(2);S1(2)+s*dy11],'.',...
0451              [S2(1);S2(1)+s*dx21],[S2(2);S2(2)+s*dy21],'.',...
0452              [S1(1);S1(1)+s*dx12],[S1(2);S1(2)+s*dy12],'.',...
0453              [S2(1);S2(1)+s*dx22],[S2(2);S2(2)+s*dy22],'.');
0454         axis([-0.5 1.5 -1 1.5]);
0455         drawnow;
0456     end
0457   end
0458   fprintf('Done!\n');
0459   
0460   % Smoothed estimate
0461   fprintf('Smoothing...');
0462   [SS,SM1] = ekf_mcda_smooth(SS1,A,Q);
0463   fprintf('Done!\n');
0464   
0465   % Plot the results
0466   if print_figures
0467       
0468       % Number of particles in the visualization
0469       nvis = 50;      
0470 
0471       % Filtered estimates
0472       clf;
0473       title('Extended Kalman Filter Estimate');
0474       hold on
0475       
0476       % Draw particles in each time step
0477       MV1 = zeros(m,NP);
0478       PV1 = zeros(m,m,NP);
0479       MV2 = zeros(m,NP);
0480       PV2 = zeros(m,m,NP);
0481       tmp = [PS{:}];
0482       W = [tmp.W];      
0483       
0484       for k = 1:n
0485           for j=1:NP
0486               MV1(:,j)   = SS1{k,j}.M{1};
0487               PV1(:,:,j) = SS1{k,j}.P{1};
0488               MV2(:,j)   = SS1{k,j}.M{2};
0489               PV2(:,:,j) = SS1{k,j}.P{2};
0490           end
0491           
0492           samp1 = gmm_rnd(MV1,PV1,W,nvis);
0493           samp2 = gmm_rnd(MV2,PV2,W,nvis);
0494           
0495           h=plot(samp1(1,:),samp1(2,:),'r.',samp2(1,:),samp2(2,:),'g.');
0496           set(h(1),'color',[0.5 0.5 0.5]);
0497           set(h(2),'color',[0.0 0.0 0.0]);      
0498           set(h,'markersize',2);          
0499       end
0500 
0501       h = plot(X1(1,:),X1(2,:),'--',...
0502                X2(1,:),X2(2,:),'--',...
0503                EST1(1,:),EST1(2,:),'-',...
0504                EST2(1,:),EST2(2,:),'-');
0505       legend([h],'Actual target 1','Actual target 2',...
0506              'Filtered target 1','Filtered target 2');
0507       set(h(1),'color',[0.0 0.0 0.0]);
0508       set(h(2),'color',[0.5 0.5 0.5]);      
0509       set(h(3),'color',[0.0 0.0 0.0]);
0510       set(h(4),'color',[0.5 0.5 0.5]);      
0511       set(h,'linewidth',1)
0512       set(gca,'FontSize',4);
0513       hold off
0514       if save_figures
0515           print('-dpsc','mcda_filtered.ps');
0516       end
0517 
0518       fprintf('Showing the filtered estimates. Press any key to continue.\n')
0519       pause;
0520       
0521       % Plot the smoothed estimates
0522       clf
0523       title('Smoothed Extended Kalman Filter Estimate');
0524       
0525       hold on
0526       % Plot smoothed particles in each time step
0527       %
0528       % Space for particles
0529       MV1 = zeros(m,NP);
0530       PV1 = zeros(m,m,NP);
0531       MV2 = zeros(m,NP);
0532       PV2 = zeros(m,m,NP);
0533       tmp = [PS{:}];
0534       W = [tmp.W];
0535       
0536       for k = 1:n
0537           for j=1:NP
0538               MV1(:,j)   = SS{k,j}.M{1};
0539               PV1(:,:,j) = SS{k,j}.P{1};
0540               MV2(:,j)   = SS{k,j}.M{2};
0541               PV2(:,:,j) = SS{k,j}.P{2};
0542           end
0543           samp1 = gmm_rnd(MV1,PV1,W,nvis);
0544           samp2 = gmm_rnd(MV2,PV2,W,nvis);
0545           
0546           h=plot(samp1(1,:),samp1(2,:),'r.',samp2(1,:),samp2(2,:),'g.');
0547           set(h,'markersize',2);
0548           set(h(1),'color',[0.5 0.5 0.5]);
0549           set(h(2),'color',[0.0 0.0 0.0]);          
0550       end
0551       h = plot(X1(1,:),X1(2,:),'--',...
0552                X2(1,:),X2(2,:),'--',...
0553                SM1{1}(1,:),SM1{1}(2,:),'-',...
0554                SM1{2}(1,:),SM1{2}(2,:),'-');
0555       legend([h],'True target 1','True target 2',...
0556              'Smoothed target 1','Smoothed target 2');
0557       set(h(1),'color',[0.0 0.0 0.0]);
0558       set(h(2),'color',[0.5 0.5 0.5]);      
0559       set(h(3),'color',[0.0 0.0 0.0]);
0560       set(h(4),'color',[0.5 0.5 0.5]);      
0561 
0562       set(h,'linewidth',1)
0563       set(gca,'FontSize',4);
0564 
0565       hold off
0566       if save_figures
0567           print('-dpsc','mcda_smoothed.ps');
0568       end
0569       
0570       fprintf('Showing the smoothed estimates.\n')
0571 
0572   end