kf_mt_demo_dp

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

 Processes the target deaths during the prediction step.


 History:
    13.02.2008 First official version

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

 $Id: kf_mt_demo_dp.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_DP Demo of tracking unknown number of Gaussian 2d signals via RBPF
0002 %
0003 % Processes the target deaths during the prediction step.
0004 %
0005 %
0006 % History:
0007 %    13.02.2008 First official version
0008 %
0009 % Copyright (C) 2005 Simo Särkkä
0010 %               2008 Jouni Hartikainen
0011 %
0012 % $Id: kf_mt_demo_dp.m, $
0013 %
0014 % This software is distributed under the GNU General Public
0015 % Licence (version 2 or later); please refer to the file
0016 % Licence.txt, included with the software, for details.
0017 
0018 % Select subset of target into scenario
0019 % Select all: sel = [1 1 1 1];
0020 % First only: sel = [1 0 0 0];
0021 %
0022 sel = [1 1 1 1];
0023 
0024 %
0025 % Common parameters
0026 %
0027 sd = 0.05;
0028 dt = 0.01;
0029 nsteps = 500;
0030 ntargets = 4;
0031 states = cell(ntargets,nsteps);
0032 NT = zeros(1,nsteps);
0033 
0034 %
0035 % Target 1:
0036 %
0037 % Create route with two turns
0038 % and some random perturbations.
0039 % Also generate measurements
0040 % with some noise added.
0041 %
0042 j = 1;
0043 if sel(j)
0044     a = zeros(1,nsteps);
0045     a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
0046     a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
0047     a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
0048     x = [0;0;1;0];
0049     for i=1:500
0050         F = [0 0  1    0;...
0051             0 0  0    1;...
0052             0 0  0   a(i);...
0053             0 0 -a(i) 0];
0054         x = expm(F*dt)*x;
0055         states{j,i} = x;
0056         NT(i) = NT(i) + 1;
0057     end
0058 end
0059 
0060 %
0061 % Target 2:
0062 %
0063 % Create straight line motion
0064 % from down to up
0065 %
0066 j = 2;
0067 if sel(j)
0068     F = [0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0];
0069     x = [0;-2;0.1;1];
0070     for i=100:350
0071         x = expm(F*dt)*x;
0072         states{j,i} = x;
0073         NT(i) = NT(i) + 1;
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;-1;1;0.1];
0087     for i=100:300
0088         x = expm(F*dt)*x;
0089         states{j,i} = x;
0090         NT(i) = NT(i) + 1;
0091     end
0092 end
0093 
0094 %
0095 % Target 4:
0096 %
0097 % Circular motion
0098 %
0099 j = 4;
0100 if sel(j)
0101     w1 = 1;
0102     w2 = 0.01;
0103     F = [0 0 1 0; 0 0 0 1; 0 0 0 w1; 0 0 -w1 0];
0104     x = [-1;0;1;0];
0105     for i=100:400
0106         x = expm(F*dt)*x;
0107         states{j,i} = x;
0108         NT(i) = NT(i) + 1;
0109     end
0110 end
0111 
0112 %
0113 % Generate actual measurements
0114 %
0115 Y = [];
0116 T = (0:nsteps-1)*dt;
0117 i = 0;
0118 for k=1:nsteps
0119     c = [];
0120     for j=1:ntargets
0121         if ~isempty(states{j,k})
0122             c = [c j];
0123         end
0124     end
0125     if ~isempty(c)
0126         c = c(categ_rnd(ones(1,length(c))/length(c),1));
0127         i = i+1;
0128         Y(:,i) = states{c,k}(1:2) + sd*randn(2,1);
0129     end
0130 end
0131 
0132 %
0133 % Plot the data
0134 %
0135 hold off;
0136 for j=1:4
0137     XX = [];
0138     for k=1:nsteps
0139         if ~isempty(states{j,k})
0140             XX = [XX states{j,k}];
0141         end
0142     end
0143     if ~isempty(XX)
0144         plot(XX(1,:),XX(2,:),'k');
0145         hold on;
0146     end
0147 end
0148 h=plot(Y(1,:),Y(2,:),'rx');
0149 set(h,'markersize',4);
0150 %  pause;
0151 
0152 %
0153 % KF parameters
0154 %
0155 M0 = [0;0;0;0];
0156 P0 = diag([10 10 10 10]);
0157 qx = 0.1;
0158 qy = 0.1;
0159 F = [0 0 1 0;
0160     0 0 0 1;
0161     0 0 0 0;
0162     0 0 0 0];
0163 [A,Q] = lti_disc(F,[],diag([0 0 qx qy]),dt);
0164 H = [1 0 0 0; 0 1 0 0];
0165 R = diag([sd^2 sd^2]);
0166 
0167 %
0168 % Initialize filter
0169 %
0170 N = 10;
0171 S = kf_nmcda_init(N,M0,P0,dt);
0172 
0173 %
0174 % Prior probabilities of clutter
0175 % and of births and deaths
0176 %
0177 cd = 1/4;       % Density of clutter
0178 cp = 0.0;       % Clutter prior
0179 alpha = 2;
0180 beta = 0.5;
0181 
0182 %
0183 % Track and animate
0184 %
0185 SS = cell(size(Y,2),size(S,2));
0186 for k=1:size(Y,2)
0187     S = kf_nmcda_predict_dp(S,A,Q,[],[],T(k),alpha,beta);
0188     [S,E] = kf_nmcda_update_dp(S,Y(:,k),T(k),H,R,cp,cd,[]);
0189 
0190     fprintf('%d/%d: %s\n',k,size(Y,2),E{1});
0191 
0192     if rem(k,2) == 0
0193         hold off;
0194         for j=1:4
0195             XX = [];
0196             for kk=1:nsteps
0197                 if ~isempty(states{j,kk})
0198                     XX = [XX states{j,kk}];
0199                 end
0200             end
0201             if ~isempty(XX)
0202                 plot(XX(1,:),XX(2,:),'k');
0203                 hold on;
0204             end
0205         end
0206         h=plot(Y(1,:),Y(2,:),'rx');
0207         set(h,'markersize',4);
0208         hold on;
0209 
0210         for j=1:4
0211             if ~isempty(states{j,k})
0212                 plot(states{j,k}(1),states{j,k}(2),'k*');
0213             end
0214         end
0215         h=plot(Y(1,:),Y(2,:),'rx');
0216         set(h,'markersize',4);
0217 
0218         cols='gbkymcgbkymc';
0219         chars='......xxxxxxoooooo******';
0220         nvis = 100;
0221         for i=1:length(S)
0222             n = round(S{i}.W*nvis);
0223             if n>0
0224                 for j=1:length(S{i}.M)
0225                     x = gauss_rnd(S{i}.M{j},S{i}.P{j},n);
0226                     h=plot(x(1,:),x(2,:),[cols(j) chars(j)]);
0227                     set(h,'markersize',4);
0228                 end
0229             end
0230         end
0231         drawnow;
0232     end
0233 
0234     %
0235     % Store
0236     %
0237     SS(k,:) = S;
0238 
0239     %
0240     % Resample if needed
0241     %
0242     S = normalize_weights(S);
0243     W = get_weights(S);
0244     n_eff = eff_particles(W);
0245     if n_eff < N/4
0246         ind = resampstr(W);
0247         S = S(ind);
0248         SS = SS(:,ind);
0249         W = ones(1,N)/N;
0250         S = set_weights(S,W);
0251         fprintf('Resampling done on time step %d\n',k);
0252     end
0253 end
0254 
0255 kf_mt_plot2d;
0256