├── .gitattributes ├── README.md ├── global_config.m ├── simulation_main.m ├── soundwave_generation ├── generate_mixed_sw.m ├── generate_receive_sw.m └── generate_transmit_sw.m ├── super_estimation ├── music_1D.m ├── super_dist.m ├── super_dist_aoa.m └── super_dist_vel.m └── traditional_estimation ├── trad_dist_FFT.m ├── trad_dist_aoa_FFT.m └── trad_dist_vel_FFT.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # AcousticSensingTutorial 2 | 3 | This is a simulation tutorial for acoustic sensing using chirp signals. 4 | 5 | The traditional_estimation folder implements the traditional approach (FFT) to estimate range, aoa, velocity. 6 | 7 | The super_estimation folder implements the super-resolution approach (MUSIC) to esimate range, aoa, velocity. 8 | -------------------------------------------------------------------------------- /global_config.m: -------------------------------------------------------------------------------- 1 | %% System Configuration 2 | total_num_of_chirps = 100; % Total number of chirps needed to be processed 3 | display_flag = 1; % Flag for displaying the result 4 | display_flag_gt = 1; % Flag for displaying the ground truth 5 | 6 | paras.system_config.total_num_of_chirps = total_num_of_chirps; 7 | paras.system_config.display_flag = display_flag; 8 | paras.system_config.display_flag_gt = display_flag_gt; 9 | %% FMCW Configuration 10 | Vs = 343; % Sound speed 11 | Fs = 44100; % Sampling frequency 12 | Fc = 18000; % Start frequency 13 | B = 4000; % Bandwidth 14 | T = 0.04; % Sweep time 15 | single_chirp_len = Fs*T; % Length of a single chirp 16 | 17 | paras.fmcw_config.Vs = Vs; 18 | paras.fmcw_config.Fs = Fs; 19 | paras.fmcw_config.Fc = Fc; 20 | paras.fmcw_config.B = B; 21 | paras.fmcw_config.T = T; 22 | paras.fmcw_config.single_chirp_len = single_chirp_len; 23 | paras.fmcw_config.total_time = 4; % For simulation 24 | %% Hardware Configuration 25 | mic_int = 0.01070; % Microphone spacing 26 | num_of_mics = 4; 27 | mic_int_vec = (0:(num_of_mics-1))*mic_int; % Uniform array 28 | 29 | paras.hardware_config.mic_int = mic_int; 30 | paras.hardware_config.num_of_mics = num_of_mics; 31 | paras.hardware_config.mic_int_vec = mic_int_vec; 32 | %% Algorithm Configuration 33 | %% ------ common ------ 34 | subsampling_factor = 40; % set to 1 to avoid ,subsampling 35 | subsampling_indices = 1:subsampling_factor:single_chirp_len; 36 | subsamp_single_chirp_len = ceil(single_chirp_len/subsampling_factor); 37 | paras.algo_config.subsamp_factor = subsampling_factor; 38 | paras.algo_config.subsamp_indices = subsampling_indices; 39 | paras.algo_config.subsamp_single_chirp_len = subsamp_single_chirp_len ; 40 | % ----- number of parameters ------ 41 | num_of_paras = 4; 42 | paras.algo_config.num_of_paras = num_of_paras; 43 | % ------ aoa ------ 44 | aoa_search_scope_min = 60; 45 | aoa_search_scope_max = 140; 46 | aoa_search_scope = [aoa_search_scope_min aoa_search_scope_max 2 20]; % min; max; search stepsize; display stepsize 47 | aoa_search = aoa_search_scope(1):aoa_search_scope(3):aoa_search_scope(2); 48 | paras.algo_config.all_aoa_search_scope = aoa_search_scope; 49 | paras.algo_config.all_aoa_search = aoa_search; 50 | paras.algo_config.aoa_search_scope = aoa_search_scope; 51 | paras.algo_config.aoa_search = aoa_search; 52 | % ------ distance ------ 53 | dist_search_scope_min = 0; 54 | dist_search_scope_max = 1; 55 | dist_search_scope = [dist_search_scope_min dist_search_scope_max 0.01 0.2]; % min; max; search stepsize; display stepsize 56 | dist_search = dist_search_scope(1):dist_search_scope(3):dist_search_scope(2); 57 | paras.algo_config.all_dist_search_scope = dist_search_scope; 58 | paras.algo_config.all_dist_search = dist_search; 59 | paras.algo_config.dist_search_scope = dist_search_scope; 60 | paras.algo_config.dist_search = dist_search; 61 | % ------ velocity ------ 62 | num_of_chirps = 4; % 2 63 | vel_search_scope_min = -0.60; 64 | vel_search_scope_max = 0.60; 65 | vel_search_scope = [vel_search_scope_min vel_search_scope_max 0.05 0.02]; 66 | vel_search = vel_search_scope(1):vel_search_scope(3):vel_search_scope(2); 67 | vel_ambiguity_interval = Vs/(2*(Fc+Fc+B)/2*T); 68 | paras.algo_config.num_of_chirps = num_of_chirps; 69 | paras.algo_config.all_vel_search = vel_search; 70 | paras.algo_config.all_vel_search_scope = vel_search_scope; 71 | paras.algo_config.vel_search = vel_search; 72 | paras.algo_config.vel_search_scope = vel_search_scope; 73 | paras.algo_config.vel_ambiguity_interval = vel_ambiguity_interval; 74 | %% ------ traditional estimation (FFT) ------ 75 | paras.algo_config.trad.dist_fft_size = 50*single_chirp_len; 76 | paras.algo_config.trad.vel_fft_size = 50*num_of_chirps; 77 | paras.algo_config.trad.aoa_fft_size = 100*num_of_mics; 78 | %% ------ super-resolution estimation (MUSIC) ------ 79 | SUB_N = floor((single_chirp_len-subsampling_factor)/subsampling_factor)+1; % number of samples after subsampling in each chirp 80 | paras.algo_config.super.smoothed_window_dist = SUB_N-10; % set to SUB_N to avoid data smoothing 81 | paras.algo_config.super.subsampling_factor_dist = subsampling_factor; 82 | paras.algo_config.super.smoothed_window_vel = num_of_chirps; 83 | paras.algo_config.super.smoothed_window_aoa = 4; -------------------------------------------------------------------------------- /simulation_main.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; 3 | close all; 4 | %% Add path 5 | addpath('soundwave_generation'); 6 | addpath('traditional_estimation'); 7 | addpath('super_estimation'); 8 | %% Global Configuration 9 | global_config; 10 | %% Parameters for the multipath 11 | % ------ One Target ------ 12 | % aoas = 100; 13 | % dists = 0.44; 14 | % amps = 0.0008/dists(1)^2; 15 | % vels = 0; 16 | 17 | % ------ Two Targets ------ 18 | aoas = [83 100]; 19 | dists = [0.41 0.44]; 20 | amps = [0.001/dists(1)^2 0.0008/dists(2)^2]; 21 | vels = [0 0]; 22 | 23 | num_of_mps = length(amps); 24 | paras.multipath.gt_amps = amps; 25 | paras.multipath.gt_aoas = aoas; 26 | paras.multipath.gt_dists = dists; 27 | paras.multipath.gt_vels = vels; 28 | paras.multipath.revised_dists = dists; 29 | paras.multipath.num_of_mps = num_of_mps; 30 | %% Generate Mixed Soundwave 31 | array_mix_sw = generate_mixed_sw(paras); 32 | %% Traditional Estimation (FFT) 33 | %% ------ distance estimation ----- 34 | fprintf('------ distance estimation by FFT ------\n'); 35 | mix_sw = squeeze(array_mix_sw(:,1,1:num_of_chirps)); 36 | trad_dist_FFT(mix_sw, paras); 37 | %% ------ distance + aoa estimation ----- 38 | fprintf('------ distance + AoA estimation by FFT ------\n'); 39 | mix_sw = squeeze(array_mix_sw(:,:,1)); 40 | trad_dist_aoa_FFT(mix_sw, paras); 41 | %% Super-resolution Estimation 42 | %% ------ distance estimation ----- 43 | fprintf('------ distance estimation by MUSIC ------\n'); 44 | mix_sw = squeeze(array_mix_sw(:,1,1)); 45 | super_dist(mix_sw, paras); 46 | %% ------ distance + aoa estimation ----- 47 | fprintf('------ distance + AoA estimation by 2D MUSIC ------\n'); 48 | mix_sw = squeeze(array_mix_sw(:,:,1)); 49 | super_dist_aoa(mix_sw.', paras); -------------------------------------------------------------------------------- /soundwave_generation/generate_mixed_sw.m: -------------------------------------------------------------------------------- 1 | function array_mix_sw = generate_mixed_sw(paras) 2 | %GENERATEMIXEDSW Generate mixed soundwave 3 | % paras parameters 4 | %% Parameters 5 | Vs = paras.fmcw_config.Vs; 6 | Fs = paras.fmcw_config.Fs; 7 | Fc = paras.fmcw_config.Fc; 8 | B = paras.fmcw_config.B; 9 | single_chirp_len = paras.fmcw_config.single_chirp_len; 10 | 11 | total_num_of_chirps = paras.system_config.total_num_of_chirps; 12 | 13 | num_of_mics = paras.hardware_config.num_of_mics; 14 | mic_int_vec = paras.hardware_config.mic_int_vec; 15 | 16 | amps = paras.multipath.gt_amps; 17 | aoas = paras.multipath.gt_aoas; 18 | dists = paras.multipath.gt_dists; 19 | vels = paras.multipath.gt_vels; 20 | num_of_mps = paras.multipath.num_of_mps; 21 | %% Generate transmit and receive soundwave 22 | amp_tx = 10; % amplitude for transmit soundwave 23 | init_phase = 0; % initial phase 24 | % ------ transmit soundwave ------ 25 | [trans_sw_cos,trans_sw_sin,~] = generate_transmit_sw(amp_tx,init_phase,paras); 26 | % ------ middle soundwave ------ 27 | % [middle_sw_cos,middle_sw_sin,~] = generate_middle_sw(amp_tx,init_phase,paras); 28 | % ------ receive soundwave ------ 29 | array_rece_sw = zeros(num_of_mics,single_chirp_len*total_num_of_chirps); 30 | for mic_idx=1:num_of_mics 31 | for mp_idx=1:num_of_mps 32 | prop_delay = mic_int_vec(mic_idx)*cosd(aoas(mp_idx))/Vs + 2*dists(mp_idx)/Vs; 33 | [mp_rece_sw,~] = generate_receive_sw(amps(mp_idx),init_phase,vels(mp_idx),prop_delay,paras); 34 | % [mp_rece_sw,rece_freq] = generate_receive_sw(amps(mp_idx),init_phase,vels(mp_idx),prop_delay,paras); 35 | % figure; 36 | % plot(rece_freq); 37 | array_rece_sw(mic_idx,:) = squeeze(array_rece_sw(mic_idx,:)) + mp_rece_sw(1:single_chirp_len*total_num_of_chirps); 38 | end 39 | % ------ add gaussian noise ------ 40 | % array_rece_sw(mic_idx, :) = awgn(array_rece_sw(mic_idx, :),0,'measured'); 41 | end 42 | %% Construct mixed soundwave 43 | array_mix_sw = zeros(single_chirp_len,num_of_mics,total_num_of_chirps); 44 | % filter out the received soundwave whose frequency is between Fc and Fc +B 45 | bpFilt_chirp = fir1(80,[(Fc-10)/(Fs/2) (Fc+B+10)/(Fs/2)],'bandpass'); 46 | % filter out the low frequency part 47 | lpFilt_chirp = fir1(80,[(1)/(Fs/2) (1000)/(Fs/2)],'bandpass'); 48 | for chirp_idx=1:total_num_of_chirps 49 | for mic_idx=1:num_of_mics 50 | %% Apply a band-pass filter to remove out-of-band noise 51 | rece_sw = array_rece_sw(mic_idx,(chirp_idx-1)*single_chirp_len+1:chirp_idx*single_chirp_len).'; 52 | rece_sw = filtfilt(bpFilt_chirp,1,rece_sw); 53 | %% Multiply the middle and received chirps 54 | mix_sw_cos = rece_sw.*trans_sw_cos; 55 | mix_sw_sin = rece_sw.*trans_sw_sin; 56 | %% Apply a low-pass filter to obtain desired sinusoids 57 | mix_sw_cos = filtfilt(lpFilt_chirp,1,mix_sw_cos); 58 | mix_sw_sin = filtfilt(lpFilt_chirp,1,mix_sw_sin); 59 | %% Signal Pluralizing 60 | array_mix_sw(:,mic_idx,chirp_idx) = mix_sw_cos + 1j*mix_sw_sin; 61 | end 62 | end 63 | end 64 | 65 | -------------------------------------------------------------------------------- /soundwave_generation/generate_receive_sw.m: -------------------------------------------------------------------------------- 1 | function [rece_sw, rece_freq] = generate_receive_sw(amp,init_phase,vel,prop_delay,paras) 2 | %GENERATEFMCWSOUNDWAVE Generate receive soundwave 3 | % amp the attenuation factor 4 | % init_phase initial phase 5 | % vel velocity 6 | % prop_delay propagation delay in the beginning 7 | % paras parameters 8 | %% Parameters 9 | Fs = paras.fmcw_config.Fs; 10 | Fc = paras.fmcw_config.Fc; 11 | B = paras.fmcw_config.B; 12 | T = paras.fmcw_config.T; 13 | Vs = paras.fmcw_config.Vs; 14 | total_time = paras.fmcw_config.total_time; 15 | %% Generate receive soundwave 16 | num_of_samples = total_time*Fs; 17 | rece_sw = zeros(1,num_of_samples); 18 | rece_freq = zeros(1,num_of_samples); 19 | for sam_idx=0:num_of_samples-1 20 | delay = prop_delay+2*vel*sam_idx/Fs/Vs; 21 | if sam_idx/Fs >= delay 22 | t_n = mod(sam_idx/Fs-delay,T); 23 | rece_freq(sam_idx+1) = Fc+B/T*t_n; 24 | phase = 2*pi*Fc*t_n+pi*B/T*t_n^2+init_phase; 25 | rece_sw(sam_idx+1) = amp*cos(phase); 26 | end 27 | end 28 | end -------------------------------------------------------------------------------- /soundwave_generation/generate_transmit_sw.m: -------------------------------------------------------------------------------- 1 | function [trans_sw_cos, trans_sw_sin, trans_freq] = generate_transmit_sw(A,init_phase,paras) 2 | %GENERATEFMCWSOUNDWAVE Generate transmit sound or receive sound 3 | % A amplitude 4 | % init_phase initial phase 5 | % Fs sampling rate 6 | % Fc start sweep frequency 7 | % B bandwith 8 | % T sweep time 9 | % total_time length of the sound wave 10 | %% Parameters 11 | Fs = paras.fmcw_config.Fs; 12 | Fc = paras.fmcw_config.Fc; 13 | B = paras.fmcw_config.B; 14 | T = paras.fmcw_config.T; 15 | %% Generate transmit soundwave 16 | t = 0:1/Fs:T-1/Fs; 17 | 18 | % Self-defined linear FMCW wave 19 | trans_freq = Fc+B/T*t; 20 | phase = Fc*t+B*power(t,2)/(2*T)+init_phase; 21 | trans_sw_cos = A*cos(2*pi*phase).'; 22 | trans_sw_sin = A*sin(2*pi*phase).'; 23 | end -------------------------------------------------------------------------------- /super_estimation/music_1D.m: -------------------------------------------------------------------------------- 1 | function [fom_freq,fom_sp] = music_1D(sig,Fs,freq_min,freq_max,freq_step,K,W) 2 | %MUSIC_1D Estimate the frequency for one dimension signal 3 | % sig input signals 4 | % Fs sampling rate 5 | % freq_min minimum frequency 6 | % freq_max maximum frequency 7 | % freq_step frequency stepsize 8 | % K subsampling factor 9 | % W length of smoothing window 10 | % 11 | % This algorithm combines the subsampling and data smoothing: 12 | % 1. Set K=1 to avoid subsampling 13 | % 2. Set W as the number of samples in the subarray after subsampling to avoid data smoothing 14 | % Reference: 15 | % Subsampling 16 | % [1] Indoor Follow Me Drone 17 | % Data Smoothing 18 | % [2] Superresolution Techniques for Time-Domain Measurements with a Network Analyzer 19 | % [3] Super-Resolution TOA Estimation With Diversity for Indoor Geolocation 20 | %% Construct the correlation matrix 21 | freq_num = ceil((freq_max-freq_min)/freq_step); % number of estimated frequencies 22 | N = length(sig); % number of samples 23 | SUB_N = floor((N-K)/K)+1; % number of samples after subsampling 24 | NUM_W = SUB_N - W + 1; % number of the smoothing window 25 | trans_matrix = eye(W,W); 26 | J = fliplr(trans_matrix); 27 | 28 | X = zeros(W, NUM_W); 29 | if SUB_N < NUM_W 30 | fprintf('Warning: The subsampling factor K is too large!\n'); 31 | return; 32 | end 33 | 34 | R_sub = 0; % autocorrelation matrix after subsampling 35 | for sub_idx=1:K 36 | % ------ subsampling ------ 37 | sub_sig = sig(sub_idx:K:sub_idx+K*(SUB_N-1)); 38 | 39 | % ------ data smoothing ----- 40 | for win_idx=1:NUM_W 41 | X(:,win_idx) = sub_sig(win_idx:win_idx+W-1); 42 | end 43 | R_sub = R_sub + 1/(2*NUM_W)*(X*X'+J*(X*X').'*J); 44 | end 45 | R_sub = R_sub/K; 46 | 47 | % ------ MUSIC ----- 48 | [V,D] = eig(R_sub); % Compute the eigenvalues and eigenvectors of the auto-correlation matrix 49 | [D_val,I] = sort(diag(D),'descend'); % Sort the eigenvectors in a descending order in terms of the magnitude of corresponding eigenvalues. 50 | 51 | % ----- Determine the noise number ------ 52 | Level=10^4; 53 | if abs(max(D_val)/min(D_val))= peak_thresh*max_peak_amp 43 | est_dist = dist_search(pks_locs(pks_idx(mp_idx))); 44 | else 45 | % avoid side lobes 46 | est_dist = all_sig_path_paras{1}.raw_dist; 47 | end 48 | else 49 | % avoid the case where # estimated paths are less than # true paths 50 | non_empty_idx = find(~cellfun(@isempty, all_sig_path_paras)); 51 | last_idx = non_empty_idx(end); 52 | est_dist = all_sig_path_paras{last_idx}.raw_dist; 53 | end 54 | 55 | sig_para_vec.amp = 0; 56 | sig_para_vec.raw_dist = est_dist; 57 | sig_para_vec.raw_aoa = 0; 58 | sig_para_vec.raw_vel = 0; 59 | all_sig_path_paras{mp_idx} = sig_para_vec; 60 | end 61 | %% Display 62 | if display_flag 63 | celldisp(all_sig_path_paras); 64 | 65 | dist_display_step = dist_search_scope(4); 66 | figure; 67 | plot(dist_search,fom_sp.','linewidth',3); 68 | if display_flag_gt 69 | % ------ Ground Truth ----- 70 | hold on; 71 | point = plot([gt_mp_dists(1) gt_mp_dists(1)],[min(pks_amps) max(pks_amps)],'--r','linewidth',3); 72 | for mp_idx=2:num_of_mps 73 | hold on; 74 | plot([gt_mp_dists(mp_idx) gt_mp_dists(mp_idx)],[min(pks_amps) max(pks_amps)],'--r','linewidth',3); 75 | end 76 | legend(point,'Groundtruth'); 77 | end 78 | xlabel('Range (m)'); 79 | xlim([dist_min dist_max]); 80 | xticks(dist_min:dist_display_step:dist_max); 81 | ylabel('Amplitude'); 82 | title('Range MUSIC'); 83 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 84 | end 85 | end 86 | 87 | -------------------------------------------------------------------------------- /super_estimation/super_dist_aoa.m: -------------------------------------------------------------------------------- 1 | function all_sig_path_paras = super_dist_aoa(sig,paras) 2 | %SUPER_DIST_AOA Estimate the distance and AoA simultaneously 3 | % sig input signal 4 | % paras parameters 5 | % Reference: "2D-MUSIC TECHNIQUE APPLIED TO A COHERENT FMCW MIMO RADAR" 6 | %% Parameters 7 | peak_thresh = 1e-6; % peak threshold: the amplitude of the peak must be larger than 0.5*max_peak_amp 8 | 9 | Vs = paras.fmcw_config.Vs; 10 | Fs = paras.fmcw_config.Fs; 11 | Fc = paras.fmcw_config.Fc; 12 | B = paras.fmcw_config.B; 13 | T = paras.fmcw_config.T; 14 | 15 | display_flag = paras.system_config.display_flag; 16 | display_flag_gt = paras.system_config.display_flag_gt; 17 | 18 | gt_mp_dists = paras.multipath.gt_dists; 19 | gt_mp_aoas = paras.multipath.gt_aoas; 20 | num_of_mps = paras.multipath.num_of_mps; 21 | 22 | mic_int_vec = paras.hardware_config.mic_int_vec; 23 | 24 | % ------ distance ------ 25 | dist_search = paras.algo_config.dist_search; 26 | dist_search_scope = paras.algo_config.dist_search_scope; 27 | % ------ aoa ----- 28 | aoa_search = paras.algo_config.aoa_search; 29 | aoa_search_scope = paras.algo_config.aoa_search_scope; 30 | 31 | smoothed_window_aoa = paras.algo_config.super.smoothed_window_aoa; 32 | smoothed_window_dist = paras.algo_config.super.smoothed_window_dist; 33 | subsampling_factor_dist = paras.algo_config.super.subsampling_factor_dist; 34 | %% Algorithm 35 | Ts = 1/Fs; 36 | [M,N] = size(sig); 37 | sub_N = floor((N-subsampling_factor_dist)/subsampling_factor_dist)+1; 38 | p1 = M-smoothed_window_aoa+1; 39 | p2 = sub_N-smoothed_window_dist+1; 40 | 41 | trans_matrix = eye(smoothed_window_aoa*smoothed_window_dist, smoothed_window_aoa*smoothed_window_dist); 42 | J = fliplr(trans_matrix); 43 | 44 | R_sub = 0; 45 | X = zeros(smoothed_window_aoa*smoothed_window_dist, p1*p2); 46 | if sub_N <= smoothed_window_dist 47 | fprintf('Warning: The subsampling factor K is too large!\n'); 48 | return; 49 | end 50 | 51 | % ------ sub-sampling & data smoothing ------ 52 | for i=1:subsampling_factor_dist 53 | % ------ sub-sampling ------ 54 | sub_sig = zeros(M,sub_N); 55 | sub_index = i:subsampling_factor_dist:i+subsampling_factor_dist*(sub_N-1); 56 | sub_sig(:,:) = sig(:,sub_index); 57 | 58 | % ------ smooth data ------ 59 | for j = 1:p1 60 | for k = 1:p2 61 | X(:,(j-1)*p2+k) = reshape(sub_sig(j:j+smoothed_window_aoa-1,k:k+smoothed_window_dist-1).',[smoothed_window_aoa*smoothed_window_dist,1]); 62 | end 63 | end 64 | R_sub = R_sub + 1/(2*p1*p2)*(X*X'+J*(X*X').'*J); 65 | end 66 | R_sub = R_sub/subsampling_factor_dist; 67 | 68 | [V,D] = eig(R_sub); % Compute the eigenvalues and eigenvectors of the auto-correlation matrix 69 | [D_val,I] = sort(diag(D),'descend'); % Sort the eigenvectors in a descending order in terms of the magnitude of corresponding eigenvalues. 70 | 71 | Level=10^4; 72 | if abs(max(D_val)/min(D_val)) < Level 73 | fprintf('Warning: The eigen values may not correct!\n The estinated DOAs may wrong!\n') 74 | end 75 | noise_num = smoothed_window_aoa*smoothed_window_dist-length(find(D_val= peak_thresh*max_peak_amp 105 | est_dist = peak_dist_vec(peak_amp_idx(mp_idx)); 106 | est_aoa = peak_aoa_vec(peak_amp_idx(mp_idx)); 107 | else 108 | % avoid side lobes 109 | est_dist = all_sig_path_paras{1}.raw_dist; 110 | est_aoa = all_sig_path_paras{1}.raw_aoa; 111 | end 112 | else 113 | % avoid the case where # estimated paths are less than # true paths 114 | non_empty_idx = find(~cellfun(@isempty, all_sig_path_paras)); 115 | last_idx = non_empty_idx(end); 116 | est_dist = all_sig_path_paras{last_idx}.raw_dist; 117 | est_aoa = all_sig_path_paras{last_idx}.raw_aoa; 118 | end 119 | 120 | sig_para_vec.amp = 0; 121 | sig_para_vec.raw_dist = est_dist; 122 | sig_para_vec.raw_aoa = est_aoa; 123 | sig_para_vec.raw_vel = 0; 124 | all_sig_path_paras{mp_idx} = sig_para_vec; 125 | end 126 | %% Display 127 | if display_flag 128 | celldisp(all_sig_path_paras); 129 | 130 | dist_min = dist_search_scope(1); 131 | dist_max = dist_search_scope(2); 132 | dist_display_step = dist_search_scope(4); 133 | aoa_min = aoa_search_scope(1); 134 | aoa_max = aoa_search_scope(2); 135 | aoa_display_step = aoa_search_scope(4); 136 | % ------ 2D ------ 137 | figure; 138 | ax = pcolor(dist_search,aoa_search,aoa_dist_map); 139 | if display_flag_gt 140 | % ------ Ground Truth ----- 141 | hold on; 142 | point = scatter(gt_mp_dists(1),gt_mp_aoas(1),80,'+r','linewidth',3); 143 | for mp_idx=2:num_of_mps 144 | hold on; 145 | scatter(gt_mp_dists(mp_idx),gt_mp_aoas(mp_idx),80,'+r','linewidth',3); 146 | end 147 | legend(point,'Groundtruth'); 148 | end 149 | set(ax, 'LineStyle','none'); 150 | xlabel('Range (m)'); 151 | xlim([dist_min dist_max]); 152 | xticks(dist_min:dist_display_step:dist_max); 153 | ylabel('AoA (Deg)'); 154 | ylim([aoa_min aoa_max]); 155 | yticks(aoa_min:aoa_display_step:aoa_max); 156 | title('Super-resolution Range-AoA'); 157 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 158 | shading interp; 159 | colorbar; 160 | 161 | % ------ 3D ------ 162 | % figure; 163 | % ax = surf(dist_search,aoa_search,aoa_dist_map); 164 | % if display_flag_gt 165 | % % ------ Ground Truth ----- 166 | % hold on; 167 | % point = scatter3(gt_mp_dists(1),gt_mp_aoas(1),peak_amp_vec(peak_amp_idx(1)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 168 | % for mp_idx=2:num_of_mps 169 | % hold on; 170 | % scatter3(gt_mp_dists(mp_idx),gt_mp_aoas(mp_idx),peak_amp_vec(peak_amp_idx(mp_idx)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 171 | % end 172 | % legend(point,'Groundtruth','Location','best'); 173 | % end 174 | % set(ax, 'LineStyle','none'); 175 | % xlabel('Range (m)'); 176 | % xlim([dist_min dist_max]); 177 | % xticks(dist_min:dist_display_step:dist_max); 178 | % ylabel('AoA (Deg)'); 179 | % ylim([aoa_min aoa_max]); 180 | % yticks(aoa_min:aoa_display_step:aoa_max); 181 | % zlabel('Amplitude'); 182 | % title('Super-resolution Range-AoA'); 183 | % set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 184 | % colormap(jet); 185 | % shading interp; 186 | % colorbar; 187 | end 188 | end 189 | 190 | -------------------------------------------------------------------------------- /super_estimation/super_dist_vel.m: -------------------------------------------------------------------------------- 1 | function all_sig_path_paras = super_dist_vel(sig,paras) 2 | %JOINT_DIST_VEL Estimate the distance and Vs simultaneously 3 | % sig input signal 4 | % paras parameters 5 | 6 | %% Parameters 7 | peak_thresh = 1e-6; % peak threshold: the amplitude of the peak must be larger than 0.5*max_peak_amp 8 | 9 | Vs = paras.fmcw_config.Vs; 10 | Fs = paras.fmcw_config.Fs; 11 | Fc = paras.fmcw_config.Fc; 12 | B = paras.fmcw_config.B; 13 | T = paras.fmcw_config.T; 14 | 15 | display_flag = paras.system_config.display_flag; 16 | display_flag_gt = paras.system_config.display_flag_gt; 17 | 18 | gt_mp_dists = paras.multipath.gt_dists; 19 | gt_mp_vels = paras.multipath.gt_vels; 20 | num_of_mps = paras.multipath.num_of_mps; 21 | 22 | % ------ distance ------ 23 | dist_search = paras.algo_config.dist_search; 24 | dist_search_scope = paras.algo_config.dist_search_scope; 25 | % ------ aoa ----- 26 | vel_search = paras.algo_config.vel_search; 27 | vel_search_scope = paras.algo_config.vel_search_scope; 28 | 29 | smoothed_window_vel = paras.algo_config.super.smoothed_window_vel; 30 | smoothed_window_dist = paras.algo_config.super.smoothed_window_dist; 31 | subsampling_factor_dist = paras.algo_config.super.subsampling_factor_dist; 32 | %% Algo 33 | [M,N] = size(sig); 34 | sub_N = floor((N-subsampling_factor_dist)/subsampling_factor_dist)+1; 35 | p1 = M-smoothed_window_vel+1; 36 | p2 = sub_N-smoothed_window_dist+1; 37 | 38 | trans_matrix = eye(smoothed_window_vel*smoothed_window_dist, smoothed_window_vel*smoothed_window_dist); 39 | J = fliplr(trans_matrix); 40 | 41 | R_sub = 0; 42 | X = zeros(smoothed_window_vel*smoothed_window_dist, p1*p2); 43 | if sub_N <= smoothed_window_dist 44 | fprintf('Warning: The subsampling factor K is too large!\n'); 45 | return; 46 | end 47 | 48 | for i=1:subsampling_factor_dist 49 | % ------ sub-sampling ------ 50 | sub_sig = zeros(M,sub_N); 51 | sub_index = i:subsampling_factor_dist:i+subsampling_factor_dist*(sub_N-1); 52 | sub_sig(:,:) = sig(:,sub_index); 53 | 54 | % ------ smooth data ------ 55 | for j = 1:p1 56 | for k = 1:p2 57 | X(:,(j-1)*p2+k) = reshape(sub_sig(j:j+smoothed_window_vel-1,k:k+smoothed_window_dist-1).',[smoothed_window_vel*smoothed_window_dist,1]); 58 | end 59 | end 60 | R_sub = R_sub + 1/(2*p1*p2)*(X*X'+J*(X*X').'*J); 61 | end 62 | R_sub = R_sub/subsampling_factor_dist; 63 | 64 | [V,D] = eig(R_sub); % Compute the eigenvalues and eigenvectors of the auto-correlation matrix 65 | [D_val,I] = sort(diag(D),'descend'); % Sort the eigenvectors in a descending order in terms of the magnitude of corresponding eigenvalues. 66 | 67 | Level=10^4; 68 | if abs(max(D_val)/min(D_val)) < Level 69 | fprintf('Warning: The eigen values may not correct!\n The estinated DOAs may wrong!\n') 70 | end 71 | noise_num = smoothed_window_vel*smoothed_window_dist-length(find(D_val= peak_thresh*max_peak_amp 103 | est_dist = peak_dist_vec(peak_amp_idx(mp_idx)); 104 | est_vel = peak_vel_vec(peak_amp_idx(mp_idx)); 105 | else 106 | % avoid side lobes 107 | est_dist = all_sig_path_paras{1}.raw_dist; 108 | est_vel = all_sig_path_paras{1}.raw_vel; 109 | end 110 | else 111 | % avoid the case where # estimated paths are less than # true paths 112 | non_empty_idx = find(~cellfun(@isempty, all_sig_path_paras)); 113 | last_idx = non_empty_idx(end); 114 | est_dist = all_sig_path_paras{last_idx}.raw_dist; 115 | est_vel = all_sig_path_paras{last_idx}.raw_vel; 116 | end 117 | 118 | sig_para_vec.amp = 0; 119 | sig_para_vec.raw_dist = est_dist; 120 | sig_para_vec.raw_aoa = 0; 121 | sig_para_vec.raw_vel = est_vel; 122 | all_sig_path_paras{mp_idx} = sig_para_vec; 123 | end 124 | %% Display 125 | if display_flag 126 | celldisp(all_sig_path_paras); 127 | 128 | dist_min = dist_search_scope(1); 129 | dist_max = dist_search_scope(2); 130 | dist_display_step = dist_search_scope(4); 131 | vel_min = vel_search_scope(1); 132 | vel_max = vel_search_scope(2); 133 | vel_display_step = vel_search_scope(4); 134 | % ------ 2D ------ 135 | figure; 136 | ax = pcolor(dist_search,vel_search,vel_dist_map); 137 | if display_flag_gt 138 | % ------ Ground Truth ----- 139 | hold on; 140 | point = scatter(gt_mp_dists(1),gt_mp_vels(1),80,'+r','linewidth',3); 141 | for mp_idx=2:num_of_mps 142 | hold on; 143 | scatter(gt_mp_dists(mp_idx),gt_mp_vels(mp_idx),80,'+r','linewidth',3); 144 | end 145 | legend(point,'Groundtruth'); 146 | end 147 | set(ax, 'LineStyle','none'); 148 | xlabel('Range (m)'); 149 | xlim([dist_min dist_max]); 150 | xticks(dist_min:dist_display_step:dist_max); 151 | ylabel('Velocity(m/s)'); 152 | ylim([vel_min vel_max]); 153 | yticks(vel_min:vel_display_step:vel_max); 154 | title('Super-resolution Range-Velocity'); 155 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 156 | shading interp; 157 | colorbar; 158 | 159 | % ------ 3D ------ 160 | figure; 161 | ax = surf(dist_search,vel_search,vel_dist_map); 162 | if display_flag_gt 163 | % ------ Ground Truth ----- 164 | hold on; 165 | point = scatter3(gt_mp_dists(1),gt_mp_vels(1),peak_amp_vec(peak_amp_idx(1)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 166 | for mp_idx=2:num_of_mps 167 | hold on; 168 | scatter3(gt_mp_dists(mp_idx),gt_mp_vels(mp_idx),peak_amp_vec(peak_amp_idx(mp_idx)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 169 | end 170 | legend(point,'Groundtruth','Location','best'); 171 | end 172 | set(ax, 'LineStyle','none'); 173 | xlabel('Range (m)'); 174 | xlim([dist_min dist_max]); 175 | xticks(dist_min:dist_display_step:dist_max); 176 | ylabel('Velocity(m/s)'); 177 | ylim([vel_min vel_max]); 178 | yticks(vel_min:vel_display_step:vel_max); 179 | zlabel('Amplitude'); 180 | title('Super-resolution Range-Velocity'); 181 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 182 | colormap(jet); 183 | shading interp; 184 | colorbar; 185 | end 186 | end 187 | 188 | -------------------------------------------------------------------------------- /traditional_estimation/trad_dist_FFT.m: -------------------------------------------------------------------------------- 1 | function [all_sig_path_paras,selected_chirp] = trad_dist_FFT(sig,paras) 2 | %TRAD_RANGE_FFT Estimate the range using FFT 3 | % sig input signal 4 | % paras parameters 5 | 6 | %% Parameters 7 | peak_thresh = 0; % peak threshold: the amplitude of the peak must be larger than peak_thresh*max_peak_amp 8 | 9 | Vs = paras.fmcw_config.Vs; 10 | Fs = paras.fmcw_config.Fs; 11 | B = paras.fmcw_config.B; 12 | T = paras.fmcw_config.T; 13 | 14 | display_flag = paras.system_config.display_flag; 15 | display_flag_gt = paras.system_config.display_flag_gt; 16 | 17 | dist_search_scope = paras.algo_config.dist_search_scope; 18 | num_of_chirps = paras.algo_config.num_of_chirps; 19 | dist_fft_size = paras.algo_config.trad.dist_fft_size; 20 | 21 | num_of_mps = paras.multipath.num_of_mps; 22 | gt_mp_dists = paras.multipath.gt_dists; 23 | %% Algorithm 24 | dist_min = dist_search_scope(1); 25 | dist_max = dist_search_scope(2); 26 | dist_search = linspace(0,Fs/2,dist_fft_size/2)*Vs*T/(2*B); 27 | dist_idx = (dist_search >= dist_min) & (dist_search <= dist_max); 28 | dist_search = dist_search(dist_idx); 29 | 30 | % ------ Range FFT ------ 31 | dist_fft = fft(sig,dist_fft_size,1); % perform row-wise fft 32 | % dist_fft_2 = dist_fft(1:dist_fft_size/2+1); 33 | % dist_fft_2(2:end-1) = flip(dist_fft(dist_fft_size/2+2:end)); 34 | % dist_fft(2:dist_fft_size/2,:) = flip(dist_fft(dist_fft_size/2+2:end,:),1); 35 | dist_fft = dist_fft(dist_idx,:); % choose the data within a certain distance, e.g. <= dist_max 36 | 37 | dist_map = abs(dist_fft).^2/dist_fft_size; 38 | 39 | chirp_idx = 1; 40 | selected_chirp = dist_map(:,chirp_idx); 41 | % ------ Background subtraction test ------ 42 | % selected_chirp = dist_map(:,chirp_idx) - paras.back_fft; 43 | [pks_amps, pks_locs] = findpeaks(selected_chirp); 44 | [~, pks_idx] = sort(pks_amps,'descend'); 45 | 46 | max_peak_amp = pks_amps(pks_idx(1)); 47 | all_sig_path_paras = cell(1,num_of_mps); 48 | for mp_idx=1:num_of_mps 49 | if mp_idx <= length(pks_idx) 50 | if pks_amps(pks_idx(mp_idx)) >= peak_thresh*max_peak_amp 51 | est_dist = dist_search(pks_locs(pks_idx(mp_idx))); 52 | else 53 | % avoid side lobes 54 | est_dist = all_sig_path_paras{1}.raw_dist; 55 | end 56 | else 57 | % avoid the case where # estimated paths are less than # true paths 58 | non_empty_idx = find(~cellfun(@isempty, all_sig_path_paras)); 59 | last_idx = non_empty_idx(end); 60 | est_dist = all_sig_path_paras{last_idx}.raw_dist; 61 | end 62 | 63 | sig_para_vec.amp = 0; 64 | sig_para_vec.raw_dist = est_dist; 65 | sig_para_vec.raw_aoa = 0; 66 | sig_para_vec.raw_vel = 0; 67 | all_sig_path_paras{mp_idx} = sig_para_vec; 68 | end 69 | %% Display 70 | if display_flag 71 | celldisp(all_sig_path_paras); 72 | 73 | dist_display_step = dist_search_scope(4); 74 | figure; 75 | plot(dist_search,selected_chirp,'linewidth',3); 76 | if display_flag_gt 77 | % ------ Ground Truth ----- 78 | hold on; 79 | point = plot([gt_mp_dists(1) gt_mp_dists(1)],[min(selected_chirp) max(selected_chirp)],'--r','linewidth',3); 80 | for mp_idx=2:num_of_mps 81 | hold on; 82 | plot([gt_mp_dists(mp_idx) gt_mp_dists(mp_idx)],[min(selected_chirp) max(selected_chirp)],'--r','linewidth',3); 83 | end 84 | legend(point,'Groundtruth'); 85 | end 86 | xlabel('Range (m)'); 87 | xlim([dist_min dist_max]); 88 | xticks(dist_min:dist_display_step:dist_max); 89 | ylabel('Amplitude'); 90 | title('Range FFT'); 91 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 92 | 93 | % % ------ 2D ------ 94 | % figure; 95 | % ax = pcolor(dist_search,1:num_of_chirps,dist_map.'); 96 | % if display_flag_gt 97 | % % ------ Ground Truth ----- 98 | % for mp_idx=1:num_of_mps 99 | % hold on; 100 | % plot([gt_mp_dists(mp_idx) gt_mp_dists(mp_idx)],[0 num_of_chirps],'--w','linewidth',3); 101 | % end 102 | % end 103 | % set(ax, 'LineStyle','none'); 104 | % xlabel('Range (m)'); 105 | % xlim([0 dist_max]); 106 | % xticks(0:dist_display_step:dist_max); 107 | % ylabel('Chirp Number'); 108 | % title('Range-Chirp FFT'); 109 | % set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 110 | % colormap(jet); 111 | % shading interp; 112 | % colorbar; 113 | % 114 | % % ------ 3D ------ 115 | % figure; 116 | % ax = surf(dist_search,1:num_of_chirps,dist_map.'); 117 | % set(ax, 'LineStyle','none'); 118 | % xlabel('Range (m)'); 119 | % xlim([0 dist_max]); 120 | % xticks(0:dist_display_step:dist_max); 121 | % ylabel('Chirp Number'); 122 | % zlabel('Amplitude'); 123 | % title('Distance-Chirp FFT'); 124 | % set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 125 | % colormap(jet); 126 | % shading interp; 127 | % colorbar; 128 | end 129 | 130 | end 131 | 132 | -------------------------------------------------------------------------------- /traditional_estimation/trad_dist_aoa_FFT.m: -------------------------------------------------------------------------------- 1 | function [all_sig_path_paras] = trad_dist_aoa_FFT(sig,paras) 2 | %TRAD_DIST_AOA_FFT Estimate the range and aoa using FFT 3 | % sig input signal 4 | % paras parameters 5 | % all_sig_path_paras path parameters for all signals (cell) 6 | %% Parameters 7 | peak_thresh = 0; % peak threshold: the amplitude of the peak must be larger than 0.5*max_peak_amp 8 | 9 | display_flag = paras.system_config.display_flag; 10 | display_flag_gt = paras.system_config.display_flag_gt; 11 | 12 | Vs = paras.fmcw_config.Vs; 13 | B = paras.fmcw_config.B; 14 | T = paras.fmcw_config.T; 15 | Fs = paras.fmcw_config.Fs; 16 | 17 | % ------ distance ------ 18 | dist_search_scope = paras.algo_config.dist_search_scope; 19 | dist_fft_size = paras.algo_config.trad.dist_fft_size; 20 | % ------ aoa ------ 21 | aoa_search_scope = paras.algo_config.aoa_search_scope; 22 | aoa_fft_size = paras.algo_config.trad.aoa_fft_size; 23 | 24 | gt_mp_dists = paras.multipath.gt_dists; 25 | gt_mp_aoas = paras.multipath.gt_aoas; 26 | num_of_mps = paras.multipath.num_of_mps; 27 | %% Algorithm 28 | dist_min = dist_search_scope(1); 29 | dist_max = dist_search_scope(2); 30 | dist_search = linspace(0,Fs/2,dist_fft_size/2)*Vs*T/(2*B); 31 | dist_idx = (dist_search >= dist_min) & (dist_search <= dist_max); 32 | dist_search = dist_search(dist_idx); 33 | 34 | aoa_min = aoa_search_scope(1); 35 | aoa_max = aoa_search_scope(2); 36 | aoa_search = acosd(linspace(-1,1,aoa_fft_size)); 37 | aoa_idx = (aoa_search>=aoa_min) & (aoa_search<=aoa_max); 38 | aoa_search = aoa_search(aoa_idx); 39 | 40 | % ------ Range FFT ------ 41 | dist_mic_fft = fft(sig,dist_fft_size,1); % perform column-wise fft 42 | dist_mic_fft = dist_mic_fft(dist_idx,:); % choose the data within a certain distance 43 | % ------ AoA FFT ------ 44 | dist_aoa_fft = fftshift(fft(dist_mic_fft,aoa_fft_size,2),2); % perform row-wise fft 45 | dist_aoa_fft = dist_aoa_fft(:,aoa_idx); % choose the data within a certain angle 46 | 47 | dist_aoa_map = abs(dist_aoa_fft).^2/dist_fft_size/aoa_fft_size; 48 | 49 | % ------ Estimate the range and velocity from the vel_dist_map ------ 50 | reg_max = imregionalmax(dist_aoa_map); 51 | [peak_row,peak_col,~] = find(reg_max); 52 | peak_dist_vec = dist_search(peak_row); 53 | peak_aoa_vec = aoa_search(peak_col); 54 | peak_amp_vec = zeros(1, length(peak_row)); 55 | for peak_idx = 1:length(peak_row) 56 | peak_amp_vec(peak_idx) = dist_aoa_map(peak_row(peak_idx),peak_col(peak_idx)); 57 | end 58 | [~,peak_amp_idx] = sort(peak_amp_vec,'descend'); 59 | max_peak_amp = peak_amp_vec(peak_amp_idx(1)); 60 | all_sig_path_paras = cell(1,num_of_mps); 61 | for mp_idx=1:num_of_mps 62 | if mp_idx <= length(peak_amp_idx) 63 | if peak_amp_vec(peak_amp_idx(mp_idx)) >= peak_thresh*max_peak_amp 64 | est_dist = peak_dist_vec(peak_amp_idx(mp_idx)); 65 | est_aoa = peak_aoa_vec(peak_amp_idx(mp_idx)); 66 | else 67 | % avoid side lobes 68 | est_dist = all_sig_path_paras{1}.raw_dist; 69 | est_aoa = all_sig_path_paras{1}.raw_aoa; 70 | end 71 | else 72 | % avoid the case where # estimated paths are less than # true paths 73 | est_dist = all_sig_path_paras{1}.raw_dist; 74 | est_aoa = all_sig_path_paras{1}.raw_aoa; 75 | end 76 | 77 | sig_para_vec.amp = 0; 78 | sig_para_vec.raw_dist = est_dist; 79 | sig_para_vec.raw_aoa = est_aoa; 80 | sig_para_vec.raw_vel = 0; 81 | all_sig_path_paras{mp_idx} = sig_para_vec; 82 | end 83 | %% Display 84 | if display_flag 85 | celldisp(all_sig_path_paras); 86 | 87 | dist_display_step = dist_search_scope(4); 88 | aoa_display_step = aoa_search_scope(4); 89 | % ------ 2D ------ 90 | figure; 91 | ax = pcolor(dist_search,aoa_search,dist_aoa_map.'); 92 | if display_flag_gt 93 | % groundtruth 94 | points = zeros(1,1); 95 | hold on; 96 | points(1) = scatter(gt_mp_dists(1),gt_mp_aoas(1),80,'+r','linewidth',3); 97 | for mp_idx=2:num_of_mps 98 | hold on; 99 | scatter(gt_mp_dists(mp_idx),gt_mp_aoas(mp_idx),80,'+r','linewidth',3); 100 | end 101 | legend(points,'Groundtruth'); 102 | end 103 | set(ax, 'LineStyle','none'); 104 | xlabel('Range (m)'); 105 | xlim([dist_min dist_max]); 106 | xticks(dist_min:dist_display_step:dist_max); 107 | ylabel('AoA (Deg)'); 108 | ylim([aoa_min aoa_max]); 109 | yticks(aoa_min:aoa_display_step:aoa_max); 110 | title('Range-AoA FFT'); 111 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 112 | shading interp; 113 | colorbar; 114 | 115 | % ------ 3D ------ 116 | % figure; 117 | % ax = surf(dist_search,aoa_search,dist_aoa_map.'); 118 | % if display_flag_gt 119 | % % groundtruth 120 | % points = zeros(1,1); 121 | % hold on; 122 | % points(1) = scatter3(gt_mp_dists(1),gt_mp_aoas(1),peak_amp_vec(peak_amp_idx(1)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 123 | % for mp_idx=2:num_of_mps 124 | % hold on; 125 | % scatter3(gt_mp_dists(mp_idx),gt_mp_aoas(mp_idx),peak_amp_vec(peak_amp_idx(mp_idx)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 126 | % end 127 | % legend(points,'Groundtruth','location','best'); 128 | % end 129 | % set(ax, 'LineStyle','none'); 130 | % xlabel('Range (m)'); 131 | % xlim([dist_min dist_max]); 132 | % xticks(dist_min:dist_display_step:dist_max); 133 | % ylabel('AoA (Deg)'); 134 | % ylim([aoa_min aoa_max]); 135 | % yticks(aoa_min:aoa_display_step:aoa_max); 136 | % zlabel('Amplitude'); 137 | % title('Range-AoA FFT'); 138 | % set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 139 | % colormap(jet); 140 | % shading interp; 141 | % colorbar; 142 | end 143 | end 144 | 145 | -------------------------------------------------------------------------------- /traditional_estimation/trad_dist_vel_FFT.m: -------------------------------------------------------------------------------- 1 | function all_sig_path_paras = trad_dist_vel_FFT(sig,paras) 2 | %TRAD_RANGE_VEL_FFT Estimate the range and velocity using FFT 3 | % sig input signal 4 | % paras parameters 5 | %% Parameters 6 | peak_thresh = 0; % peak threshold: the amplitude of the peak must be larger than 0.5*max_peak_amp 7 | 8 | display_flag = paras.system_config.display_flag; 9 | display_flag_gt = paras.system_config.display_flag_gt; 10 | 11 | Vs = paras.fmcw_config.Vs; 12 | B = paras.fmcw_config.B; 13 | T = paras.fmcw_config.T; 14 | Fs = paras.fmcw_config.Fs; 15 | Fc = paras.fmcw_config.Fc; 16 | 17 | % ------ distance ------ 18 | dist_search_scope = paras.algo_config.dist_search_scope; 19 | dist_fft_size = paras.algo_config.trad.dist_fft_size; 20 | % ------ velocity ------ 21 | vel_search_scope = paras.algo_config.vel_search_scope; 22 | vel_fft_size = paras.algo_config.trad.vel_fft_size; 23 | 24 | gt_mp_dists = paras.multipath.gt_dists; 25 | gt_mp_vels = paras.multipath.gt_vels; 26 | num_of_mps = paras.multipath.num_of_mps; 27 | %% Algos 28 | dist_min = dist_search_scope(1); 29 | dist_max = dist_search_scope(2); 30 | dist_search = linspace(0,Fs/2,dist_fft_size/2)*Vs*T/(2*B); 31 | dist_idx = (dist_search >= dist_min) & (dist_search <= dist_max); 32 | dist_search = dist_search(dist_idx); 33 | 34 | doppler_freq = 1/T; 35 | vel_min = vel_search_scope(1); 36 | vel_max = vel_search_scope(2); 37 | vel_search = linspace(-doppler_freq/2,doppler_freq/2,vel_fft_size)*Vs/(2*Fc); 38 | vel_idx = (vel_search>=vel_min) & (vel_search<=vel_max); 39 | vel_search = vel_search(vel_idx); 40 | 41 | % ------ Range FFT ------ 42 | dist_chirp_fft = fft(sig,dist_fft_size,1); % perform column-wise fft 43 | dist_chirp_fft = dist_chirp_fft(dist_idx,:); % choose the data within a certain distance, e.g. <= dist_max 44 | % ------ Doppler FFT ------ 45 | dist_vel_fft = fftshift(fft(dist_chirp_fft,vel_fft_size,2),2); % perform row-wise fft 46 | dist_vel_fft = dist_vel_fft(:,vel_idx); % choose the data within a certain velocity, e.g. <= vel_max 47 | 48 | dist_vel_map = abs(dist_vel_fft).^2/dist_fft_size/vel_fft_size; 49 | 50 | % ------ Estimate the range and velocity from the vel_dist_map ------ 51 | reg_max = imregionalmax(dist_vel_map); 52 | [peak_row,peak_col,~] = find(reg_max); 53 | peak_dist_vec = dist_search(peak_row); 54 | peak_vel_vec = vel_search(peak_col); 55 | peak_amp_vec = zeros(1, length(peak_row)); 56 | for peak_idx = 1:length(peak_row) 57 | peak_amp_vec(peak_idx) = dist_vel_map(peak_row(peak_idx),peak_col(peak_idx)); 58 | end 59 | [~,peak_amp_idx] = sort(peak_amp_vec,'descend'); 60 | max_peak_amp = peak_amp_vec(peak_amp_idx(1)); 61 | all_sig_path_paras = cell(num_of_mps,1); 62 | for mp_idx=1:num_of_mps 63 | if mp_idx <= length(peak_amp_idx) 64 | if peak_amp_vec(peak_amp_idx(mp_idx)) >= peak_thresh*max_peak_amp 65 | est_dist = peak_dist_vec(peak_amp_idx(mp_idx)); 66 | est_vel = peak_vel_vec(peak_amp_idx(mp_idx)); 67 | else 68 | % avoid side lobes 69 | est_dist = all_sig_path_paras{1}.raw_dist; 70 | est_vel = all_sig_path_paras{1}.raw_vel; 71 | end 72 | else 73 | % avoid the case where # estimated paths are less than # true paths 74 | est_dist = all_sig_path_paras{1}.raw_dist; 75 | est_vel = all_sig_path_paras{1}.raw_vel; 76 | end 77 | 78 | sig_para_vec.amp = 0; 79 | sig_para_vec.raw_dist = est_dist; 80 | sig_para_vec.raw_aoa = 0; 81 | sig_para_vec.raw_vel = est_vel; 82 | all_sig_path_paras{mp_idx} = sig_para_vec; 83 | end 84 | 85 | %% Display 86 | if display_flag 87 | celldisp(all_sig_path_paras); 88 | 89 | dist_display_step = dist_search_scope(4); 90 | vel_display_step = vel_search_scope(4); 91 | % ------ 2D ------ 92 | figure; 93 | ax = pcolor(dist_search,vel_search,dist_vel_map.'); 94 | if display_flag_gt 95 | % groundtruth 96 | points = zeros(1,1); 97 | hold on; 98 | points(1) = scatter(gt_mp_dists(1),gt_mp_vels(1),80,'+r','linewidth',3); 99 | for mp_idx=2:num_of_mps 100 | hold on; 101 | scatter(gt_mp_dists(mp_idx),gt_mp_vels(mp_idx),80,'+r','linewidth',3); 102 | end 103 | legend(points,'Groundtruth'); 104 | end 105 | set(ax, 'LineStyle','none'); 106 | xlabel('Range (m)'); 107 | xlim([dist_min dist_max]); 108 | xticks(dist_min:dist_display_step:dist_max); 109 | ylabel('Velocity(m/s)'); 110 | ylim([vel_min vel_max]); 111 | yticks(vel_min:vel_display_step:vel_max); 112 | title('Range-Velocity FFT'); 113 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 114 | shading interp; 115 | colorbar; 116 | 117 | % ------ 3D ------ 118 | figure; 119 | ax = surf(dist_search,vel_search,dist_vel_map.'); 120 | if display_flag_gt 121 | % groundtruth 122 | points = zeros(1,1); 123 | hold on; 124 | points(1) = scatter3(gt_mp_dists(1),gt_mp_vels(1),peak_amp_vec(peak_amp_idx(1)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 125 | for mp_idx=2:num_of_mps 126 | hold on; 127 | scatter3(gt_mp_dists(mp_idx),gt_mp_vels(mp_idx),peak_amp_vec(peak_amp_idx(mp_idx)),'o','MarkerEdgeColor',[0.8500, 0.3250, 0.0980],'markerfacecolor','w','linewidth',2); 128 | end 129 | legend(points,'Groundtruth','location','best'); 130 | end 131 | set(ax, 'LineStyle','none'); 132 | xlabel('Range (m)'); 133 | xlim([dist_min dist_max]); 134 | xticks(dist_min:dist_display_step:dist_max); 135 | ylabel('Velocity(m/s)'); 136 | ylim([vel_min vel_max]); 137 | yticks(vel_min:vel_display_step:vel_max); 138 | zlabel('Amplitude'); 139 | title('Range-Velocity FFT'); 140 | set(gca,'linewidth',1.5,'fontsize',20,'fontname','Arial'); 141 | colormap(jet); 142 | shading interp; 143 | colorbar; 144 | end 145 | 146 | end 147 | 148 | --------------------------------------------------------------------------------