Home > src > demos > clutter_demo > clutter_demo.m

clutter_demo

PURPOSE ^

CLUTTER_DEMO MCDA demo using the CWPV-model with clutter measurements.

SYNOPSIS ^

This is a script file.

DESCRIPTION ^

 CLUTTER_DEMO MCDA demo using the CWPV-model with clutter measurements.


 History:
    13.12.2007 JH Initial version

 Copyright (C) 2007 Jouni Hartikainen

 $Id: clutter_demo.m, $

 This software is distributed under the GNU General Public
 Licence (version 2 or later); please refer to the file
 Licence.txt, included with the software, for details.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 % CLUTTER_DEMO MCDA demo using the CWPV-model with clutter measurements.
0002 %
0003 %
0004 % History:
0005 %    13.12.2007 JH Initial version
0006 %
0007 % Copyright (C) 2007 Jouni Hartikainen
0008 %
0009 % $Id: clutter_demo.m, $
0010 %
0011 % This software is distributed under the GNU General Public
0012 % Licence (version 2 or later); please refer to the file
0013 % Licence.txt, included with the software, for details.
0014 
0015 print_figures = 1;
0016 save_figures = 0;
0017 
0018 % Transition matrix for the continous-time system.
0019 F = [0 0 1 0;
0020     0 0 0 1;
0021     0 0 0 0;
0022     0 0 0 0];
0023 m = size(F,2);
0024 
0025 % Noise effect matrix for the continous-time system.
0026 L = [0 0;
0027     0 0;
0028     1 0;
0029     0 1];
0030 
0031 % Stepsize
0032 dt = 0.1;
0033 
0034 % Process noise variance
0035 q = .1;
0036 Qc = diag([q q]);
0037 
0038 % Discretization of the continous-time system.
0039 [A,Q] = lti_disc(F,L,Qc,dt);
0040 
0041 % Measurement model.
0042 H = [1 0 0 0;
0043      0 1 0 0];
0044 
0045 % Variance in the measurements.
0046 r1 = 0.05;
0047 r2 = 0.05;
0048 R = diag([r1 r1]);
0049 
0050 % Number of time steps
0051 n = 240;
0052 % Space for states
0053 X_r = zeros(size(F,1),n);
0054 
0055 % Accelerations in different time steps
0056 a = zeros(1,n);
0057 a(1,10:30)  = pi/42/dt + 0.01*randn(1,21);
0058 a(1,35:55) = -pi/42/dt + 0.01*randn(1,21);
0059 a(1,80:105) = -pi/52/dt + 0.01*randn(1,26);
0060 a(1,140:165) = -pi/52/dt + 0.01*randn(1,26);
0061 a(1,190:210) = -pi/42/dt + 0.01*randn(1,21);
0062 a(1,215:235) = pi/42/dt + 0.01*randn(1,21);
0063 
0064 % Initial state
0065 x = [-4;-0.2;1;0];
0066 
0067 % Generate the trajectory
0068 for i=1:n
0069     F = [0 0  1    0;...
0070         0 0  0    1;...
0071         0 0  0   a(i);...
0072         0 0 -a(i) 0];
0073     x = expm(F*dt)*x;
0074     X_r(1:4,i) = x;
0075 end
0076 
0077 % Space for measurements
0078 Y = zeros(size(H,1),n);
0079 % Generate the measurements.
0080 for i = 1:n
0081     Y(:,i) = H*X_r(:,i) + gauss_rnd(zeros(size(Y,1),1), R);
0082 end
0083 %
0084 
0085 %
0086 % Parameters for RBMCDA
0087 %
0088 N1  = 10;     % number of particles
0089 NT = 1;       % number of targets
0090 TP = 1;       % target prior
0091 CP = 0.5;     % clutter prior
0092 
0093 % Region of the clutter
0094 cx_min = -5;
0095 cx_max = 5;
0096 cy_min = -4;
0097 cy_max = 4;
0098 c_width = cx_max - cx_min;
0099 c_height = cy_max - cy_min;
0100 
0101 % Clutter density
0102 CD = 1/(c_width*c_height);    % clutter is [-5,5]x[-4,4]
0103 
0104 
0105 % True measurement indicators
0106 TC = ones(1,size(X_r,2));
0107 
0108 % Generate clutter measurements
0109 for i=1:size(X_r,2)
0110     if rand < CP
0111         TC(i) = 0;
0112         Y(1,i) = c_width*rand(1,1) + cx_min;
0113         Y(2,i) = c_height*rand(1,1) + cy_min;
0114     end
0115 end
0116 
0117 % Print the trajectory and measurements
0118 if print_figures
0119     set(gcf,'PaperType','a4');
0120     set(gcf,'PaperPosition',[0.25 2.5 3.8 3]);
0121     set(gca,'Position',[0.25 2.5 3.8 3]);
0122 
0123     % Find non-clutter and clutter measurements
0124     I1 = find(TC);
0125     I2 = find(TC == 0);
0126 
0127     clf;
0128     h=plot(X_r(1,:),X_r(2,:),'-',...
0129         Y(1,I1),Y(2,I1),'ko',...
0130         Y(1,I2),Y(2,I2),'kx');
0131     set(h(1),'color',[0.3 0.3 0.3]);
0132     set(h,'markersize',4);
0133     legend('True trajectory',...
0134         'Measurement',...
0135         'Clutter Measurement')
0136     set(h,'markersize',4);
0137     set(h,'linewidth',0.5);
0138     set(gca,'FontSize',4);
0139     xlim([cx_min cx_max]);
0140     ylim([cy_min cy_max]);
0141     if save_figures
0142         print('-dpsc','clutter_trajectory.ps');
0143     end
0144     fprintf('Plotting trajectory and measurements. Press any key to continue.\n');
0145     pause
0146 end
0147 
0148 % Space for prior estimates
0149 MM1 = cell(1,NT);
0150 PP1 = cell(1,NT);
0151 % Prior for target 1
0152 MM1{1} = X_r(:,1);
0153 PP1{1} = diag([0.1 0.1 0.1 0.1]);
0154 
0155 % Generate the particle structures
0156 S = mcda_init(N1,MM1,PP1);
0157 
0158 % Space for RBMCDA estimates
0159 MM = zeros(m,n);
0160 SS = cell(n,N1);
0161 
0162 
0163 % Prior estimate for KF
0164 M0 = X_r(:,1);
0165 P0 = diag([0.1 0.1 0.1 0.1]);
0166 
0167 % Space for KF estimates
0168 MM_kf0 = zeros(m,n);
0169 PP_kf0 = zeros(m,m,n);
0170 
0171 clf;
0172 fprintf('Filtering...\n');
0173 for k=1:size(Y,2)
0174     % Track with KF
0175     [M0,P0] = kf_predict(M0,P0,A,Q);
0176     [M0,P0] = kf_update(M0,P0,Y(:,k),H,R);
0177     MM_kf0(:,k)   = M0;
0178     PP_kf0(:,:,k) = P0;
0179 
0180     % Track with RBMCDA
0181     S = kf_mcda_predict(S,A,Q);
0182     [S,C] = kf_mcda_update(S,Y(:,k),H,R,TP,CP,CD);
0183 
0184     M = calculate_mean(S,1);
0185     MM(:,k) = M;
0186 
0187     %
0188     % Resample RBMCDA if needed
0189     %
0190     n_eff = eff_particles(S);
0191     if n_eff < N1/4
0192         ind = resample(S);
0193         S = S(:,ind);
0194         % Set weights to be uniform
0195         W  = ones(1,N1)/N1;
0196         S = set_weights(S,W);
0197         fprintf('Resampling 1 done on time step %d\n',k);
0198     end
0199     % Save the particle structures after resampling
0200     SS(k,:) = S;
0201 
0202     %
0203     % Animate
0204     %
0205     if print_figures
0206         hold off;
0207         nvis = 100;
0208         MV = zeros(m,N1);
0209         PV = zeros(m,m,N1);
0210         for j=1:N1
0211             MV(:,j) = S{j}.M{1};
0212             PV(:,:,j) = S{j}.P{1};
0213         end
0214         W = get_weights(S);
0215         samp = gmm_rnd(MV,PV,W,nvis);
0216         h = plot(X_r(1,1:k),X_r(2,1:k),'g-.',...
0217                  Y(1,1:k),Y(2,1:k),'rx',...
0218                  samp(1,:),samp(2,:),'b.',...
0219                  MM(1,1:k),MM(2,1:k),'k-');
0220         set(h(3),'markersize',2);
0221 
0222         xlim([cx_min cx_max]);
0223         ylim([cy_min cy_max]);
0224 
0225         drawnow;
0226     end
0227 end
0228 
0229 fprintf('Smoothing...');
0230 [SM,S_EST1] = kf_mcda_smooth(SS,A,Q);
0231 fprintf('Done!\n');
0232 
0233 % Calculate RMSE
0234 rmse_mc1 = sqrt(mean(mean((MM(1:2,:)-X_r(1:2,:)).^2)));
0235 rmse_smc1 = sqrt(mean(mean((S_EST1{1}(1:2,:)-X_r(1:2,:)).^2)));
0236 
0237 % Print RMSE
0238 fprintf('RMSE_mc1: %.3f\n',rmse_mc1);
0239 fprintf('RMSE_smc1: %.3f\n',rmse_smc1);
0240 
0241 
0242 % Plot the filtered particles
0243 if print_figures
0244     clutter_plot1;
0245     if save_figures
0246         print('-dpsc','clutter1.ps');
0247     end
0248     fprintf('Plotting filtering results. Press any key to continue.\n');
0249     pause
0250 
0251     % Plot the smoothed particles
0252     clutter_plot2;
0253     if save_figures
0254         print('-dpsc','clutter2.ps');
0255     end
0256     fprintf('Plotting smoothing results.\n');
0257 end
Generated on Tue 19-Feb-2008 16:59:37 by m2html © 2003