Description of ekf_mt

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
