├── Lab_Report_SensorFusion.pdf ├── MatLAB_scripts ├── ExtendedKF.m ├── KalmanFilter.m ├── gauss_mix.m ├── particle_filter.m └── phd.m └── README.md /Lab_Report_SensorFusion.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/omair-khalid/Target-Tracking-Algorithms---Detailed-Comparison-and-Implementation/b5bdac5be709adc960e8aa2b918f8ba177fec1ab/Lab_Report_SensorFusion.pdf -------------------------------------------------------------------------------- /MatLAB_scripts/ExtendedKF.m: -------------------------------------------------------------------------------- 1 | % Lab 2 - Extended Kalman Filter 2 | %Yu Liu, Wajhat Akhtar, Omair Khalid 3 | % Date - 03/10/2017 4 | clear all 5 | close all 6 | 7 | numP = 300; %number of iterations 8 | %Initialize x0 9 | %x0 = [10;1;10;0]; 10 | x0 = [-200;80;-200;20] 11 | 12 | %Process noise Standard Deviation 13 | sigma_q = 1.0; 14 | %observation noise Standard Deviation 15 | sigma_r = 5.0; 16 | 17 | deltaT = 0.5; %time step 18 | 19 | %Covariance of Prior 20 | P = 1000 * eye(4,4); 21 | 22 | %State Transition Matrix (Markov kernel) 23 | f = [1, deltaT;0 1]; 24 | F = [f zeros(2,2);zeros(2,2) f]; 25 | 26 | %Covariance Process Noise 27 | q = [deltaT^4/4 deltaT^3/2;deltaT^3/2 deltaT^2]; 28 | Q = sigma_q^2 * [q ,zeros(2,2);zeros(2,2) q]; %process noise matrix 29 | 30 | 31 | %Transform - State to Measurement Space 32 | %H = [1,0,0,0;0,0,1,0]; 33 | 34 | %Covariance of Observation noise 35 | %R = sigma_r^2 * [1,0;0,1]; 36 | R = diag([50^2 0.005^2]); 37 | 38 | 39 | X = []; 40 | Z = []; 41 | 42 | %Simulator 43 | for i = 1:numP 44 | 45 | x = mvnrnd(F*x0,Q)'; %predicted state 46 | X = [X,x]; 47 | range = sqrt(x0(1)^2+x0(3)^2); 48 | bearing = atan2(x0(3),x0(1)); 49 | y = [range;bearing]; 50 | 51 | z = mvnrnd(y,R)'; %measurement 52 | Z = [Z,z]; 53 | x0 = x; 54 | end 55 | 56 | 57 | figure(1), hold on; 58 | plot(X(1,:),X(3,:),'bx'); 59 | hold on 60 | plot(Z(1,:).*cos(Z(2,:)),Z(1,:).*sin(Z(2,:)),'g+'); 61 | 62 | hold on 63 | %% 64 | %Kalman Filter 65 | pause(4); 66 | for j = 1:numP 67 | %Kalman statr prediction 68 | range = sqrt(x0(1)^2+x0(3)^2); 69 | bearing = atan2(x0(3),x0(1)); 70 | y0 = [range; bearing]; 71 | J = [cos(bearing) 0 sin(bearing) 0; 72 | -sin(bearing)/range 0 cos(bearing)/range 0]; 73 | m = F*x0; 74 | P = F*P*F' + Q; 75 | K = P*J' * inv(J*P*J' + R); 76 | Updated_mean = m + K * (Z(:,j) - y0); 77 | Updated_P = (eye(4,4) - K*J)*P; 78 | x0 = Updated_mean; 79 | P = Updated_P; 80 | plot(x0(1),x0(3),'r+'); 81 | hold on 82 | 83 | pause(0.1); 84 | 85 | title( ['Extended Kalman Filter Tracking - Time Step = ' num2str( j )] ) 86 | legend('Simulator','Measurement', 'Kalman Update','Location','southeast') 87 | 88 | end 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /MatLAB_scripts/KalmanFilter.m: -------------------------------------------------------------------------------- 1 | % Lab 1 : Particle Filter (MULTI SESNSOR FUSION AND TRACKING ) 2 | % Author : Wajahat Akhtar, Yu Liu and Omair Khalid 3 | 4 | %Lab 1 - Multi Sensor Fusion and Tracking 5 | %Date - 02/10/2017 6 | clear all 7 | close all 8 | %------------------------- 9 | %Initialization 10 | pause(3); 11 | sigmaQ = 1.2;%std of the Process Noise 12 | sigmaP = 1.2;%std of the Prior at iteration zero 13 | sigmaR = 1.2;%std of the Observation Noise 14 | 15 | deltaT = 1; 16 | 17 | %State Transition Matrix 18 | f = [1 deltaT;0 1]; 19 | F = [f zeros(2,2); zeros(2,2) f]; 20 | 21 | %Process Noise Covariance matrix 22 | q = [(deltaT^4)/4 (deltaT^3)/2;(deltaT^3)/2 (deltaT^2)]; 23 | Q = sigmaQ^2.*[q zeros(2,2); zeros(2,2) q]; 24 | 25 | %Covariance of the Prior at iteration zero 26 | P = 1000*eye(4,4); 27 | 28 | %Mapping of the state on to the measurement space 29 | H = [1 0 0 0; 0 0 1 0]; 30 | 31 | %Observation Noise Covariance matrix 32 | R = (sigmaR^2).*[1 0;0 1]; 33 | 34 | %------------------------- 35 | %Simulator 36 | 37 | x0 = [0;0;0;0]; 38 | numP = 50; 39 | 40 | for i = 1:numP 41 | 42 | x = mvnrnd(F*x0,Q); 43 | X(:,i) = x'; 44 | z = mvnrnd(H*x',R); 45 | Z(:,i) = z'; 46 | x0 = x'; 47 | end 48 | 49 | plot(X(1,:),X(3,:),'bx'); 50 | hold on; 51 | plot(Z(1,:),Z(2,:),'g+'); 52 | hold on; 53 | %----------------------- 54 | pause(3); 55 | X_new = []; 56 | x0 = [10 20 0.23 49]'; 57 | for j=1:numP 58 | %Construction of Kalman Update 59 | m = F*x0; 60 | P = F*P*F' + Q; 61 | 62 | K = (P*H')*inv(H*P*H' +R); 63 | P_ = (eye(4,4) - K*H)*P; 64 | m_ = m + K*(Z(:,j) - H*m); 65 | 66 | %Update 67 | 68 | 69 | x0 = m_; 70 | P = P_; 71 | %str = sprintf('Kalman Filter Tracking %j',variable); 72 | X_new = [X_new x0]; 73 | 74 | plot(x0(1,:),x0(3,:),'r*'); 75 | 76 | %title(str); 77 | legend('Simulator','Measurement', 'Kalman Update') 78 | title( ['Kalman Filter Tracking - Time Step = ' num2str( j )] ) 79 | 80 | hold on; 81 | 82 | pause(0.25) 83 | 84 | 85 | end 86 | 87 | 88 | -------------------------------------------------------------------------------- /MatLAB_scripts/gauss_mix.m: -------------------------------------------------------------------------------- 1 | % Lab 4 : Particle Filter (MULTI SESNSOR FUSION AND TRACKING ) 2 | % Author : Wajahat Akhtar, Yu Liu and Omair Khalid 3 | 4 | close all; 5 | clear all; 6 | timestep = 30; 7 | x0 = [200, 10, 200, 10]'; % set range for state vector ([x, x_speed, y, y_speed]) 8 | x0_init = x0 .* rand(4, 1); % target's initial state 9 | 10 | sigma_Q = 2; 11 | sigma_R = 2; 12 | 13 | delta_t = 1; 14 | % motion model (markov kernel) 15 | F = [1, delta_t, 0, 0; 16 | 0, 1, 0, 0; 17 | 0, 0, 1, delta_t; 18 | 0, 0, 0, 1]; 19 | 20 | q1 = [(delta_t^4) / 4, (delta_t^3) / 2; (delta_t^3) / 2, delta_t^2]; 21 | q2 = [0, 0; 0, 0]; 22 | % motion uncertainty 23 | Q = sigma_Q^2 * [q1, q2; q2, q1]; 24 | 25 | % measurement uncertainty 26 | R = sigma_R^2 * [1, 0; 0, 1]; 27 | 28 | % map state to measurement 29 | H = [1, 0, 0, 0; 0, 0, 1, 0]; 30 | I = eye(4); 31 | num_gauss = 15; % initial number of gaussian mixure 32 | gauss = cell(num_gauss, 1); 33 | x0_g = [500, 10, 500, 10]'; 34 | for i = 1:num_gauss 35 | gauss{i}.weight = 1 / num_gauss; 36 | gauss{i}.mean = x0_g .* rand(4, 1); 37 | gauss{i}.cov = diag( [10, 10, 10, 10]); 38 | end 39 | 40 | x_gt = cell(timestep, 1); % ground truth 41 | z = cell(timestep, 1); % measurement/observation 42 | figure, hold on, 43 | xlim([-100, 500]); 44 | ylim([-100, 500]); 45 | legend('groundtruth', 'measurement', 'tracker'); 46 | % writerObj = VideoWriter('gaussia_sum4.avi'); 47 | % writerObj.FrameRate = 2; 48 | % open(writerObj); 49 | 50 | % Target trajectory simulator 51 | for k = 1:timestep 52 | x_gt{k} = mvnrnd(F * x0_init, Q)'; % motion model 53 | x0_init = x_gt{k}; 54 | z{k} = mvnrnd(H * x_gt{k}, R)'; % measurement model 55 | plot(x_gt{k}(1), x_gt{k}(3), 'bx', 'LineWidth',2); pause(0.1) 56 | plot(z{k}(1), z{k}(2), 'g+', 'LineWidth',2); pause(0.1) 57 | 58 | wk_sum = 0; 59 | % Kalman prediction and update 60 | for j = 1:size(gauss, 1) 61 | gauss{j}.mean_pred = F* gauss{j}.mean; 62 | gauss{j}.cov_pred = F*gauss{j}.cov*F' + Q; 63 | gauss{j}.kalman_g = gauss{j}.cov_pred * H' * (H*gauss{j}.cov_pred*H' + R)^-1; 64 | gauss{j}.mean_update = gauss{j}.mean_pred + gauss{j}.kalman_g * (z{k}-H * gauss{j}.mean_pred); 65 | gauss{j}.cov_update = (I - gauss{j}.kalman_g * H) * gauss{j}.cov_pred; 66 | 67 | gauss{j}.weight = gauss{i}.weight * mvnpdf(z{k}, H*gauss{j}.mean_update, R + H*gauss{j}.cov_update*H'); 68 | wk_sum = wk_sum + gauss{j}.weight; 69 | end 70 | 71 | x = []; 72 | y = []; 73 | % Update Gaussian weight by likelihood and normalize 74 | for j = 1:size(gauss, 1) 75 | gauss{j}.weight = gauss{j}.weight / wk_sum; 76 | gauss{j}.weight 77 | gauss{j}.mean = gauss{j}.mean_update; 78 | gauss{j}.cov = gauss{j}.cov_update; 79 | x = [x, gauss{j}.mean(1)]; 80 | y = [y, gauss{j}.mean(3)]; 81 | end 82 | 83 | h = plot(x, y, 'ro'); pause(0.5); 84 | legend('groundtruth', 'measurement', 'tracker'); 85 | 86 | M(k) = getframe; 87 | 88 | set(h,'Visible','off'); 89 | end 90 | 91 | -------------------------------------------------------------------------------- /MatLAB_scripts/particle_filter.m: -------------------------------------------------------------------------------- 1 | % Lab 4 : Particle Filter (MULTI SESNSOR FUSION AND TRACKING ) 2 | % Author : Wajahat Akhtar, Alpha Liu and Omair Khalid 3 | 4 | clc 5 | clear all ; 6 | close all ; 7 | 8 | %% Initialization Particle filter 9 | delta_t = 1 ; % Frequency = frames per sec ( freqeucy ) 10 | Sig_measurement = 2 ; % Noise variance value for observation noise 11 | Sig_simulator = 2 ; % Noise vairance value for measurement noise 12 | 13 | sigma_observation = (Sig_measurement)^2 ; % Q matrice observation noise 14 | sigma_simulator = (Sig_simulator)^2 ; % R matrice measurement noise 15 | 16 | % Creating Ground truth constant velocity model 17 | w = [] ; 18 | % Covariance matrix 19 | P = [1000 0 0 0 ; 0 1000 0 0 ; 0 0 1000 0 ; 0 0 0 100 ] ; 20 | % Identity 21 | I = eye(4); 22 | % State Transition Model 23 | F = [ [ 1 delta_t ; 0 1] , [0 0 ; 0 0 ] ; [0 0 ; 0 0 ] , [ 1 delta_t ; 0 1] ]; 24 | % Process noise Variance 25 | Q = (sigma_simulator)^2 * [ [ ((delta_t)^4 /4) ((delta_t)^3 /2) ; ((delta_t)^3 /2) (delta_t)^2 ] , [0 0 ; 0 0 ] ; [0 0 ; 0 0 ] , [ ((delta_t)^4 /4) ((delta_t)^3 /2) ; ((delta_t)^3 /2) (delta_t)^2 ] ]; 26 | % R matrice 27 | R = (sigma_observation)^2 * eye(2); 28 | % H matrice 29 | H = [1 0 0 0 ; 0 0 1 0 ] ; 30 | 31 | 32 | %% Generating particles i to N for a single target 33 | num_targets = 1 ; 34 | P = cell(num_targets,1); 35 | N = 120 ; % number of particles 36 | x_size = 200 ; % intial x position of object 37 | y_size = 200 ; % intial y position of object 38 | x_velocity_std = 10 ; % intializing velocity x of object 39 | y_velocity_std = 10 ; % intializing velocity y of object 40 | 41 | particles = [ 200 + 600 ; x_velocity_std ; 200 + 600 ;y_velocity_std ].*rand(4,N) ; 42 | state_g = [ x_size ; x_velocity_std ; y_size;y_velocity_std ].*rand(4,num_targets) ; 43 | 44 | ground_truth = cell(num_targets,1) ; 45 | ground_truth_vect = [] ; 46 | measurement = cell(num_targets,1) ; 47 | measurement_vect = [] ; 48 | 49 | w_n = []; 50 | w_sum = 0 ; 51 | time_step = 25 ; 52 | 53 | %% Plotting particles 54 | % figure; 55 | % hold on 56 | % plot(particles(1,:),particles(3,:),'+r'); 57 | 58 | %% SIMULATOR 59 | for k = 1 : time_step 60 | 61 | for i = 1 : num_targets 62 | state_g(:,i) = mvnrnd( F * state_g(:,i) , Q )';% Observation 63 | ground_truth{i} = state_g(:,i) ; 64 | 65 | z_temp = mvnrnd( H*state_g(:,i) , R );% measurement 66 | measurement{i} = z_temp' ; 67 | 68 | end 69 | ground_truth_vect = [ground_truth_vect , ground_truth ]; 70 | measurement_vect = [ measurement_vect , measurement ] ; 71 | 72 | end 73 | 74 | 75 | simulator_state_gt = [] ; 76 | simulator_measurement_gt = [] ; 77 | 78 | %% PLOTTING SIMULATOR VALUES 79 | 80 | figure ; 81 | hold on 82 | % Writing Video Script for figures Uncomment if needed 83 | % writerObj = VideoWriter('particle_filter.avi'); 84 | % writerObj.FrameRate = 2; 85 | % open(writerObj); 86 | 87 | for i = 1 : num_targets 88 | 89 | simulator_measurement_gt_tmp_x = []; 90 | simulator_measurement_gt_tmp_y =[]; 91 | simulator_state_gt_tmp_x = []; 92 | simulator_state_gt_tmp_y = []; 93 | 94 | for j = 1: time_step 95 | simulator_measurement_gt_tmp_x(j,1) = measurement_vect{i,j}(1,1) ; 96 | simulator_measurement_gt_tmp_y(j,1) = measurement_vect{i,j}(2,1) ; 97 | simulator_state_gt_tmp_x(j,1) = ground_truth_vect{i,j}(1,1) ; 98 | simulator_state_gt_tmp_y(j,1) = ground_truth_vect{i,j}(3,1) ; 99 | 100 | 101 | plot(simulator_state_gt_tmp_x(j,1),simulator_state_gt_tmp_y(j,1),'-o'); 102 | plot(simulator_measurement_gt_tmp_x(j,1) , simulator_measurement_gt_tmp_y(j,1),'-x' ) 103 | % legend( 'Ground truth',' Measurements'); 104 | % title(['Simlator for Single object Tracking with Time Step k = ' num2str(j) ]) ; 105 | % frame = getframe(gcf); 106 | % writeVideo(writerObj, frame); 107 | pause(0.5) 108 | 109 | end 110 | end 111 | 112 | % Closing Video 113 | % close(writerObj); 114 | 115 | state_vect = cell(1,time_step) ; 116 | sensor_measure_vect = cell(1,time_step) ; 117 | state_tmp = [] ; 118 | measure_tmp = [] ; 119 | 120 | %% GENRATING PARTICLES 121 | 122 | % For i to N Evaluating the importance weight 123 | for i = 1 : N 124 | w(i) = 1 / N ; 125 | end 126 | 127 | % initializing weights 128 | weight_sum = 0 ; 129 | w_covar = 0 ; 130 | w_mean = 0 ; 131 | 132 | 133 | for k = 1 : time_step 134 | 135 | %% Prediction 136 | particles = mvnrnd( (F * particles)' , Q )'; % Measurement 137 | plot(particles(1,:),particles(3,:),'g+') ; 138 | 139 | %% Estimation 140 | for i = 1 : N 141 | w_n(i) = mvnpdf( measurement_vect{1,k}(:,1), H * particles(:,i), R ) ; 142 | w_sum = w_sum + w_n(i) ; 143 | end 144 | 145 | %% Normalize the weight 146 | for i = 1 : N 147 | w(i) = w_n(i) / w_sum ; 148 | end 149 | w_sum = 0 ; 150 | 151 | %% RESAMPLING 152 | % Mean 153 | for i = 1 : N 154 | w_mean = w_mean + ( w(i)* particles(:,i) ) ; 155 | end 156 | 157 | % Covariance 158 | for i = 1 : N 159 | w_covar = w_covar + w(i) * (w_mean - particles(:,i) ) * (w_mean - particles(:,i) )' ; 160 | end 161 | 162 | % Update 163 | for i = 1 : N 164 | updated_particles(:,i) = mvnrnd(w_mean ,w_covar ) ; 165 | end 166 | 167 | particles = updated_particles ; 168 | plot(particles(1,:),particles(3,:),'+r'); 169 | legend(' Ground truth',' Measurements ','Particles','Weighted Particles') ; 170 | w_covar = 0 ; 171 | w_mean = 0 ; 172 | 173 | end 174 | 175 | disp('End '); 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /MatLAB_scripts/phd.m: -------------------------------------------------------------------------------- 1 | % Lab 4 : Particle Filter (MULTI SESNSOR FUSION AND TRACKING ) 2 | % Author : Wajahat Akhtar, Yu Liu and Omair Khalid 3 | 4 | close all; 5 | clear all; 6 | 7 | %% param definition 8 | num_target =10; 9 | timestep = 60; 10 | x0 = [200, 10, 200, 10]'; % set range for state vector ([x, x_speed, y, y_speed]) 11 | x0_fa = [800, 800]'; % set false alarm x and y range 12 | x_false_alarm_all = []; 13 | 14 | x0_all = []; % initialize all targets' state randomly 15 | for i = 1:num_target 16 | x0_all = [x0_all, x0 .* rand(4, 1)]; 17 | end 18 | 19 | sigma_Q = 0.5; 20 | sigma_R = 2; 21 | 22 | delta_t = 1; 23 | % motion model (markov kernel) 24 | F = [1, delta_t, 0, 0; 25 | 0, 1, 0, 0; 26 | 0, 0, 1, delta_t; 27 | 0, 0, 0, 1]; 28 | 29 | q1 = [(delta_t^4) / 4, (delta_t^3) / 2; (delta_t^3) / 2, delta_t^2]; 30 | q2 = [0, 0; 0, 0]; 31 | % motion uncertainty 32 | Q = sigma_Q^2 * [q1, q2; q2, q1]; 33 | 34 | % measurement uncertainty 35 | R = sigma_R^2 * [1, 0; 0, 1]; 36 | 37 | % map state to measurement 38 | H = [1, 0, 0, 0; 0, 0, 1, 0]; 39 | I = diag([1, 1, 1, 1]); 40 | 41 | % area of the surveillance region 42 | vol = x0(1) * x0(3); 43 | 44 | % adjustable params for non-ideal tracking 45 | lambda =1; % false alarm level 46 | Pd = 0.98; % detection rate (% of detecting an object) 47 | Ps = 0.99; % survival rate (% of an object to survive in this timestep) 48 | 49 | merge_thresh = 4; % U 50 | max_gauss_num = num_target*1; % Jmax 51 | 52 | % initializes gaussians mixure 53 | num_gauss = num_target*2; % initial number of gaussian mixure 54 | gauss = cell(num_gauss, 1); 55 | for i = 1:num_gauss 56 | gauss{i}.weight = 0.5; 57 | gauss{i}.mean = x0 .* rand(4, 1); 58 | gauss{i}.cov = diag( [10, 10, 10, 10]); 59 | end 60 | 61 | % define new-birth gaussian mixures 62 | num_inject_gauss = ceil(num_target*3); 63 | inject_gauss = cell(num_inject_gauss, 1); 64 | for i = 1:num_inject_gauss 65 | inject_gauss{i}.weight = 0.5; 66 | inject_gauss{i}.cov = diag( [10, 10, 10, 10]); 67 | end 68 | 69 | lose_track = []; % for checking which objects lose tracking as time goes by 70 | %% trajectory simulator + PHD 71 | x_gt = cell(timestep, 1); % ground truth 72 | z = cell(timestep, 1); % measurement/observation 73 | figure, hold on, 74 | xlim([-100, 700]); 75 | ylim([-100, 700]); 76 | 77 | % writerObj = VideoWriter('phd6.avi'); 78 | % writerObj.FrameRate = 2; 79 | % open(writerObj); 80 | % main control loop! 81 | for k = 1:timestep 82 | % data simulation 83 | num_fase_alarm = poissrnd(lambda); % every timestep, differnt number of false alarm 84 | x_gt_tem = []; 85 | z_tem = []; 86 | 87 | for j = 1: num_target % each target object follows its own trajectory (groudtruth) and are observed (measurement) accordingly 88 | if rand < Ps % target survives in this timestep 89 | x_gt_tem = [x_gt_tem, mvnrnd(F * x0_all(:, j), Q)']; % motion model 90 | else 91 | % target dies (equvalent to setting its positions ridiculously high, which is outside the surveillance region) 92 | x_gt_tem = [x_gt_tem, [-1000; 0; -1000; 0]]; 93 | lose_track = [lose_track, j]; 94 | lose_track = unique(lose_track); 95 | 96 | % max number of gaussian (tracker) is reduced due to permanent 97 | % loss of target 98 | max_gauss_num = num_target -size(lose_track, 2); 99 | end 100 | 101 | % measurement of this target is obtained 102 | if rand < Pd 103 | z_tem = [z_tem, mvnrnd(H * x_gt_tem(:,j), R)']; % meas model; 104 | end 105 | end 106 | 107 | % add false alarm into the measurement cell 108 | x_gt{k} = x_gt_tem; 109 | z{k} = z_tem; 110 | x0_all = x_gt{k}; 111 | 112 | for p = 1:num_fase_alarm 113 | x_false_alarm_all = [x_false_alarm_all, x0_fa .* rand(2, 1)]; % randomly positioned false alarm % fa = false alarm range 114 | end 115 | z{k} = [z{k}, x_false_alarm_all]; % each measurement can track actual object and/or false alarm 116 | 117 | plot (x_gt{k}(1,:), x_gt{k}(3,:), 'bx', 'LineWidth',1); pause(0.05) 118 | 119 | % the "if" statement is added becasue sometime at a timestep, there 120 | % exists no actual measurement nor false alarm (empty cell leads to error) 121 | if ~isempty(z{k}) 122 | h2 = plot (z{k}(1,:), z{k}(2,:), 'g+', 'LineWidth',1); pause(0.05) % need to watch out when no measurement is obtained 123 | end 124 | % set(h2,'Visible','off'); 125 | 126 | x_false_alarm_all = []; 127 | 128 | % PHD prediction for each already-existing gaussian component 129 | for j = 1:num_gauss 130 | gauss{j}.mean = F* gauss{j}.mean; % motion model 131 | gauss{j}.cov = F*gauss{j}.cov*F' + Q; % measurement model cov 132 | gauss{j}.weight = Ps * gauss{j}.weight; 133 | end 134 | 135 | % inject new-birth gaussians 136 | for inj = 1:num_inject_gauss 137 | inject_gauss{inj}.mean = x0 .* rand(4, 1); 138 | end 139 | % as time goes by, increase the range where new-birth gaussians can be spawned as the 140 | % objects move around 141 | x0 = [x0(1)*1.05; x0(2); x0(3)*1.05; x0(4)]; 142 | 143 | num_gauss = num_gauss + num_inject_gauss; 144 | gauss = [gauss; inject_gauss]; % concatenate existing and birth gaussians 145 | 146 | % construct PHD update components 147 | for j = 1: num_gauss 148 | gauss{j}.Hm = H*gauss{j}.mean; 149 | gauss{j}.sd = R + H*gauss{j}.cov*H'; 150 | gauss{j}.kalmen = gauss{j}.cov * H' * (gauss{j}.sd)^-1; 151 | gauss{j}.weight_record = gauss{j}.weight; 152 | gauss{j}.update_cov = (I - gauss{j}.kalmen * H) * gauss{j}.cov; % store variable for later use 153 | gauss{j}.cov_record = gauss{j}.cov; 154 | gauss{j}.mean_record = gauss{j}.mean; 155 | end 156 | 157 | % PHD update (in the case where no target has been detected) 158 | for j = 1: num_gauss 159 | gauss{j}.weight = (1-Pd) * gauss{j}.weight_record; 160 | gauss{j}.cov = gauss{j}.cov_record; 161 | gauss{j}.mean = gauss{j}.mean_record; 162 | end 163 | 164 | % more gaussian generated per measurement 165 | for jz = 1:size(z{k}, 2) 166 | weight_sum = 0; 167 | for j = 1: num_gauss 168 | % new gaussians 169 | gauss{jz*num_gauss + j}.weight = gauss{j}.weight_record * Pd * mvnpdf(z{k}(:,jz), gauss{j}.Hm, gauss{j}.sd); % dont forget * wk-1 170 | weight_sum = weight_sum + gauss{jz*num_gauss + j}.weight; 171 | gauss{jz*num_gauss + j}.mean = gauss{j}.mean_record + gauss{j}.kalmen * (z{k}(:,jz) - gauss{j}.Hm); 172 | gauss{jz*num_gauss + j}.cov = gauss{j}.update_cov; 173 | end 174 | for j = 1:num_gauss 175 | gauss{jz*num_gauss + j}.weight = gauss{jz*num_gauss + j}.weight / (lambda / vol + weight_sum); 176 | end 177 | end 178 | num_gauss = size(gauss, 1); 179 | 180 | % pruning to remove low-weight gaussians and merge gaussians that are 181 | % close together 182 | 183 | % delete gaussians with low weight 184 | low_weight_list = []; 185 | for j = 1:num_gauss 186 | gauss{j}.weight 187 | if gauss{j}.weight <= 10^-4 || isnan(gauss{j}.weight) % remove gaussian weight below 10^-4 188 | low_weight_list = [low_weight_list; j]; 189 | gauss{j} = []; 190 | end 191 | end 192 | gauss = gauss(~cellfun('isempty',gauss)); 193 | num_gauss = size(gauss, 1); 194 | 195 | % merge gaussians close to each other 196 | gauss_merge = {}; 197 | while num_gauss ~= 0 198 | gauss_weights = []; 199 | for j = 1:num_gauss 200 | gauss_weights = [gauss_weights; gauss{j}.weight]; 201 | end 202 | [~, max_ind] = max(gauss_weights); 203 | important_mean = gauss{max_ind}.mean; 204 | merge_list = []; 205 | 206 | merge_weight_total = 0; 207 | % find close gaussians 208 | for j = 1:num_gauss 209 | mean_dist = (gauss{j}.mean -important_mean)' * (gauss{j}.cov)^-1 * (gauss{j}.mean -important_mean); 210 | if mean_dist <= merge_thresh 211 | merge_list = [merge_list; j]; 212 | merge_weight_total = merge_weight_total + gauss{j}.weight; 213 | end 214 | end 215 | 216 | % calculate merged weight, mean and cov 217 | merge_mean_sum = 0; 218 | for j = 1:size(merge_list, 1) 219 | merge_mean_sum = merge_mean_sum + gauss{merge_list(j)}.mean * gauss{merge_list(j)}.weight; 220 | end 221 | merge_mean = merge_mean_sum / merge_weight_total; 222 | 223 | merge_cov_sum = 0; 224 | for j = 1:size(merge_list, 1) 225 | mean_diff = merge_mean - gauss{merge_list(j)}.mean; 226 | merge_cov_sum = merge_cov_sum + gauss{merge_list(j)}.weight * (gauss{merge_list(j)}.cov + mean_diff * mean_diff'); 227 | gauss{merge_list(j)} = []; 228 | end 229 | merge_cov = merge_cov_sum / merge_weight_total; 230 | 231 | merged_gauss = struct('weight',merge_weight_total, 'mean', merge_mean, 'cov', merge_cov); 232 | gauss_merge = [gauss_merge; merged_gauss]; 233 | gauss = gauss(~cellfun('isempty',gauss)); 234 | % disp('number of gauss left'); 235 | num_gauss = num_gauss - size(merge_list, 1); 236 | end 237 | 238 | gauss = gauss_merge; 239 | 240 | disp('after pruning and merging:') 241 | num_gauss = size(gauss, 1) 242 | % final pruning, by only keeping the largest-weight N number of 243 | % gaussians 244 | gauss_weights = []; 245 | final_gauss = {}; 246 | if num_gauss > max_gauss_num 247 | for j = 1:num_gauss 248 | gauss_weights = [gauss_weights; gauss{j}.weight]; 249 | end 250 | [~, max_ind] = sort(gauss_weights, 'descend'); 251 | for j = 1: max_gauss_num 252 | final_gauss = [final_gauss; gauss{max_ind(j)}]; 253 | end 254 | end 255 | gauss = final_gauss; 256 | disp('after choosing the highest weight gaussian:') 257 | num_gauss = size(gauss, 1) 258 | 259 | timer = 10; 260 | x = []; % to store all remaining gaussians' x component (display purpose) 261 | y = []; % to store all remaining gaussians' y component (display purpose) 262 | 263 | for j = 1:num_gauss 264 | x = [x; gauss{j}.mean(1)]; 265 | y = [y; gauss{j}.mean(3)]; 266 | 267 | % timer = timer -1; 268 | % if timer == 0 269 | % set(h3,'Visible','off'); 270 | % timer = 10; 271 | % end 272 | end 273 | h3 = plot (x, y, 'ro'); pause(0.2) 274 | legend('groundtruth', 'observation', 'tracker'); 275 | % frame = getframe(gcf); 276 | % writeVideo(writerObj, frame); 277 | set(h3,'Visible','off'); 278 | end 279 | 280 | % close(writerObj); 281 | 282 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Target-Tracking-Algorithms---Detailed-Comparison-and-Implementation 2 | 3 | 4 | Explored and implemented in detail the solutions of (single/multiple) target-tracking problems under the Bayesian framework, and demonstrated the workings of Kalman filters, EKF, Gaussian Filter, PHD Filter, and Particle Filter through simulations. 5 | 6 | Project Members: Omair Khalid | Yu Liu | Wajahat Akhtar 7 | --------------------------------------------------------------------------------