multisignal_demo_dp

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Demonstration of tracking unknown varying number of Gaussian
 signals via Rao-Blackwellized Particle Filtering

       Copyright (C) Simo Särkkä, 2003

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %
0003 % Demonstration of tracking unknown varying number of Gaussian
0004 % signals via Rao-Blackwellized Particle Filtering
0005 %
0006 %       Copyright (C) Simo Särkkä, 2003
0007 %
0008 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0009 
0010 %  rand('state',0);
0011 %  randn('state',0);
0012 %  rand('state',1);
0013 %  randn('state',1);
0014 
0015 %
0016 % Generate data
0017 %
0018 dt = 0.01;
0019 sd = 0.2;
0020 T = (1:1500)*dt;
0021 X = cell(1,3);
0022 X{1} = sin(T);
0023 X{2} = 4 + 0.5*sin(0.9*T);
0024 X{3} = -2 + cos(0.5*T);
0025 X{4} = 3-0.3*T;
0026 
0027 
0028 %
0029 % Prior probabilities of clutter
0030 % and of births and deaths
0031 %
0032 cd = 1/10;      % Density of clutter
0033 cp = 0.01;      % Clutter prior
0034 
0035 alpha = 2;      % Parameters of gamma distribution
0036 beta = 1;       % modeling the deaths of signals
0037 
0038 %alpha = 2;    % Parameters of gamma distribution
0039 %beta = 0.4;     % modeling the deaths of signals
0040 
0041 %alpha = 2;      % Parameters of gamma distribution
0042 %beta = 0.1;     % modeling the deaths of signals
0043 
0044 %  beta = 0.5;
0045 %  beta = 0.1;
0046 
0047 % Signals currently visible
0048 c = [1 0 0 1]/2;
0049 
0050 Y = zeros(1,size(T,2));
0051 NT = [];
0052 C  = [];
0053 
0054 for k=1:size(T,2)
0055     NT = [NT sum(c~=0)];
0056 
0057     % Clutter measurement
0058     if rand < cp
0059         Y(:,k) = 10*rand-5;
0060         % Measurement from an actual target
0061     else
0062         %
0063         j = categ_rnd(c);
0064         Y(:,k) = X{j}(:,k)+sd*randn(size(Y,1),1);
0065     end
0066 
0067     %%%%%%  Change visibility of signals %%%%%%
0068     if k == 100
0069         c = [1 1 0 1];
0070         c = c ./ sum(c);
0071     end
0072 
0073     if k == 200
0074         c = [1 1 1 1];
0075         c = c ./ sum(c);
0076     end
0077 
0078     if k == 400
0079         c = [1 0 1 1];
0080         c = c ./ sum(c);
0081     end
0082 
0083     if k == 500
0084         c = [1 0 0 1];
0085         c = c ./ sum(c);
0086     end
0087 
0088     if k == 550
0089         c = [1 0 1 1];
0090         c = c ./ sum(c);
0091     end
0092 
0093     if k == 600
0094         c = [1 1 1 1];
0095         c = c ./ sum(c);
0096     end
0097 
0098     if k == 800
0099         c = [0 1 1 1];
0100         c = c ./ sum(c);
0101     end
0102 
0103     if k == 1000
0104         c = [0 1 0 1];
0105         c = c ./ sum(c);
0106     end
0107     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0108     % Store visibility vectors
0109     C = [C;c];
0110 end
0111 
0112 %
0113 % Dynamic and measurement models
0114 %
0115 F = [0 1; 0 0];
0116 L = [0;1];
0117 q = 0.1;
0118 [A,Q] = lti_disc(F,L,q,dt);
0119 H = [1 0];
0120 R = sd^2;
0121 
0122 %
0123 % Prior for all signals
0124 %
0125 M0 = [0;0];
0126 P0 = diag([100 10]);
0127 
0128 %
0129 % Initialize filter
0130 %
0131 N = 10;
0132 S = kf_nmcda_init(N,M0,P0,dt);
0133 
0134 % Storage for particles
0135 SS = cell(size(T,2),size(S,2));
0136 
0137 % Signal tracking
0138 for k=1:size(T,2)
0139 
0140     [S,E1] = kf_nmcda_predict_dp(S,A,Q,[],[],T(k),alpha,beta);
0141     [S,E2] = kf_nmcda_update_dp(S,Y(:,k),T(k),H,R,cp,cd,[]);
0142     fprintf('%d/%d: %s / %s\n',k,size(Y,2),E1{1},E2{1});
0143 
0144     if rem(k,10) == 0
0145         hold off;
0146         plot(T,Y,'r.');
0147         hold on;
0148         cols='gbkymcgbkymcgbkymc';
0149         chars='......oooooo******';
0150         smp = [];
0151         nvis = 100;
0152         for i=1:length(S)
0153             n = round(S{i}.W*nvis);
0154             if n>0
0155                 for j=1:length(S{i}.M)
0156                     x = gauss_rnd(S{i}.M{j},S{i}.P{j},n);
0157                     c_ind = rem(j,length(cols))+1;
0158                     plot(T(k),x(1,:),[cols(c_ind) chars(c_ind)]);
0159                 end
0160             end
0161         end
0162         drawnow;
0163     end
0164 
0165     %
0166     % Store
0167     %
0168     SS(k,:) = S;
0169 
0170     %
0171     % Resample if needed
0172     %
0173     S = normalize_weights(S);
0174     n_eff = eff_particles(S);
0175     if n_eff < N/4
0176         W = get_weights(S);
0177         ind = resampstr(W);
0178         S  = S(ind);
0179         SS = SS(:,ind);
0180         W = ones(1,N)/N;
0181         S = set_weights(S,W);
0182         fprintf('Resampling done on time step %d\n',k);
0183     end
0184 
0185 end
0186 
0187 multisignal_plot;
0188 
0189