Description of ekf_mt_demo2

0001 % EKF_MT_DEMO2 BOT Tracking of unknown number of 2D signals with 2d attributes
0002 %
0003 % Processes the target deaths during the prediction step.
0004 %
0005 % History:
0006 %    13.02.2008 First official version
0007 %
0008 % Copyright (C) 2005 Simo Särkkä
0009 %               2008 Jouni Hartikainen
0010 %
0011 % $Id: ekf_mt_demo2.m, $
0012 %
0013 % This software is distributed under the GNU General Public
0014 % Licence (version 2 or later); please refer to the file
0015 % Licence.txt, included with the software, for details.
0016 
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 % Sensors
0026 sensors = {};
0027 c = 0;
0028 c = c + 1;
0029 sensors{c} = [-1;-2];
0030 c = c + 1;
0031 sensors{c} = [-1;1];
0032 c = c + 1;
0033 sensors{c} = [1;-2];
0034 c = c + 1;
0035 sensors{c} = [1;1];
0036 sensors = [sensors{:}];
0037 
0038 %
0039 % Common parameters
0040 %
0041 N  = 10; % Number of particles
0042 sd  = 0.05;
0043 sda = 0.01;
0044 dt  = 0.01;
0045 nsteps = 500;
0046 ntargets = 4;
0047 nsens = sum(sel);
0048 states = cell(ntargets,nsteps);
0049 attrs  = cell(ntargets,1);
0050 attrs{1} = [0;1];
0051 attrs{2} = [1;0];
0052 attrs{3} = [2;2];
0053 attrs{4} = [0;-2];
0054 
0055 %
0056 % Measurement function and derivative
0057 %
0058 h_func     = @az_h_2a;
0059 dh_dx_func = @az_dh_dx_2a;
0060 der_check(h_func, dh_dx_func, 1, randn(6,1), sensors);
0061 
0062 %
0063 % Target 1:
0064 %
0065 % Create route with two turns
0066 % and some random perturbations.
0067 % Also generate measurements
0068 % with some noise added.
0069 %
0070 j = 1;
0071 if sel(j)
0072     a = zeros(1,nsteps);
0073     a(1,50:100)  = pi/2/51/dt + 0.01*randn(1,51);
0074     a(1,200:250) = pi/2/51/dt + 0.01*randn(1,51);
0075     a(1,350:400) = pi/2/51/dt + 0.01*randn(1,51);
0076     x = [0;0;1;0];
0077     for i=1:500
0078         F = [0 0  1    0;...
0079             0 0  0    1;...
0080             0 0  0   a(i);...
0081             0 0 -a(i) 0];
0082         x = expm(F*dt)*x;
0083         states{j,i} = x;
0084     end
0085 end
0086 
0087 %
0088 % Target 2:
0089 %
0090 % Create straight line motion
0091 % from down to up
0092 %
0093 j = 2;
0094 if sel(j)
0095     F = [0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0];
0096     x = [0;-2;0.1;1];
0097     for i=100:350
0098         x = expm(F*dt)*x;
0099         states{j,i} = x;
0100     end
0101 end
0102 
0103 %
0104 % Target 3:
0105 %
0106 % Create straight line motion
0107 % from left to right
0108 %
0109 j = 3;
0110 if sel(j)
0111     F = [0 0 1 0; 0 0 0 1; 0 0 0 0; 0 0 0 0];
0112     x = [-1;-1;1;0.1];
0113     for i=100:300
0114         x = expm(F*dt)*x;
0115         states{j,i} = x;
0116     end
0117 end
0118 
0119 %
0120 % Target 4:
0121 %
0122 % Circular motion
0123 %
0124 j = 4;
0125 if sel(j)
0126     w1 = 1;
0127     w2 = 0.01;
0128     F = [0 0 1 0; 0 0 0 1; 0 0 0 w1; 0 0 -w1 0];
0129     x = [-1;0;1;0];
0130     for i=150:400
0131         x = expm(F*dt)*x;
0132         states{j,i} = x;
0133     end
0134 end
0135 
0136 %
0137 % Generate actual measurements
0138 % and attribute measurements
0139 %
0140 ATT = {};
0141 Z   = {};
0142 sid = {};
0143 T = (0:nsteps-1)*dt;
0144 i = 0;
0145 for k=1:nsteps
0146     Z{k}   = [];
0147     ATT{k} = [];
0148     sid{k} = [];
0149     for j=1:ntargets
0150         if ~isempty(states{j,k})
0151             for i=1:size(sensors,2)
0152                 z = az_h(states{j,k},sensors(:,i)) + sd*randn;
0153                 a = attrs{j} + sda*randn(2,1);
0154                 Z{k}   = [Z{k}   z];
0155                 ATT{k} = [ATT{k} a];
0156                 sid{k} = [sid{k} i];
0157             end
0158         end
0159     end
0160     ind  = randperm(size(Z{k},2));
0161     Z{k}   = Z{k}(:,ind);
0162     ATT{k} = ATT{k}(:,ind);
0163     sid{k} = sid{k}(:,ind);
0164 end
0165 
0166 %
0167 % Plot the data
0168 %
0169 hold off;
0170 for j=1:4
0171     XX = [];
0172     for k=1:nsteps
0173         if ~isempty(states{j,k})
0174             XX = [XX states{j,k}];
0175         end
0176     end
0177     if ~isempty(XX)
0178         plot(XX(1,:),XX(2,:),'k');
0179         hold on;
0180     end
0181 end
0182 %  h=plot(Y(1,:),Y(2,:),'rx');
0183 %  set(h,'markersize',4);
0184 axis([-1.5 1.5 -2.5 1]);
0185 drawnow;
0186 %  pause;
0187 
0188 %
0189 % KF parameters
0190 %
0191 M0 = [0;0;0;0;0;0];
0192 P0 = diag([4 4 4 4 4 4]);
0193 qx = 0.1;
0194 qy = 0.1;
0195 qa = 0.01;
0196 F = [0 0 1 0 0 0;
0197     0 0 0 1 0 0;
0198     0 0 0 0 0 0;
0199     0 0 0 0 0 0;
0200     0 0 0 0 0 0;
0201     0 0 0 0 0 0];
0202 [A,Q] = lti_disc(F,[],diag([0 0 qx qy qa qa]),dt);
0203 R  = diag([sd^2 sda^2 sda^2]);
0204 
0205 %
0206 % Initialize filter
0207 %
0208 S = nmcda_init(N,M0,P0,dt);
0209 
0210 %
0211 % Prior probabilities of clutter
0212 % and of births and deaths
0213 %
0214 alpha = 2;
0215 beta = 1;
0216 cd = 1/2/pi;    % Density of clutter
0217 cp = 0.01;      % Clutter prior
0218 
0219 %
0220 % Track and animate
0221 %
0222 count = 0;
0223 SS = {};
0224 for k=1:nsteps
0225     [S,W] = ekf_nmcda_predict_dp(S,W,A,Q,[],[],[],T(k),alpha,beta);
0226     for j=1:length(Z{k})
0227         z = [Z{k}(j);ATT{k}(:,j)];
0228         [S,W,E] = ekf_nmcda_update_dp(S,W,z,T(k),dh_dx_func,R,h_func,...
0229             [],cp,cd,[],sensors(:,sid{k}(j)));
0230         count = count + 1;
0231         SS(count,:) = S;
0232         fprintf('%d/%d: %s\n',k,size(Z,2),E{1});
0233     end
0234 
0235     if rem(k,1) == 0
0236         %
0237         % Plot true trajectories
0238         %
0239         hold off;
0240         for j=1:4
0241             XX = [];
0242             for kk=1:nsteps
0243                 if ~isempty(states{j,kk})
0244                     XX = [XX states{j,kk}];
0245                 end
0246             end
0247             if ~isempty(XX)
0248                 plot(XX(1,:),XX(2,:),'k');
0249                 hold on;
0250             end
0251         end
0252 
0253         %
0254         % Plot measurements
0255         %
0256         %    h=plot(Y(1,:),Y(2,:),'rx');
0257         %    set(h,'markersize',4);
0258         hold on;
0259         len = 3;
0260         for j=1:length(Z{k})
0261             dx = len*cos(Z{k}(j));
0262             dy = len*sin(Z{k}(j));
0263             sx = sensors(1,sid{k}(j));
0264             sy = sensors(2,sid{k}(j));
0265             h = plot(sx,sy,'^');
0266             set(h,'markersize',10);
0267             plot([sx;sx+dx],[sy;sy+dy],'--');
0268         end
0269 
0270         %
0271         % Plot targets
0272         %
0273         for j=1:4
0274             if ~isempty(states{j,k})
0275                 plot(states{j,k}(1),states{j,k}(2),'k*');
0276             end
0277         end
0278         cols='gbkymcgbkymcgbkymcgbkymc';
0279         chars='......xxxxxxoooooo******';
0280         nvis = 100;
0281         for i=1:length(S)
0282             n = round(S{i}.W*nvis);
0283             if n>0
0284                 for j=1:length(S{i}.M)
0285                     x = gauss_rnd(S{i}.M{j},S{i}.P{j},n);
0286                     h = plot(x(1,:),x(2,:),[cols(j) chars(j)]);
0287                     set(h,'markersize',6);
0288                 end
0289             end
0290         end
0291         axis([-1.5 1.5 -2.5 1]);
0292         title('Tracking with EKF/NMCDA and Attributes');
0293         drawnow;
0294     end
0295 
0296     %
0297     % Resample if needed
0298     %
0299     S = normalize_weights(S);
0300     W = get_weights(S)
0301     n_eff = eff_particles(W);
0302     %    fprintf('n_eff = %d\n',round(n_eff));
0303     if n_eff < N/4
0304         ind = resample(W);
0305         S = S(ind);
0306         SS = SS(:,ind);
0307         W = ones(1,N)/N;
0308         S = set_weights(S,W);
0309         fprintf('Resampling done on time step %d\n',k);
0310     end
0311 
0312 end
ekf_mt_demo2_dp

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
