multisignal_demo

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

 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 %
0011 % Generate data
0012 %
0013 dt = 0.01;
0014 sd = 0.2;
0015 T = (1:1500)*dt;
0016 X = cell(1,3);
0017 % Real signals
0018 X{1} = sin(T);
0019 X{2} = 4 + 0.5*sin(0.9*T);
0020 X{3} = -2 + cos(0.5*T);
0021 X{4} = 3-0.3*T;
0022 
0023 % Probabilities of
0024 c = [1 0 0 1]/2;
0025 
0026 Y = zeros(1,size(T,2));
0027 NT = [];
0028 C  = [];
0029 for k=1:size(T,2)
0030     NT = [NT sum(c~=0)];
0031 
0032     j = categ_rnd(c);
0033     if rand<0.01
0034         % Clutter
0035         Y(:,k) = 10*rand-5;
0036     else
0037         % Gaussian noise
0038         Y(:,k) = X{j}(:,k)+sd*randn(size(Y,1),1);
0039     end
0040 
0041     if k == 100
0042         c = [1 1 0 1];
0043         c = c ./ sum(c);
0044     end
0045 
0046     if k == 200
0047         c = [1 1 1 1];
0048         c = c ./ sum(c);
0049     end
0050 
0051     if k == 400
0052         c = [1 0 1 1];
0053         c = c ./ sum(c);
0054     end
0055 
0056     if k == 500
0057         c = [1 0 0 1];
0058         c = c ./ sum(c);
0059     end
0060 
0061     if k == 550
0062         c = [1 0 1 1];
0063         c = c ./ sum(c);
0064     end
0065 
0066     if k == 600
0067         c = [1 1 1 1];
0068         c = c ./ sum(c);
0069     end
0070 
0071     if k == 800
0072         c = [0 1 1 1];
0073         c = c ./ sum(c);
0074     end
0075 
0076     if k == 1000
0077         c = [0 1 0 1];
0078         c = c ./ sum(c);
0079     end
0080 
0081     C = [C;c];
0082 end
0083 
0084 %
0085 % Dynamic and measurement models
0086 %
0087 F = [0 1; 0 0];
0088 L = [0;1];
0089 q = 0.1;
0090 [A,Q] = lti_disc(F,L,q,dt);
0091 H = [1 0];
0092 R = sd^2;
0093 
0094 %
0095 % Prior for all signals
0096 %
0097 M0 = [0;0];
0098 P0 = diag([100 10]);
0099 
0100 %
0101 % Initialize particle structures
0102 %
0103 N = 10;  
0104 S = kf_nmcda_init(N,M0,P0,dt);
0105 
0106 %
0107 % Prior probabilities of clutter
0108 % and of births and deaths
0109 %
0110 cd = 1/10;      % Density of clutter
0111 cp = 0.01;      % Clutter prior
0112 
0113 alpha = 2;      % Parameters of gamma distribution
0114 beta = 1;       % modeling the deaths of signals
0115 
0116 %alpha = 2;      % Parameters of gamma distribution
0117 %beta = 0.1;       % modeling the deaths of signals
0118 
0119 %
0120 % Signal tracking
0121 %
0122 SS = cell(size(T,2),size(S,2));
0123 for k=1:size(Y,2)
0124     % Filter with RBMCDA
0125     S = kf_nmcda_predict(S,A,Q);
0126     S = kf_nmcda_update(S,Y(:,k),T(k),H,R,cp,cd,[],alpha,beta);
0127     fprintf('%d/%d: %s / %s\n',k,size(Y,2),E1{1},E2{1});
0128     
0129     % Resample if needed
0130     n_eff = eff_particles(S);
0131     if n_eff < N/4
0132         ind = resample(S);
0133         S  = S(ind);
0134         SS = SS(:,ind);
0135         for i = 1:N
0136             S{i}.W = 1/N;
0137         end
0138         fprintf('Resampling done on time step %d\n',k);
0139     end
0140     
0141     % Plot estimates every 10th sampling period
0142     if rem(k,10) == 0
0143         hold off;
0144         plot(T,Y,'r.');
0145         hold on;
0146         cols='gbkymcgbkymc';
0147         chars='......oooooo';               
0148         
0149         smp = [];
0150         nvis = 100;
0151         for i=1:length(S)
0152             n = round(S{i}.W*nvis);
0153             if n>0
0154                 for j=1:length(S{i}.M)
0155                     x = gauss_rnd(S{i}.M{j},S{i}.P{j},n);
0156                     c_ind = rem(j,length(cols))+1;
0157                     plot(T(k),x(1,:),[cols(c_ind) chars(c_ind)]);
0158                 end
0159             end
0160         end
0161         drawnow;
0162     end
0163 
0164     % Store particles
0165     SS(k,:) = S;
0166 end
0167 
0168 % Plot the signals
0169 multisignal_plot;
0170 
0171