kf_mt_demo

 KF_MT_DEMO Demo of tracking unknown number of Gaussian 2d signals via RBPF


 History:
    13.12.2007 First official version

 Copyright (C) 2003 Simo Särkkä
               2007 Jouni Hartikainen

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