├── DBSCAN ├── DBSCAN.m ├── PlotClusterinResult.m ├── PlotDBSCANResult.m ├── dbscan_main.m ├── desktop.ini ├── generateTargets.m ├── license.txt ├── mydata.mat ├── target_stats.mat └── www.yarpiz.com.url ├── DENSTREAM ├── DENSTREAM.m ├── Merging.m ├── denstream_init.m ├── denstream_main.m ├── findClosestMicroCluster.m ├── generateMicroClusters.m ├── getTargets.m ├── test │ ├── test_RadarSim.m │ └── test_concatenateStructs.m ├── updateClusterPlot.m └── updateMicroCluster.m ├── README.md ├── radar_signal_clustering.gif └── robust_localization_based_on_radar_signal_clustering.pdf /DBSCAN/DBSCAN.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2015, Yarpiz (www.yarpiz.com) 3 | % All rights reserved. Please read the "license.txt" for license terms. 4 | % 5 | % Project Code: YPML110 6 | % Project Title: Implementation of DBSCAN Clustering in MATLAB 7 | % Publisher: Yarpiz (www.yarpiz.com) 8 | % 9 | % Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team) 10 | % 11 | % Contact Info: sm.kalami@gmail.com, info@yarpiz.com 12 | % 13 | 14 | function [IDX, isnoise]=DBSCAN(X,epsilon,MinPts) 15 | 16 | C=0; 17 | 18 | n=size(X,1); 19 | IDX=zeros(n,1); 20 | 21 | D=pdist2(X,X); 22 | 23 | visited=false(n,1); 24 | isnoise=false(n,1); 25 | 26 | for i=1:n 27 | if ~visited(i) 28 | visited(i)=true; 29 | 30 | Neighbors=RegionQuery(i); 31 | if numel(Neighbors)=MinPts 54 | Neighbors=[Neighbors Neighbors2]; %#ok 55 | end 56 | end 57 | if IDX(j)==0 58 | IDX(j)=C; 59 | end 60 | 61 | k = k + 1; 62 | if k > numel(Neighbors) 63 | break; 64 | end 65 | end 66 | end 67 | 68 | function Neighbors=RegionQuery(i) 69 | Neighbors=find(D(i,:)<=epsilon); 70 | end 71 | 72 | end 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /DBSCAN/PlotClusterinResult.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2015, Yarpiz (www.yarpiz.com) 3 | % All rights reserved. Please read the "license.txt" for license terms. 4 | % 5 | % Project Code: YPML110 6 | % Project Title: Implementation of DBSCAN Clustering in MATLAB 7 | % Publisher: Yarpiz (www.yarpiz.com) 8 | % 9 | % Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team) 10 | % 11 | % Contact Info: sm.kalami@gmail.com, info@yarpiz.com 12 | % 13 | 14 | function PlotClusterinResult(X, IDX) 15 | 16 | k=max(IDX); 17 | 18 | Colors=hsv(k); 19 | 20 | Legends = {}; 21 | for i=0:k 22 | Xi=X(IDX==i,:); 23 | if i~=0 24 | Style = 'x'; 25 | MarkerSize = 8; 26 | Color = Colors(i,:); 27 | Legends{end+1} = ['Cluster #' num2str(i)]; 28 | else 29 | Style = 'o'; 30 | MarkerSize = 6; 31 | Color = [0 0 0]; 32 | if ~isempty(Xi) 33 | Legends{end+1} = 'Noise'; 34 | end 35 | end 36 | if ~isempty(Xi) 37 | plot(Xi(:,1),Xi(:,2),Style,'MarkerSize',MarkerSize,'Color',Color); 38 | end 39 | hold on; 40 | end 41 | hold off; 42 | axis equal; 43 | grid on; 44 | legend(Legends); 45 | legend('Location', 'NorthEastOutside'); 46 | 47 | end -------------------------------------------------------------------------------- /DBSCAN/PlotDBSCANResult.m: -------------------------------------------------------------------------------- 1 | function PlotDBSCANResult( ax,X,IDX,colors ) 2 | 3 | k = max(IDX); 4 | Legends = {}; 5 | 6 | for i=0:k 7 | Xi = X(IDX==i,:); 8 | if i~=0 9 | Style = '.'; 10 | MarkerSize = 10; 11 | Color = colors(i,:); 12 | Legends{end+1} = ['Cluster #' num2str(i)]; 13 | else 14 | Style = '.'; 15 | MarkerSize = 10; 16 | Color = [0 0 0]; 17 | if ~isempty(Xi) 18 | Legends{end+1} = 'Noise'; 19 | end 20 | end 21 | if ~isempty(Xi) 22 | plot(ax,Xi(:,1),Xi(:,2),Style,'MarkerSize',MarkerSize,'Color',Color); 23 | end 24 | hold on; 25 | end 26 | % hold off; 27 | grid on; 28 | % legend(Legends); 29 | % legend('Location', 'NorthEastOutside'); 30 | 31 | end -------------------------------------------------------------------------------- /DBSCAN/dbscan_main.m: -------------------------------------------------------------------------------- 1 | % 2 | % Copyright (c) 2015, Yarpiz (www.yarpiz.com) 3 | % All rights reserved. Please read the "license.txt" for license terms. 4 | % 5 | % Project Code: YPML110 6 | % Project Title: Implementation of DBSCAN Clustering in MATLAB 7 | % Publisher: Yarpiz (www.yarpiz.com) 8 | % 9 | % Developer: S. Mostapha Kalami Heris (Member of Yarpiz Team) 10 | % 11 | % Contact Info: sm.kalami@gmail.com, info@yarpiz.com 12 | % 13 | 14 | clc; 15 | clear; 16 | close all; 17 | 18 | %% Load Data 19 | 20 | data=load('mydata'); 21 | X=data.X; 22 | 23 | 24 | %% Run DBSCAN Clustering Algorithm 25 | 26 | epsilon=0.5; 27 | MinPts=10; 28 | IDX=DBSCAN(X,epsilon,MinPts); 29 | 30 | 31 | %% Plot Results 32 | 33 | PlotClusterinResult(X, IDX); 34 | title(['DBSCAN Clustering (\epsilon = ' num2str(epsilon) ', MinPts = ' num2str(MinPts) ')']); 35 | -------------------------------------------------------------------------------- /DBSCAN/desktop.ini: -------------------------------------------------------------------------------- 1 | [.ShellClassInfo] 2 | InfoTip=This folder is shared online. 3 | IconFile=C:\Program Files\Google\Drive\googledrivesync.exe 4 | IconIndex=16 5 | -------------------------------------------------------------------------------- /DBSCAN/generateTargets.m: -------------------------------------------------------------------------------- 1 | function [ P ] = generateTargets( Ntargets,max,min ) 2 | %UNTITLED3 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % init P 6 | P = zeros(Ntargets,4); 7 | 8 | for i=1:Ntargets 9 | xy_cov = rand(); 10 | MU = [(max(1) - min(1))*rand() + min(1); 11 | (max(2) - min(2))*rand() + min(2); 12 | (max(3) - min(3))*rand() + min(3)]; 13 | 14 | SIGMA = [abs(max(1) - min(1)*rand() + min(1)), xy_cov, 0; 15 | xy_cov, (max(2) - min(2))*rand() + min(2), 0; 16 | 0, 0, (max(3) - min(3))*rand() + min(3)]; 17 | 18 | % simulate data from multivariate random distribution 19 | P(i,1:3) = mvnrnd(MU,SIGMA); 20 | 21 | % P(i).x = data(1); 22 | % P(i).y = data(2); 23 | % P(i).peakVal = data(3); 24 | 25 | end 26 | 27 | % add time data to points - same for all points in a single scan 28 | P(:,4) = rem(now,1); 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /DBSCAN/license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Yarpiz (www.yarpiz.com) 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in 13 | the documentation and/or other materials provided with the distribution 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /DBSCAN/mydata.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cstahoviak/radar-signal-clustering/d86634a3ea4c83f79b650fc0a929b70b45b25606/DBSCAN/mydata.mat -------------------------------------------------------------------------------- /DBSCAN/target_stats.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cstahoviak/radar-signal-clustering/d86634a3ea4c83f79b650fc0a929b70b45b25606/DBSCAN/target_stats.mat -------------------------------------------------------------------------------- /DBSCAN/www.yarpiz.com.url: -------------------------------------------------------------------------------- 1 | [{000214A0-0000-0000-C000-000000000046}] 2 | Prop3=19,2 3 | [InternetShortcut] 4 | IDList= 5 | URL=http://www.yarpiz.com/ 6 | HotKey=0 7 | -------------------------------------------------------------------------------- /DENSTREAM/DENSTREAM.m: -------------------------------------------------------------------------------- 1 | function [ p_mc,o_mc ] = DENSTREAM( P,p_mc,o_mc,lambda,eps,beta,mu,Tp,distType,t_now,colors ) 2 | %UNTITLED3 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | %%% INPUTS: 6 | % p_mc potential-micro-clusters 7 | % o_mc outlier-micro-clusters 8 | 9 | % lamda fading factor 10 | % eps radius of epsilon-neighborhood 11 | % minPts minimum number of points in a cluster, DBSCAN parameter 12 | % beta parameter to distinguish between relative weights of p- and o-micro-clusters 13 | % mu core micro-cluster weught, integer value 14 | % Tp checking period for cluster weights, i.e. "pruning" period 15 | % t_now current time 16 | 17 | %%% OUTPUTS: 18 | % p_mc potential-micro-clusters 19 | % o_mc outlier-micro-clusters 20 | 21 | %% BEGIN FUNCTION 22 | 23 | Ntargets = size(P,1); 24 | 25 | % What if no iniitial clusters are created?? 26 | % Run DBSCAN on next scan of targets to try to create initial set of 27 | % p-micro-clusters? (Implemented this solution) Other alternatives? 28 | 29 | % loop through all points in current radar scan 30 | for i=1:Ntargets 31 | % not sure if I should used t_now passed to the DENSTREAM function 32 | % call, or create a new t_now at each iteration of this loop 33 | t_now = toc; 34 | 35 | % merge current target into cluster map 36 | [ p_mc,o_mc ] = Merging( P(i,:),p_mc,o_mc,lambda,eps,beta,mu, ... 37 | distType,t_now,colors ); 38 | end 39 | % disp('FINISHED MERGING ALL TARGETS') 40 | 41 | Np_mc = length(p_mc); 42 | No_mc = length(o_mc); 43 | 44 | % if current time is multiple of "pruning" interval 45 | % disp(mod(round(t_now),Tp)) 46 | if ~mod(round(t_now),Tp) 47 | disp('DENSTREAM: pruning interval reached') 48 | 49 | % p-micro-cluster "pruning" 50 | if ~isempty(p_mc) 51 | i = 1; 52 | count = 1; 53 | while count <= Np_mc 54 | if p_mc(i).weight < beta*mu 55 | % delete o-micro-cluster 56 | fprintf('DENSTREAM: p-micro-cluster %d deleted\n',count) 57 | p_mc(i) = []; 58 | else 59 | i = i+1; 60 | end 61 | count = count+1; 62 | end 63 | end 64 | 65 | % o-micro-cluster "pruning" 66 | if ~isempty(o_mc) 67 | i = 1; 68 | count = 1; 69 | while count <= No_mc 70 | xi = (2^(-lambda*(t_now - o_mc(i).t0 + Tp)) - 1) / ... 71 | (2^(-lambda*Tp) - 1); 72 | if o_mc(i).weight < xi 73 | % delete o-micro-cluster 74 | fprintf('DENSTREAM: o-micro-cluster %d deleted\n',count) 75 | o_mc(i) = []; 76 | else 77 | i = i+1; 78 | end 79 | count = count+1; 80 | end 81 | end 82 | 83 | end % end if mod(t_now,Tp) 84 | 85 | end 86 | 87 | -------------------------------------------------------------------------------- /DENSTREAM/Merging.m: -------------------------------------------------------------------------------- 1 | function [ p_mc,o_mc ] = Merging( P,p_mc,o_mc,lambda,eps,beta,mu,distType,t_now,colors ) 2 | %UNTITLED4 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | %%% INPUTS: 6 | % p_mc set of potential-micro-clusters 7 | % o_mc set of outlier-micro-clusters 8 | 9 | % lamda fading factor 10 | % eps radius of epsilon-neighborhood 11 | % minPts minimum number of points in a cluster, DBSCAN parameter 12 | % beta parameter to distinguish between relative weights of p- and o-micro-clusters 13 | % mu core micro-cluster weught, integer value 14 | % Tp checking period for cluster weights, i.e. "pruning" period 15 | % t_now current time 16 | 17 | %%% OUTPUTS: 18 | % p_mc set of potential-micro-clusters 19 | % o_mc set of outlier-micro-clusters 20 | 21 | %% BEGIN FUNCTION 22 | 23 | % try to merge P(i) into the nearest p-micro-cluster 24 | % NOTE: assumes that at least one p-mc is created after the initial result 25 | % of DBSCAN clsutering, requires {beta,mu} to be tuned 26 | index = findClosestMicroCluster( P,p_mc,distType ); 27 | 28 | % calculate new radius of p_mc if P is merged into it 29 | p_mc_temp = updateMicroCluster( P,p_mc(index),lambda,t_now,'p' ); 30 | 31 | if p_mc_temp.radius <= eps 32 | % merge P into nearest p-micro-cluster, p_mc(index) 33 | p_mc(index) = p_mc_temp; 34 | elseif isempty(o_mc) 35 | % create new o-micro-cluster at P because none currently exist 36 | disp('MERGING: no o-micro-clusters exist, create new') 37 | o_mc = updateMicroCluster( P,[],lambda,t_now,'o' ); 38 | else 39 | % disp('P_MC RADIUS > EPS') 40 | % try to merge P into nearest o-micro-cluster 41 | index = findClosestMicroCluster( P,o_mc,distType ); 42 | 43 | % calculate new radius of o_mc if P is merged into it 44 | o_mc_temp = updateMicroCluster( P,o_mc(index),lambda,t_now,'o' ); 45 | % fprintf('MERGING: new o-mc radius = %f\t eps = %f\n', ... 46 | % o_mc_temp.radius,eps); 47 | 48 | if o_mc_temp.radius <= eps 49 | % merge P into nearest o-micro-cluster, o_mc(index) 50 | o_mc(index) = o_mc_temp; 51 | 52 | if o_mc(index).weight >= beta*mu 53 | % create a new p-mc at c_o 54 | % NOTE: assumes at least one p-micro-cluster currently exists 55 | disp('MERGING: create new p-micro-cluster') 56 | p_mc_temp = rmfield(o_mc(index),'t0'); 57 | p_mc_temp.color = colors(length(p_mc)+1,:); % assign next color to new p-mc 58 | p_mc = [p_mc; p_mc_temp]; 59 | 60 | % remove o_mc(index) from o-mc buffer 61 | o_mc(index) = []; 62 | end 63 | 64 | else 65 | disp('MERGING: add new o-micro-cluster to outlier-buffer') 66 | % create new o-micro-cluster at P and insert it into the o-mc buffer 67 | o_mc_new = updateMicroCluster( P,[],lambda,t_now,'o' ); 68 | 69 | % in some cases where o_mc is instantiated as a row vector instead 70 | % of a column vector, this concatenation fails... problem NOT resolved 71 | o_mc = [o_mc; o_mc_new]; 72 | end 73 | 74 | 75 | end 76 | 77 | 78 | end 79 | 80 | -------------------------------------------------------------------------------- /DENSTREAM/denstream_init.m: -------------------------------------------------------------------------------- 1 | function [ P,idx ] = denstream_init( ax,Ntargets_max,MU,SIGMA,eps,minPts,MIN,MAX,waitTime,colors,RADAR_SIM,AGP,chirp_params ) 2 | %UNTITLED2 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % init array of targets 6 | P = []; 7 | 8 | idx = zeros(Ntargets_max,1); 9 | while sum(idx) == 0 10 | % simulate radar targets {P}_i 11 | [ targets ] = getTargets( Ntargets_max,MU,SIGMA,RADAR_SIM,AGP,chirp_params ); 12 | P = [P; targets]; 13 | 14 | % plot initial points before DBSCAN clustering 15 | scatter(ax,P(:,2),P(:,3),15,'kx'); grid on; 16 | % xlim([MIN(1)-5 MAX(1)+5]); ylim([MIN(2)-5 MAX(2)+5]); 17 | xlim([MIN(1) MAX(1)]); ylim([MIN(2) MAX(2)]); 18 | pause(waitTime); 19 | 20 | % Apply DBSCAN to the first set of points {P} to initilize the online process 21 | idx = DBSCAN( P(:,2:3),eps,minPts ); 22 | end 23 | % hold on; 24 | 25 | % plot DBSCAN results - does this assign the same colors to the 26 | % p-micro-clusters that I assign to them in generateMicroClusters()?? 27 | PlotDBSCANResult(ax,P(:,2:3),idx,colors); 28 | grid on; hold on; 29 | title(['DBSCAN Clustering (\epsilon = ' num2str(eps) ', minPts = ' num2str(minPts) ')']); 30 | % xlim([MIN(1)-5 MAX(1)+5]); ylim([MIN(2)-5 MAX(2)+5]); 31 | xlim([MIN(1) MAX(1)]); ylim([MIN(2) MAX(2)]); 32 | pause(waitTime); 33 | 34 | end 35 | 36 | -------------------------------------------------------------------------------- /DENSTREAM/denstream_main.m: -------------------------------------------------------------------------------- 1 | %% Header 2 | 3 | % Filename: denstream_main.m 4 | % Author: Carl Stahoviak 5 | % Date Created: 07/11/2018 6 | % Lasted Edited: 07/17/2018 7 | 8 | % **Description here** 9 | 10 | % ToDo: 11 | % 1. Change how targets are simulated. Place Some number of targets in the 12 | % scene with their own mean and std dev. Then pull randomly from these 13 | % distributions for successive scans to me merged with map. 14 | 15 | clc; 16 | clear; 17 | close ALL; 18 | % this is a new line that I just added 19 | 20 | %% DENSTREAM PARAMETERS 21 | 22 | lambda = 2; % fading factor 23 | eps = 0.25; % radius of epsilon-neighborhood 24 | minPts = 2; % minimum number of points in a cluster, DBSCAN parameter 25 | beta = 0.001; % parameter to distinguish between relative weights of p- and o-micro-clusters 26 | mu = 3; % core micro-cluster weught, integer value 27 | Tp = 3; % checking period for cluster weights, i.e. "pruning" period 28 | 29 | % Equation 4.1, DENSTREAM paper 30 | % Tp = (-1/lambda)*log2(1 - 1/(beta*mu)); 31 | 32 | distType = 'euclid'; 33 | 34 | %% RADAR SIMULATOR PARAMETERS 35 | 36 | % enable Radar Simulator 37 | RADAR_SIM = 0; 38 | 39 | % define chirp parameters 40 | [ chirp_params ] = func_defineChirpParameters( 'best_range_res' ); 41 | 42 | % load antenna gain pattern 43 | [ antenna_gain_pattern ] = func_antenna_gain_pattern; 44 | AGP = antenna_gain_pattern; 45 | 46 | %% DENSTREAM PARAMETERS 47 | % lambda = 2; % fading factor 48 | % eps = 0.05; % radius of epsilon-neighborhood 49 | % minPts = 2; % minimum number of points in a cluster, DBSCAN parameter 50 | % beta = 0.001; % parameter to distinguish between relative weights of p- and o-micro-clusters 51 | % mu = 3; % core micro-cluster weught, integer value 52 | % Tp = 3; % checking period for cluster weights, i.e. "pruning" period 53 | 54 | 55 | %% ADDITIONAL PARAMETERS 56 | 57 | if RADAR_SIM 58 | % [x y z velocity peakVal] min/max values 59 | MAX = [ 5 6 0 0 1]; 60 | MIN = [-5 -1 0 0 100]; 61 | else 62 | % [x y z velocity peakVal] min/max values 63 | MAX = [ 10 5 0 0 1]; 64 | MIN = [-10 0 0 0 100]; 65 | end 66 | 67 | % assign unique color to each p-micro-cluster 68 | colors = hsv(64); % hsv(N) assumes a max of N p-micro-clusters 69 | 70 | % define wait time after new data has been populated to plot 71 | waitTime = 2; 72 | 73 | % create plotting figure 74 | figure(1); 75 | ax = gca; 76 | 77 | %% INITIALIZATION 78 | 79 | % start system stopwatch 80 | tic; 81 | 82 | % maximum number of targets per scan 83 | Ntargets_max = 64; 84 | 85 | % generate target statistics 86 | % load('target_stats.mat') 87 | MU = zeros(Ntargets_max,5); 88 | for i=1:Ntargets_max 89 | % MU = [x y z velocity peakVal] 90 | MU(i,:) = [(MAX(1) - MIN(1))*rand() + MIN(1), ... 91 | (MAX(2) - MIN(2))*rand() + MIN(2), ... 92 | (MAX(3) - MIN(3))*rand() + MIN(3), ... 93 | (MAX(4) - MIN(4))*rand() + MIN(4), ... 94 | (MAX(5) - MIN(5))*rand() + MIN(5)]; 95 | end 96 | 97 | % need to get a better idea of what these uncertainties actually are 98 | SIGMA = [0.5 0 0 0 0; 99 | 0 0.5 0 0 0; 100 | 0 0 0.5 0 0; 101 | 0 0 0 2 0; 102 | 0 0 0 0 2]; 103 | 104 | % get initial set of points (targets), with at least one cluster as 105 | % identified by DBSCAN using the {eps,minPts} parameters 106 | [ P,idx ] = denstream_init( ax,Ntargets_max,MU,SIGMA,eps,minPts, ... 107 | MIN,MAX,waitTime,colors,RADAR_SIM,AGP,chirp_params ); 108 | 109 | % generate initial set of p-micro-clusters 110 | % NOTE: no o-micro-clusters generated from initial scan 111 | t_now = toc; 112 | [ p_mc,o_mc ] = generateMicroClusters( P,idx,t_now,lambda,beta,mu,colors ) 113 | 114 | % update plot of micro-clusters 115 | updateClusterPlot( ax,MIN,MAX,p_mc,o_mc ); 116 | pause(waitTime); 117 | 118 | % return; 119 | 120 | %% MAIN LOOP 121 | 122 | % total number of radar scans to be merged with map via DENSTREAM 123 | Nscans = 10; 124 | 125 | count = 1; 126 | while(count <= Nscans) 127 | fprintf('p-mc: %d\t o-mc: %d\n',length(p_mc),length(o_mc)); 128 | 129 | % simulate radar targets {P}_i 130 | [ P ] = getTargets( Ntargets_max,MU,SIGMA,RADAR_SIM,AGP,chirp_params ); 131 | 132 | % plot points in new scan with black 'x' 133 | scatter(ax,P(:,2),P(:,3),15,'kx'); hold off; 134 | % xlim([MIN(1)-5 MAX(1)+5]); ylim([MIN(2)-5 MAX(2)+5]); 135 | xlim([MIN(1) MAX(1)]); ylim([MIN(2) MAX(2)]); 136 | pause(waitTime); 137 | 138 | % pass {P}_i to DENSTREAM() 139 | [ p_mc,o_mc ] = DENSTREAM( P,p_mc,o_mc,lambda,eps,beta,mu,Tp, ... 140 | distType,t_now,colors ); 141 | 142 | % update plot of micro-clusters 143 | updateClusterPlot( ax,MIN,MAX,p_mc,o_mc ); 144 | pause(waitTime); 145 | 146 | count = count+1; 147 | end 148 | -------------------------------------------------------------------------------- /DENSTREAM/findClosestMicroCluster.m: -------------------------------------------------------------------------------- 1 | function [ index ] = findClosestMicroCluster( P,mc,type ) 2 | %UNTITLED Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % ToDo: 6 | % 1. Figure out how to create an nx1 array from all the values of the same 7 | % field in an array of structs 8 | 9 | %% BEGIN FUNCTION 10 | 11 | Nmc = length(mc); 12 | dist = zeros(Nmc,1); 13 | 14 | for i=1:Nmc 15 | if strcmp(type,'mahal') 16 | % find closest micro-cluster based on Mahalanobis distance 17 | 18 | Y = [P(2:3), P(6)]; % observation P(i) 19 | X = [mc(i).center, mc(i).peakVal]; % reference 20 | 21 | dist(i) = pdist2(X,Y,'mahalanobis'); 22 | 23 | elseif strcmp(type,'euclid') 24 | % find closest micro-cluster based on Euclidean distance 25 | 26 | Y = P(2:3); % observation P(i) 27 | X = mc(i).center; % reference 28 | 29 | dist(i) = pdist2(X,Y,'euclidean'); 30 | 31 | elseif strcmp(type,'hamming') 32 | % find closest micro-cluster based on Hamming distance 33 | 34 | else 35 | % return ERROR 36 | 37 | end 38 | 39 | end 40 | 41 | % get index of minimum distance value 42 | [~,index] = min(dist); 43 | 44 | end 45 | 46 | -------------------------------------------------------------------------------- /DENSTREAM/generateMicroClusters.m: -------------------------------------------------------------------------------- 1 | function [ p_mc,o_mc ] = generateMicroClusters( P,idx,t_now,lambda,beta,mu,colors ) 2 | %UNTITLED2 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % NOTE: 6 | % 1. DBSCAN outliers are assigned an idx value of zero and will not be 7 | % assigned to either p- or o-micro-clusters 8 | % 2. For the initial scan, only p-micro-clusters should be generated, with 9 | % weight equal to sum(2^0) = N, the number of points in the cluster. 10 | 11 | % UNFINISHED: 12 | % 1. radius calculation from Definition 3.4 is yielding imaginary 13 | % numbers... 14 | % 2. Use repmat() to init array of micro-cluster structs 15 | 16 | %% BEGIN FUNCTION 17 | 18 | % init micro-cluster struct - how to init array of empty structs? 19 | % ANS: Use repmat - https://stackoverflow.com/questions/13664090/how-to-initialize-an-array-of-structs-in-matlab 20 | p_mc = struct([]); 21 | o_mc = struct([]); 22 | 23 | % loop through each cluster identified by DBSCAN 24 | for i=1:max(idx) 25 | cluster_idx = (idx == i); 26 | 27 | % create new micro-cluster 28 | points = P(cluster_idx,:); 29 | mc = updateMicroCluster( points,[],lambda,t_now,'' ); 30 | 31 | if mc.weight >= beta*mu 32 | % add unique color to p-micro-cluster 33 | mc.color = colors(i,:); 34 | 35 | % assign micro-cluster to list of p-micro-clusters 36 | if isempty(p_mc) 37 | % create new array of p-micro-clusters 38 | p_mc = mc; 39 | else 40 | % append current micro-cluster to array of p-micro-clusters 41 | p_mc = [p_mc; mc]; 42 | end 43 | else 44 | % add initializtion time to o-micro-cluster 45 | mc.to = t_now; 46 | 47 | % assign micro-cluster to list of o-micro-clusters 48 | if isempty(o_mc) 49 | % create new array of o-micro-clusters 50 | o_mc = mc; 51 | else 52 | % append current micro-cluster to array of p-micro-clusters 53 | o_mc = [o_mc; mc]; 54 | end 55 | end 56 | 57 | end 58 | 59 | 60 | end 61 | 62 | -------------------------------------------------------------------------------- /DENSTREAM/getTargets.m: -------------------------------------------------------------------------------- 1 | function [ P ] = getTargets( Ntargets_max,MU,SIGMA,RADAR_SIM,AGP,chirp_params ) 2 | %UNTITLED3 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % get current time 6 | t_now = toc; 7 | 8 | % simulate radar targets {P}_i 9 | allTargets = mvnrnd(MU,SIGMA); 10 | 11 | if RADAR_SIM 12 | % NOTE (8/16): Not working yet. Still need to work with Christopher 13 | % on getting targets from his radar simulater (in progress) 14 | 15 | Ntargets_RDRSIM = 5; 16 | 17 | RCS = 50*ones(Ntargets_RDRSIM,1); % radar cross section [m^2] 18 | points = allTargets(1:Ntargets_RDRSIM,1:2); % 2D target locations [m] 19 | velocity = zeros(Ntargets_RDRSIM,1); % target velocity [m/s] 20 | AoA = zeros(Ntargets_RDRSIM,1); % target direction of travel [deg] 21 | % 0 deg - moving left 22 | % 90 deg - moving away from sensor 23 | % 180 deg - moving right 24 | % 270 deg - moving towards sensor 25 | 26 | target_attribute = [RCS, points, velocity, AoA]; 27 | 28 | [ targets ] = func_fmcw_radar_simulator_2018_0815( target_attribute, ... 29 | AGP,chirp_params); 30 | P = [ones(64,1)*t_now, targets(:,2:5), targets(:,1)]; 31 | 32 | else 33 | % random number of targets between 0 and Ntargets_max 34 | Ntargets = randi(Ntargets_max); 35 | 36 | % sample Ntargets from array of Ntargets_max targets 37 | P = [ones(Ntargets,1)*t_now, ... 38 | datasample(allTargets,Ntargets,'Replace',false)]; 39 | end 40 | 41 | 42 | 43 | end 44 | 45 | -------------------------------------------------------------------------------- /DENSTREAM/test/test_RadarSim.m: -------------------------------------------------------------------------------- 1 | %% Test Radar Simulator 2 | 3 | clc; 4 | clear; 5 | close ALL; 6 | 7 | Ntargets_RDRSIM = 5; 8 | 9 | RCS = 50*ones(Ntargets_RDRSIM,1); 10 | points = [-10 10; 0 0; 10 10; 0 20; 5 50]; 11 | velocity = zeros(Ntargets_RDRSIM,1); 12 | AoA = zeros(Ntargets_RDRSIM,1); 13 | 14 | target_attribute = [RCS, points, velocity, AoA]; 15 | [ targets ] = func_fmcw_radar_simulator_2018_0721( target_attribute ); 16 | 17 | fprintf('MAX SNR = %f\n',max(targets(:,1))); 18 | fprintf('MIN SNR = %f\n',min(targets(:,1))); 19 | 20 | figure(1) 21 | scatter(points(:,1),points(:,2),20,'kx'); hold on; 22 | scatter(targets(:,2),targets(:,3),10,'filled') -------------------------------------------------------------------------------- /DENSTREAM/test/test_concatenateStructs.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; 3 | close ALL; 4 | 5 | tic; 6 | 7 | for i=1:10 8 | struct_array(i).points = i*ones(1,5); 9 | struct_array(i).t = toc; 10 | end 11 | 12 | struct2.points = zeros(1,5); 13 | struct2.t = toc; 14 | 15 | struct_array(end+1) = struct2 16 | 17 | struct_array2 = [struct_array, struct2] 18 | -------------------------------------------------------------------------------- /DENSTREAM/updateClusterPlot.m: -------------------------------------------------------------------------------- 1 | function updateClusterPlot( ax,MIN,MAX,p_mc,o_mc ) 2 | 3 | Legends = {}; 4 | 5 | % plot all p-micro-clusters 6 | for i=1:length(p_mc) 7 | Style = '.'; 8 | MarkerSize = 10; 9 | Color = p_mc(i).color; 10 | Legends{end+1} = ['Cluster #' num2str(i)]; 11 | 12 | X = p_mc(i).points(:,2); 13 | Y = p_mc(i).points(:,3); 14 | 15 | plot(ax,X,Y,Style,'MarkerSize',MarkerSize,'Color',Color); 16 | % xlim([MIN(1)-5 MAX(1)+5]); ylim([MIN(2)-5 MAX(2)+5]); 17 | xlim([MIN(1) MAX(1)]); ylim([MIN(2) MAX(2)]); 18 | hold on; 19 | 20 | % plot radius of p-micro-cluster 21 | viscircles(ax,p_mc(i).center,p_mc(i).radius,... 22 | 'Color',Color,'LineWidth',1); 23 | hold on; 24 | end 25 | 26 | for i=1:length(o_mc) 27 | Style = '.'; 28 | MarkerSize = 10; 29 | Color = 'k'; 30 | Legends{end+1} = ['O-Cluster #' num2str(i)]; 31 | 32 | X = o_mc(i).points(:,2); 33 | Y = o_mc(i).points(:,3); 34 | 35 | plot(ax,X,Y,Style,'MarkerSize',MarkerSize,'Color',Color); 36 | % xlim([MIN(1)-5 MAX(1)+5]); ylim([MIN(2)-5 MAX(2)+5]); 37 | xlim([MIN(1) MAX(1)]); ylim([MIN(2) MAX(2)]); 38 | hold on; 39 | 40 | end 41 | hold on; 42 | grid on; 43 | % legend(Legends); 44 | % legend('Location', 'NorthEastOutside'); 45 | 46 | end -------------------------------------------------------------------------------- /DENSTREAM/updateMicroCluster.m: -------------------------------------------------------------------------------- 1 | function [ mc ] = updateMicroCluster( P,mc,lambda,t_now,mc_type ) 2 | %UNTITLED2 Summary of this function goes here 3 | % Detailed explanation goes here 4 | 5 | % define fading function 6 | fading = @(t) 2^(-lambda*(t_now - t)); 7 | 8 | PRINT_RADIUS = 0; 9 | 10 | %% BEGIN FUNCTION 11 | 12 | % add point to micro-cluster 13 | if isempty(mc) 14 | % add point to NEW micro-cluster 15 | % ERROR HERE: 'A dot name structure assignment is illegal when the 16 | % structure is empty. Use a subscript on the structure.' 17 | % Not yet resolved... only happens in infrequent circumstances. 18 | % mc(1).points = P; % possible error resolution 19 | % disp('GOT HERE') 20 | mc.points = P; 21 | 22 | % add 't0' field to NEW o-micro-cluster 23 | if strcmp(mc_type,'o') 24 | mc.t0 = t_now; 25 | end 26 | else 27 | % add point to existing micro-cluster 28 | mc.points = [mc.points; P]; 29 | end 30 | 31 | % init micro-cluster incremental parameters 32 | mc.weight = 0; 33 | mc.CF1 = zeros(1,2); 34 | mc.CF2 = zeros(1,2); 35 | 36 | % calculate p-micro-cluster parameters, {w,CF_1,CF_2} 37 | for j=1:size(mc.points,1) 38 | mc.weight = mc.weight + fading(mc.points(j,1)); 39 | mc.CF1 = mc.CF1 + fading(mc.points(j,1))*mc.points(j,2:3); 40 | mc.CF2 = mc.CF2 + fading(mc.points(j,1))*mc.points(j,2:3).^2; 41 | end 42 | 43 | % calculate p-micro-cluster parameters, {center,radius} = f(w,CF1,CF2) 44 | mc.center = mc.CF1 / mc.weight; 45 | mc.radius = sqrt(norm(mc.CF2) / mc.weight - ... 46 | (norm(mc.CF1) / mc.weight)^2); % not working... 47 | if PRINT_RADIUS 48 | fprintf('\nmc.radius =\n'); 49 | disp(mc.radius) 50 | end 51 | 52 | % radius measurement with switched terms 53 | mc.radius = sqrt((norm(mc.CF1) / mc.weight)^2 - ... 54 | norm(mc.CF2) / mc.weight); 55 | if PRINT_RADIUS 56 | disp(mc.radius) 57 | end 58 | 59 | % unweighted radius measurement 60 | dist = pdist2(mc.center,mc.points(:,2:3),'euclidean'); 61 | mc.radius = max(dist); 62 | if PRINT_RADIUS 63 | disp(mc.radius) 64 | end 65 | 66 | end 67 | 68 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # radar-signal-clustering 2 | Implementation of the DENSTREAM algorithm for clustering radar targets based on [Robust Localization based on Radar Signal Clustering](https://ieeexplore.ieee.org/document/7535485), Schuster, et al 2016. 3 | 4 | 5 | 6 | 7 | ## Status 8 | 1. Framework of algorithm currently implemented for simulated targets only. Have not used radar data from the TI mmWave 1642 or 1843 boards yet. Clustering parameters will need to tuned for real radar data. 9 | -------------------------------------------------------------------------------- /radar_signal_clustering.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cstahoviak/radar-signal-clustering/d86634a3ea4c83f79b650fc0a929b70b45b25606/radar_signal_clustering.gif -------------------------------------------------------------------------------- /robust_localization_based_on_radar_signal_clustering.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cstahoviak/radar-signal-clustering/d86634a3ea4c83f79b650fc0a929b70b45b25606/robust_localization_based_on_radar_signal_clustering.pdf --------------------------------------------------------------------------------