Description of ekf_mt

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
