Description of ekf_mt_demo1

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

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
