kf_mt_demo2

 KF_MT_DEMO2 Tracking unknown number of 2D signals with measurement restriction


 History:
    13.02.2008 First official version

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

 $Id: kf_mt_demo2.m, $

 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 % KF_MT_DEMO2 Tracking unknown number of 2D signals with measurement restriction
0002 %
0003 %
0004 % History:
0005 %    13.02.2008 First official version
0006 %
0007 % Copyright (C) 2005 Simo Särkkä
0008 %               2008 Jouni Hartikainen
0009 %
0010 % $Id: kf_mt_demo2.m, $
0011 %
0012 % This software is distributed under the GNU General Public
0013 % Licence (version 2 or later); please refer to the file
0014 % Licence.txt, included with the software, for details.
0015 
0016 randn('state',0);
0017 rand('state',0);
0018 
0019 %
0020 % Select subset of target into scenario
0021 % Select all: sel = [1 1 1 1];
0022 % First only: sel = [1 0 0 0];
0023 %
0024 sel = [1 1 1 1];
0025 %  sel = [0 1 0 0];
0026 
0027 %
0028 % Common parameters
0029 %
0030 N = 100;
0031 sd = 0.05;
0032 dt = 0.01;
0033 nsteps = 500;
0034 ntargets = 4;
0035 states = cell(ntargets,nsteps);
0036 times  = dt * (0:nsteps-1);
0037 
0038 %
0039 % Prior probabilities of clutter
0040 % and of births and deaths
0041 %
0042 cd = 1/16; % Density of clutter
0043 pd = 0.95; % Detection probability
0044 mc = 10;    % Mean number of clutter measurements
0045 alpha = 2;
0046 beta = 0.5;
0047 
0048 %
0049 % Target 1:
0050 %
0051 % Create route with two turns
0052 % and some random perturbations.
0053 % Also generate measurements
0054 % with some noise added.
0055 %
0056 j = 1;
0057 if sel(j)
0058     a = zeros(1,nsteps);
0059     a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
0060     a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
0061     a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
0062     x = [0;1;1;0];
0063     for i=1:500
0064         F = [0 0  1    0;...
0065             0 0  0    1;...
0066             0 0  0   a(i);...
0067             0 0 -a(i) 0];
0068         x = expm(F*dt)*x;
0069         states{j,i} = x;
0070     end
0071 end
0072 
0073 %
0074 % Target 2:
0075 %
0076 % Create straight line motion
0077 % from down to up
0078 %
0079 j = 2;
0080 if sel(j)
0081     F = [0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0];
0082     x = [0;-1;0.1;1];
0083     for i=100:350
0084         x = expm(F*dt)*x;
0085         states{j,i} = x;
0086     end
0087 end
0088 
0089 %
0090 % Target 3:
0091 %
0092 % Create straight line motion
0093 % from left to right
0094 %
0095 j = 3;
0096 if sel(j)
0097     F = [0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0];
0098     x = [-1;0;1;0.1];
0099     for i=100:300
0100         x = expm(F*dt)*x;
0101         states{j,i} = x;
0102     end
0103 end
0104 
0105 %
0106 % Target 4:
0107 %
0108 % Circular motion
0109 %
0110 j = 4;
0111 if sel(j)
0112     w1 = 1;
0113     w2 = 0.01;
0114     F = [0 0 1 0; 0 0 0 1; 0 0 0 w1; 0 0 -w1 0];
0115     x = [-1;1;1;0];
0116     for i=100:400
0117         x = expm(F*dt)*x;
0118         states{j,i} = x;
0119     end
0120 end
0121 
0122 %
0123 % Generate actual measurements
0124 %
0125 Y = [];
0126 C = [];
0127 T = [];
0128 t = 0;
0129 i = 0;
0130 for k=1:nsteps
0131     y = [];
0132     c = [];
0133     n = 0;
0134     for j=1:ntargets
0135         if ~isempty(states{j,k})
0136             if (rand<pd)
0137                 y = [y states{j,k}(1:2)+sd*randn(2,1)];
0138                 c = [c j];
0139             end
0140             n = n + 1;
0141         end
0142     end
0143 
0144     nc = poisson_rnd(mc);
0145     for j=1:nc
0146         y = [y [4*rand-2;4*rand-2]];
0147         c = [c 0];
0148     end
0149 
0150     ind = randperm(size(y,2));
0151     y = y(:,ind);
0152     c = c(ind);
0153 
0154     for j=1:size(y,2)
0155         i = i+1;
0156         Y(:,i) = y(:,j);
0157         T(i) = t;
0158         C(i) = c(j);
0159         NT(i) = n;
0160     end
0161     t = t + dt;
0162 end
0163 
0164 %
0165 % Plot the data
0166 %
0167 hold off;
0168 for j=1:4
0169     XX = [];
0170     for k=1:nsteps
0171         if ~isempty(states{j,k})
0172             XX = [XX states{j,k}];
0173         end
0174     end
0175     if ~isempty(XX)
0176         plot(XX(1,:),XX(2,:),'k');
0177         hold on;
0178     end
0179 end
0180 h=plot(Y(1,:),Y(2,:),'rx');
0181 set(h,'markersize',4);
0182 %  pause;
0183 
0184 %
0185 % KF parameters
0186 %
0187 M0 = [0;0;0;0];
0188 P0 = diag([10 10 10 10]);
0189 qx = 0.1;
0190 qy = 0.1;
0191 F = [0 0 1 0;
0192     0 0 0 1;
0193     0 0 0 0;
0194     0 0 0 0];
0195 [A,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt);
0196 H = [1 0 0 0; 0 1 0 0];
0197 R = diag([sd^2 sd^2]);
0198 
0199 % Initialize particle structures for filter
0200 S = kf_nmcda_init(N,M0,P0,dt);
0201 
0202 %
0203 % Track and animate
0204 %
0205 %  SS = cell(size(Y,2),size(S,2));
0206 SS = {};
0207 t = 0;
0208 k = 1;
0209 count = 0;
0210 while k <= size(Y,2)
0211     if T(k) - t > 0
0212         S = kf_nmcda_predict(S,A,Q);
0213     end
0214     
0215     % Gather all the measurements of the current
0216     % sampling period into a matrix.
0217     y = [];
0218     t = T(k);
0219     while t == T(k)
0220         y = [y Y(:,k)];
0221         k = k+1;
0222         if k > size(Y,2)
0223             break;
0224         end
0225     end
0226 
0227     [SA,E] = kf_nmcda_update_sa(S,y,t,H,R,pd,cd,[],alpha,beta);
0228 
0229     %
0230     % Store
0231     %
0232     for j=1:length(SA)
0233         count = count+1;
0234         S = SA{j};
0235         SS(count,:) = S;
0236     end
0237 
0238     W = get_weights(S);
0239     [tmp,ind] = max(W);
0240 
0241     fprintf('Step %d/%d: Targets = %d\n',k,size(Y,2),length(S{ind}.M));
0242     for j=1:size(E,2)
0243         fprintf('[%d] %s.\n',j,E{ind,j});
0244     end
0245 
0246     if (rem(k,10) == 0) & (k <= size(Y,2))
0247         hold off;
0248         for j=1:4
0249             XX = [];
0250             for kk=1:nsteps
0251                 if ~isempty(states{j,kk})
0252                     XX = [XX states{j,kk}];
0253                 end
0254             end
0255             if ~isempty(XX)
0256                 plot(XX(1,:),XX(2,:),'k');
0257                 hold on;
0258             end
0259         end
0260 
0261         ind0 = find(C == 0);
0262         ind1 = find(C ~= 0);
0263         h=plot(Y(1,ind0),Y(2,ind0),'rx',Y(1,ind1),Y(2,ind1),'bx');
0264         set(h,'markersize',4);
0265         hold on;
0266 
0267         ind = find(T == T(k));
0268         h=plot(Y(1,ind),Y(2,ind),'ko');
0269         set(h,'markersize',10);
0270 
0271         ind = find(times == T(k));
0272         for j=1:4
0273             if ~isempty(ind)
0274                 if ~isempty(states{j,ind})
0275                     plot(states{j,ind}(1),states{j,ind}(2),'k*');
0276                 end
0277             end
0278         end
0279 
0280         cols='gbkymcgbkymcgbkymcgbkymc';
0281         chars='......xxxxxxoooooo******';
0282         nvis = 100;
0283         for i=1:length(S)
0284             n = round(S{i}.W*nvis);
0285             if n>0
0286                 for j=1:length(S{i}.M)
0287                     x = gauss_rnd(S{i}.M{j},S{i}.P{j},n);
0288                     h = plot(x(1,:),x(2,:),[cols(j) chars(j)]);
0289                     set(h,'markersize',10);
0290                 end
0291             end
0292         end
0293         drawnow;
0294     end
0295 
0296     %
0297     % Resample if needed
0298     %
0299     n_eff = eff_particles(S);
0300     %    fprintf('n_eff = %d\n',round(n_eff));
0301     if n_eff < N/4
0302         ind = resample(S);
0303         S = S(ind);
0304         SS = SS(:,ind);
0305         for i = 1:N
0306             S{i}.W = 1/N;
0307         end
0308         fprintf('Resampling done on time step %d\n',k);
0309     end
0310 end
0311 
0312 %  if 0
0313 ind = resample(W);
0314 S = S(ind);
0315 SS = SS(:,ind);
0316 for i = 1:N
0317     S{i}.W = 1/N;
0318 end
0319 
0320 fprintf('Resampling done at end.\n');
0321 
0322 kf_mt_plot2d;