ukf_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.

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

 History:
    17.3.2005 SS  The first implementation

 Copyright (C) 2005 Simo Särkkä
               2008 Jouni Hartikainen

 $Id: $

 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.

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