Yujen Ku / Object Detection in Urban Scenarios using Vehicular Communication Signals

Download as
Browse Code ยป

Commit b9d8be5b4b4f766f0c501457e8c01e61065a684a

Authored by Yujen Ku
1 parent 7ccb9a1271
Exists in master

the new code

Showing 1 changed file with 570 additions and 0 deletions Side-by-side Diff

mmwave_objectdetection.m View file @ b9d8be5
  1 +clear;
  2 +alpha_est_list = [];
  3 +
  4 +raw_rx_dec_record = 0;
  5 +%% Params:
  6 +
  7 +%control param
  8 +Detail_plot = 0;
  9 +showdetail = 0;
  10 +DETECTION_OFFSET = 50; % to add packet detection error
  11 +INTERPOLATE = 0;
  12 +
  13 +%snr settings (run estimation under different snr values)
  14 +snr_value = [0]; %snr_value = [10,20,50,100,200,500,1000];
  15 +
  16 +%how many cycles of packets you are going to measure(each cycle: 2 nulling estimation packets andd 5 object detecting packets)
  17 +total_num_est = 100;
  18 +
  19 +%how many packet used in one measurement. (not including 2 packets for nulling)
  20 +num_avgpkt = 5;
  21 +
  22 +%variables for estimated parameters
  23 +obj_est_gridx_record = zeros(length(snr_value),total_num_est);
  24 +obj_est_gridy_record = zeros(length(snr_value),total_num_est);
  25 +ant_est_veloc_record = zeros(length(snr_value),total_num_est);
  26 +
  27 +
  28 +for snr_i = 1:1:length(snr_value)
  29 + snr = snr_value(snr_i);
  30 + disp([ 'creating case snr = ' num2str(snr)]);
  31 +%environment param
  32 +% | ||
  33 +% (0,0)+-------------------- <---v_obj -----o(x_obj)
  34 +% | ||
  35 +% |< x_wall >||
  36 +% | ||
  37 +% | ||
  38 +% | ||
  39 +% ^ ||
  40 +% | ||
  41 +%(y_ant) car(ant) ||
  42 +
  43 +%setting the initial position, velocity of object and antenna
  44 + x_obj = 3; %initial position of moving object(m)
  45 + x_ori_obj = x_obj;
  46 + y_ant = -10; %initial position of antenna(m)
  47 + v_ant = 10; %velocity of antenna(m/s)
  48 + v_obj = -1.5; %velocity of moving object(m/s)
  49 + x_wall = 1; %horizontal distance of wall to y axis (m)
  50 + thick_wall = 0.01; %2*thickness of wall (m)
  51 +
  52 +% __/ Tx
  53 +%
  54 +% __/ Rx
  55 +%
  56 +% __/ Tx
  57 +
  58 + dis_ant = 0.1; %distance between antennas
  59 +
  60 + % Waveform params
  61 + N_OFDM_SYMS = 500; % Number of OFDM symbols
  62 + MOD_ORDER = 2; % Modulation order in power of 2 (1/2/4/6 = BSPK/QPSK/16-QAM/64-QAM)
  63 + TX_SCALE = 1.0; % Scale for Tx waveform ([0:1])
  64 +
  65 + % OFDM params
  66 + SC_IND_PILOTS = [-150,-130,-110,-90,-70,-50,-30,-10,10,30,50,70,90,110,130,150]+256; % Pilot subcarrier indices
  67 + SC_IND_DATA = [-177:-151, -149:-131, -129:-111, -109:-91, -89:-71, -69:-51, -49:-31, -29:-11, -9:-2, 2:9, 11:29, 31:49, 51:69, 71:89, 91:109, 111:129,131:149 , 151:177] + 256; % Data subcarrier indices
  68 + N_SC = 512; % Number of subcarriers
  69 + CP_LEN = 128; % Cyclic prefix length
  70 + N_DATA_SYMS = N_OFDM_SYMS * length(SC_IND_DATA); % Number of data symbols (one per data-bearing subcarrier per OFDM symbol)
  71 +
  72 + channel_coding = .5; % coding rate
  73 + trellis_end_length = 8; % bits for trellis to end
  74 +
  75 + v_c = 299792458; %light speed (m/s)
  76 + SC_spacing = 5.165625e6; %sub-carrier spacing(Hz)
  77 + center_freq = 60e9; %center frequency
  78 + symbol_time = 1/SC_spacing; %duration of 1 OFDM symbol
  79 + sample_time = symbol_time/N_SC; %duration of 1 sample
  80 +
  81 + pck_interval = 200e-6; %interval between two packets
  82 + sample_interval = ceil(pck_interval/sample_time); %number of samples between two packets
  83 +
  84 + distance_real = zeros(1,total_num_est); %record the real distance of each measurement
  85 + distance_esti = zeros(1,total_num_est); %record the estimate distance of each measurement
  86 + ant_est_velo = zeros(1,total_num_est); %record the estimate antenna velocity of each measurement
  87 + ant_est_velo(1,1) = v_ant; %initial the estimation as the true speed
  88 + gridx_real = zeros(1,total_num_est); %record the real x position of the object
  89 + gridy_real = zeros(1,total_num_est); %record the real y position of the object
  90 + alpha_est_list = zeros(1,total_num_est); %record the estiamte AoA of each measurement
  91 + alpha_real_list = zeros(1,total_num_est); %record the real AoA of each measurement
  92 + est_pktwindow = 30; %do the measurement after every 'est_pktwindow' number of packets
  93 + num_est = 0;
  94 +
  95 + obj_mov_ppkt = pck_interval*v_obj;
  96 + for t = 1:total_num_est*est_pktwindow
  97 + if mod(t,est_pktwindow) == 0
  98 + disp([ ' creating signal t = ' num2str(t)]);
  99 + end
  100 +
  101 + %update the start index of time, sample and position of object
  102 + time_start = (t-1)*pck_interval+1;
  103 + sample_start = (t-1)*sample_interval+1;
  104 + x_obj = x_obj+v_obj*pck_interval; %new x location of object
  105 + y_ant = y_ant+v_ant*pck_interval; %new y location of antenna
  106 +
  107 + %periodically measurement
  108 + if mod(t,est_pktwindow)>( 2+num_avgpkt) || mod(t,est_pktwindow) == 0
  109 + %do nothing
  110 + else
  111 +
  112 + if mod(t,est_pktwindow) == 3
  113 + num_est = num_est+1;
  114 + distance_real(1,num_est) = sqrt(x_obj^2+y_ant^2);
  115 + end
  116 + %% Preamble
  117 + if mod(t,est_pktwindow) == 1
  118 + H_tx2 = ones(1,N_SC);
  119 + distance_avg = zeros(1,num_avgpkt);
  120 + alpha_avg = zeros(1,num_avgpkt);
  121 + velo_avg = zeros(1,num_avgpkt);
  122 + num_avgpkt_i = 1;
  123 + end
  124 + % reference [802.11ad PHY]
  125 +
  126 + Ga_128_f_1 = [ 1,1,-1,-1,-1,-1,-1,-1,-1,1,-1,1,1,-1,-1,1,1,1,-1,-1,1,1,1,1,-1,1,-1,1,-1,1,1,-1,...
  127 + -1,-1,1,1,1,1,1,1,1,-1,1,-1,-1,1,1,-1,1,1,-1,-1,1,1,1,1,-1,1,-1,1,-1,1,1,-1,...
  128 + 1,1,-1,-1,-1,-1,-1,-1,-1,1,-1,1,1,-1,-1,1,1,1,-1,-1,1,1,1,1,-1,1,-1,1,-1,1,1,-1,...
  129 + 1,1,-1,-1,-1,-1,-1,-1,-1,1,-1,1,1,-1,-1,1,-1,-1,1,1,-1,-1,-1,-1,1,-1,1,-1,1,-1,-1,1];
  130 + Gb_128_f_1 = [-1,-1, 1, 1, 1, 1, 1, 1, 1,-1, 1,-1,-1, 1, 1,-1,-1,-1,1,1,-1,-1,-1,-1,1,-1,1,-1,1,-1,-1,1,...
  131 + 1, 1,-1,-1,-1,-1,-1,-1,-1, 1,-1, 1, 1,-1,-1, 1,-1,-1,1,1,-1,-1,-1,-1,1,-1,1,-1,1,-1,-1,1,...
  132 + 1,1,-1,-1,-1,-1,-1,-1,-1,1,-1,1,1,-1,-1,1,1,1,-1,-1,1,1,1,1,-1,1,-1,1,-1,1,1,-1,...
  133 + 1,1,-1,-1,-1,-1,-1,-1,-1,1,-1,1,1,-1,-1,1,-1,-1,1,1,-1,-1,-1,-1,1,-1,1,-1,1,-1,-1,1];
  134 + Ga_128_t = ifft(Ga_128_f_1,128);
  135 + Ga_128_t_neg = ifft(-1*Ga_128_f_1,128);
  136 + Gb_128_t = ifft(Gb_128_f_1,128);
  137 + Gb_128_t_neg = ifft(-1*Gb_128_f_1,128);
  138 +
  139 + Gv_512_f = [-Gb_128_f_1, Ga_128_f_1, -Gb_128_f_1, -Ga_128_f_1];
  140 + Gu_512_f = [-Gb_128_f_1, -Ga_128_f_1, Gb_128_f_1, -Ga_128_f_1];
  141 +
  142 + %Nulling for preamble
  143 + Gv_512_f_2 = H_tx2.*[-Gb_128_f_1, Ga_128_f_1, -Gb_128_f_1, -Ga_128_f_1];
  144 + Gu_512_f_2 = H_tx2.*[-Gb_128_f_1, -Ga_128_f_1, Gb_128_f_1, -Ga_128_f_1];
  145 +
  146 + %Fourier transform of preamble from freq domain to time domain
  147 + %first antenna
  148 + Gv_512_t = ifft(Gv_512_f,512);
  149 + Gu_512_t = ifft(Gu_512_f,512);
  150 + %second antenna
  151 + Gv_512_t_2 = ifft(Gv_512_f_2,512);
  152 + Gu_512_t_2 = ifft(Gu_512_f_2,512);
  153 +
  154 + %produce preamble
  155 + preamble_1 = [repmat(Ga_128_t, 1, 16) Ga_128_t_neg Gv_512_t Gu_512_t Gb_128_t_neg]; %preamble for Tx1
  156 + preamble_2 = [repmat(Ga_128_t, 1, 16) Ga_128_t_neg Gv_512_t_2 Gu_512_t_2 Gb_128_t_neg]; %preamble for Tx2
  157 +
  158 + %% Generate a payload of random integers
  159 + number_of_bits = (N_DATA_SYMS * MOD_ORDER - 2*trellis_end_length) * channel_coding;
  160 + tx_data = randi(2, 1, number_of_bits) - 1;
  161 +
  162 + % Forward Error Correction
  163 + tx_data = double([tx_data zeros(1,trellis_end_length) ]); % 8 bits padding
  164 + trel = poly2trellis(7, [171 133]); % Define trellis
  165 + tx_code = convenc(tx_data,trel); % convultional encoder
  166 +
  167 + % bits to signal space mapping these are your x_k from the class
  168 + tx_syms = mapping(tx_code', MOD_ORDER, 1);
  169 +
  170 + if Detail_plot
  171 + figure(1);
  172 + scatter(real(tx_syms), imag(tx_syms),'filled');
  173 + title(' Signal Space of transmitted bits');
  174 + xlabel('I'); ylabel('Q');
  175 + title('Tx data for 64QAM data set')
  176 + end
  177 +
  178 + % Reshape the symbol vector to a matrix with one column per OFDM symbol,
  179 + tx_syms_mat = reshape(tx_syms, length(SC_IND_DATA), N_OFDM_SYMS);
  180 +
  181 + % Define the pilot tone values as BPSK symbols
  182 + pilots = [-1, 1, -1, 1, 1, -1, -1, -1, -1, -1, 1, 1, 1, -1, 1, 1].';
  183 +
  184 + % Repeat the pilots across all OFDM symbols
  185 + pilots_mat = repmat(pilots, 1, N_OFDM_SYMS);
  186 +
  187 +
  188 + %% IFFT
  189 +
  190 + % Construct the IFFT input matrix
  191 + ifft_in_mat = zeros(N_SC, N_OFDM_SYMS);
  192 +
  193 + % Insert the data and pilot values; other subcarriers will remain at 0
  194 + ifft_in_mat(SC_IND_DATA, :) = tx_syms_mat;
  195 + ifft_in_mat(SC_IND_PILOTS, :) = pilots_mat;
  196 +
  197 + %Perform the IFFT --> frequency to time translation
  198 + tx_payload_mat = ifft(ifft_in_mat, N_SC, 1); %payload for Tx1
  199 + tx_payload_mat_2 = ifft(ifft_in_mat.*repmat(H_tx2',1,N_OFDM_SYMS), N_SC, 1); %payload for Tx2
  200 + % Insert the cyclic prefix
  201 + if(CP_LEN > 0)
  202 + tx_cp = tx_payload_mat((end-CP_LEN+1 : end), :);
  203 + tx_payload_mat = [tx_cp; tx_payload_mat];
  204 + tx_cp_2 = tx_payload_mat_2((end-CP_LEN+1 : end), :);
  205 + tx_payload_mat_2 = [tx_cp_2; tx_payload_mat_2];
  206 + end
  207 +
  208 + % Reshape to a vector
  209 + tx_payload_vec = reshape(tx_payload_mat, 1, numel(tx_payload_mat));
  210 + tx_payload_vec_2 = reshape(tx_payload_mat_2, 1, numel(tx_payload_mat_2));
  211 +
  212 + % Construct the full time-domain OFDM waveform
  213 + tx_vec = [preamble_1 tx_payload_vec];
  214 + tx_vec_2 = [preamble_2 tx_payload_vec_2];
  215 +
  216 + %% Interpolate,
  217 + if INTERPOLATE
  218 + % Interpolation filter basically implements the DAC before transmission
  219 + % On the receiver's end decimation is performed to implement the ADC
  220 +
  221 + % Define a half-band 2x interpolation filter response
  222 + interp_filt2 = zeros(1,43);
  223 + interp_filt2([1 3 5 7 9 11 13 15 17 19 21]) = [12 -32 72 -140 252 -422 682 -1086 1778 -3284 10364];
  224 + interp_filt2([23 25 27 29 31 33 35 37 39 41 43]) = interp_filt2(fliplr([1 3 5 7 9 11 13 15 17 19 21]));
  225 + interp_filt2(22) = 16384;
  226 + interp_filt2 = interp_filt2./max(abs(interp_filt2));
  227 +
  228 + % Pad with zeros for transmission to deal with delay through the interpolation filter
  229 + tx_vec_padded = [tx_vec, zeros(1, ceil(length(interp_filt2)/2))];
  230 + tx_vec_2x = zeros(1, 2*numel(tx_vec_padded));
  231 + tx_vec_2x(1:2:end) = tx_vec_padded;
  232 + tx_vec_air = filter(interp_filt2, 1, tx_vec_2x);
  233 +
  234 + if Detail_plot
  235 + figure(2);
  236 + plot(abs(tx_vec_2x));
  237 + hold on;
  238 + plot(abs(tx_vec_air(22:end)));
  239 + xlim([20,50]);
  240 + title('Interpolation visualized');
  241 + xlabel('time'); ylabel('amplitude');
  242 + legend('y = pre filtering','y = post filtering')
  243 + end
  244 + % Scale the Tx vector to +/- 1, becasue ADC and DAC take samples input from
  245 + % 1 to -1
  246 + tx_vec_air = TX_SCALE .* tx_vec_air ./ max(abs(tx_vec_air));
  247 +
  248 + if Detail_plot
  249 + figure(3);
  250 + plot(db(abs(fftshift(fft(tx_vec_air)))));
  251 + %plot(db(abs(fftshift(fft(tx_vec_2x)))));
  252 + %xlim([20000,60000]); ylim([0,65]);
  253 + % in this plot, why do see four peaks?
  254 + end
  255 + else
  256 + tx_vec_air = tx_vec;
  257 + tx_vec_air_2 = tx_vec_2;
  258 + end
  259 + %% This part of code is for simulating the wireless channel.
  260 +
  261 + %send to 2 transmitter:
  262 + tx1_vec_air = tx_vec_air;
  263 + tx2_vec_air = tx_vec_air_2;
  264 +
  265 + % AWGN:
  266 + TRIGGER_OFFSET_TOL_NS = 3000; % Trigger time offset toleration between Tx and Rx that can be accomodated
  267 + SAMP_FREQ = 1/sample_time; % Sampling frequency
  268 +
  269 + rx_vec_air_tx1 = [tx1_vec_air, zeros(1,ceil((TRIGGER_OFFSET_TOL_NS*1e-9) / (1/SAMP_FREQ)))];
  270 + rx_vec_air_tx2 = [tx2_vec_air, zeros(1,ceil((TRIGGER_OFFSET_TOL_NS*1e-9) / (1/SAMP_FREQ)))];
  271 +
  272 + %add noise
  273 + noise_power = var(rx_vec_air_tx1) * 10 ^(-snr/20);
  274 + rx_vec_air_tx1 = rx_vec_air_tx1 + noise_power*complex(randn(1,length(rx_vec_air_tx1)), randn(1,length(rx_vec_air_tx1)));
  275 +
  276 + noise_power = var(rx_vec_air_tx2) * 10 ^(-snr/20);
  277 + rx_vec_air_tx2 = rx_vec_air_tx2 + noise_power*complex(randn(1,length(rx_vec_air_tx2)), randn(1,length(rx_vec_air_tx2)));
  278 +
  279 + % Decimate %DAC in receiver
  280 + if INTERPOLATE
  281 + raw_rx_dec_tx1 = filter(interp_filt2, 1, rx_vec_air_tx1);
  282 + raw_rx_dec_tx1 = [zeros(1,DETECTION_OFFSET) raw_rx_dec_tx1(1:2:end)];
  283 +
  284 + raw_rx_dec_tx2 = filter(interp_filt2, 1, rx_vec_air_tx2);
  285 + raw_rx_dec_tx2 = [zeros(1,DETECTION_OFFSET) raw_rx_dec_tx2(1:2:end)];
  286 + else
  287 + raw_rx_dec_tx1 = rx_vec_air_tx1;
  288 + raw_rx_dec_tx2 = rx_vec_air_tx2;
  289 + end
  290 +
  291 + %create four paths and their delay, phase change, doppler
  292 + %calculating distance
  293 + tempa_1 = (-2*dis_ant-sqrt( 4*dis_ant^2 - 4*(1-v_c^2/v_ant^2)*(4*x_wall^2+dis_ant^2) ))/2/(1-v_c^2/v_ant^2); %precise
  294 + tempa_2 = (-2*dis_ant+sqrt( 4*dis_ant^2 - 4*(1-v_c^2/v_ant^2)*(4*x_wall^2+dis_ant^2) ))/2/(1-v_c^2/v_ant^2); %precise
  295 + distance_tx1wal = tempa_1*v_c/v_ant;
  296 + distance_tx1obj = sqrt(x_obj^2+(y_ant-dis_ant)^2)+sqrt(x_obj^2+(y_ant-tempa_2)^2);
  297 + distance_tx2wal = -tempa_2*v_c/v_ant;
  298 + distance_tx2obj = sqrt(x_obj^2+(y_ant+dis_ant)^2)+sqrt(x_obj^2+(y_ant-tempa_2)^2);
  299 +
  300 + %calculating correspoding delay
  301 + Delay_tx1wal = distance_tx1wal/v_c;
  302 + Delay_tx1obj = distance_tx1obj/v_c;
  303 + Delay_tx2wal = distance_tx2wal/v_c;
  304 + Delay_tx2obj = distance_tx2obj/v_c;
  305 +
  306 + %calculating channel condition(phase shift)
  307 + Phase_tx1wal = exp(1i*2*pi*distance_tx1wal*center_freq/v_c);
  308 + Phase_tx1obj = exp(1i*2*pi*distance_tx1obj*center_freq/v_c);
  309 + Phase_tx2wal = exp(1i*2*pi*distance_tx2wal*center_freq/v_c);
  310 + Phase_tx2obj = exp(1i*2*pi*distance_tx2obj*center_freq/v_c);
  311 +
  312 + %attenuation according to distance
  313 + Atten_tx1wal = 68.0630 + 20*log10(distance_tx1wal);%dB
  314 + Atten_tx1obj_wal = 400*( thick_wall*abs( y_ant/x_obj ) ) ;%dB
  315 + Atten_tx1obj_air = 68.0630 + 20*log10(distance_tx1obj-thick_wall*abs(y_ant/x_obj));%dB
  316 + Atten_tx2wal = 68.0630 + 20*log10(distance_tx2wal);%dB
  317 + Atten_tx2obj_wal = 400*( thick_wall*abs(y_ant/x_obj) ) ;%dB
  318 + Atten_tx2obj_air = 68.0630 + 20*log10(distance_tx2obj-thick_wall*abs(y_ant/x_obj));%dB
  319 +
  320 + %Doppler effect(Frequece shift)
  321 + theda_velo = atan(abs(v_ant/v_obj));
  322 + theda_grid = atan(abs(y_ant/x_obj));
  323 + theda_dopr = abs(theda_velo-theda_grid);
  324 + doppler_FO = center_freq* sqrt( v_ant^2 + v_obj^2 )* cos(theda_dopr)/v_c ;
  325 +
  326 + %create 4 signals for each path
  327 + raw_rx_dec_tx1wal = raw_rx_dec_tx1*Phase_tx1wal/sqrt( 10^(Atten_tx1wal/10) );
  328 + raw_rx_dec_tx1obj = raw_rx_dec_tx1*Phase_tx1obj/sqrt( 10^((Atten_tx1obj_wal+Atten_tx1obj_air)/10) ).* exp(-1i*2*pi*doppler_FO*sample_time*[0:length(raw_rx_dec_tx1)-1]);
  329 + raw_rx_dec_tx2wal = raw_rx_dec_tx2*Phase_tx2wal/sqrt( 10^(Atten_tx2wal/10) );
  330 + raw_rx_dec_tx2obj = raw_rx_dec_tx2*Phase_tx2obj/sqrt( 10^((Atten_tx2obj_wal+Atten_tx2obj_air)/10) ).* exp(-1i*2*pi*doppler_FO*sample_time*[0:length(raw_rx_dec_tx2)-1]);
  331 +
  332 + %conbine all 4 reflected signal in reciver
  333 + raw_rx_dec = 0;
  334 + if mod(t,est_pktwindow) ~= 2 %at step 2, there is no signal from TX1
  335 + if showdetail
  336 + fprintf('t = %d , Wall Reflc-signal from Tx1 with ', t );
  337 + end
  338 + raw_rx_dec = form_raw_rx_dec(1,raw_rx_dec,Delay_tx1wal,sample_time,raw_rx_dec_tx1wal,center_freq,showdetail); % refleted signal from tx 1 to wall to Rx
  339 + if showdetail
  340 + fprintf('t = %d , Objt Reflc-signal from Tx1 with ', t );
  341 + end
  342 + raw_rx_dec = form_raw_rx_dec(1,raw_rx_dec,Delay_tx1obj,sample_time,raw_rx_dec_tx1obj,center_freq,showdetail); % refleted signal from tx 1 to object to Rx
  343 + end
  344 + if mod(t,est_pktwindow) ~= 1 %at step 1, there is no signal from TX2
  345 + if showdetail
  346 + fprintf('t = %d , Wall Reflc-signal from Tx2 with ', t );
  347 + end
  348 + raw_rx_dec = form_raw_rx_dec(1,raw_rx_dec,Delay_tx2wal,sample_time,raw_rx_dec_tx2wal,center_freq,showdetail); % refleted signal from tx 2 to wall to Rx
  349 + if showdetail
  350 + fprintf('t = %d , Objt Reflc-signal from Tx2 with ', t );
  351 + end
  352 + raw_rx_dec = form_raw_rx_dec(1,raw_rx_dec,Delay_tx2obj,sample_time,raw_rx_dec_tx2obj,center_freq,showdetail);
  353 + end
  354 +
  355 + %% Reveiver Ends
  356 + if mod(t,est_pktwindow) == 1 || mod(t,est_pktwindow) == 2 %step 1 and step 2
  357 + %packet detection
  358 + lts_corr = abs(conv(conj(fliplr(Ga_128_t)), sign(raw_rx_dec)));
  359 + lts_corr = lts_corr(32:end-32);
  360 + LTS_CORR_THRESH=.8;
  361 + lts_peaks = find(lts_corr > LTS_CORR_THRESH*max(lts_corr));
  362 + [LTS1, LTS2] = meshgrid(lts_peaks,lts_peaks);
  363 + [lts_last_peak_index,y] = find(LTS2-LTS1 == length(Ga_128_t));
  364 +
  365 + channel_training_ind = lts_peaks(max(lts_last_peak_index)) + 128/2;
  366 + payload_ind = lts_peaks(max(lts_last_peak_index)) + 128/2 + 9*128;
  367 +
  368 + %CFO
  369 + Ga_128_t_start_ind = min(max( lts_peaks(max(lts_last_peak_index)) - 16.5*128,1),length(raw_rx_dec)-16.5*128);
  370 + rx_Ga_128_t = raw_rx_dec(Ga_128_t_start_ind:Ga_128_t_start_ind+16.5*128-1);
  371 + rx_cfo_est = zeros(1,15);
  372 + for iGa = 1:15
  373 + rx_Ga_128_1 = rx_Ga_128_t(1+(iGa-1)*128:iGa*128);
  374 + rx_Ga_128_2 = rx_Ga_128_t(1+iGa*128:(iGa+1)*128);
  375 + rx_cfo_est(1,iGa) = mean(unwrap(angle(rx_Ga_128_2 .* conj(rx_Ga_128_1))));
  376 + rx_cfo_est(1,iGa) = rx_cfo_est(1,iGa)/(2*pi*128);
  377 + end
  378 + rx_cfo_est_final = mean(rx_cfo_est);
  379 +
  380 + rx_cfo_corr_t = exp(-1i*2*pi*rx_cfo_est_final*[0:length(raw_rx_dec)-1]);
  381 + rx_dec_cfo_corr = raw_rx_dec .* rx_cfo_corr_t;
  382 +
  383 + % channel estimation:
  384 + Gv512_ind_start = channel_training_ind ;
  385 + Gv512_ind_end = channel_training_ind + 512 - 1;
  386 + Gu512_ind_start = channel_training_ind + 512;
  387 + Gu512_ind_end = channel_training_ind + 512*2 - 1;
  388 +
  389 + rx_Gv512 = raw_rx_dec(Gv512_ind_start:Gv512_ind_end);
  390 + rx_Gu512 = raw_rx_dec(Gu512_ind_start:Gu512_ind_end);
  391 +
  392 + rx_Gv512_f = fft(rx_Gv512);
  393 + rx_Gu512_f = fft(rx_Gu512);
  394 +
  395 + H_vest = rx_Gv512_f./Gv_512_f;
  396 + H_uest = rx_Gu512_f./Gu_512_f;
  397 + H_est = ( H_vest+H_uest )/2;
  398 + if mod(t,est_pktwindow) == 1
  399 + H_1 = H_est;
  400 + else
  401 + H_tx2 = - H_1./H_est;
  402 + end
  403 + lts_corr = abs(conv(conj(fliplr(Gu_512_t)), sign(raw_rx_dec)));
  404 + lts_corr = lts_corr(32:end-32);
  405 + LTS_CORR_THRESH=.8;
  406 + lts_peaks = find(lts_corr > LTS_CORR_THRESH*max(lts_corr));
  407 +
  408 + else %mod(t,num_avgpkt) == 0 || 3~6: step 3
  409 + %% start extract object information
  410 + %packet detection
  411 + corr_target = [ Gv_512_t, Gu_512_t];
  412 + lts_corr = abs(conv(conj(fliplr(corr_target)), sign(raw_rx_dec)));
  413 + lts_corr = lts_corr(32:end-32);
  414 + LTS_CORR_THRESH=.8;
  415 + [~,lts_peaks] = max(lts_corr);
  416 +
  417 + %ToF estimation
  418 + D_3 = lts_peaks-128*17-1024+32 ;
  419 + distance_avg(num_avgpkt_i) = D_3*sample_time*v_c/2;
  420 + %average smoothing
  421 + if num_avgpkt_i == 5
  422 + distance_esti(1,num_est) = mean(distance_avg) ;
  423 + end
  424 + if showdetail
  425 + disp(['OBJT delay estimation = ' num2str(D_3)]);
  426 + end
  427 +
  428 + %CFO estimation:
  429 + Ga_128_t_start_ind = min ( max(lts_peaks-1024 -128*17,1), length(raw_rx_dec) - 16*128 +1) ;
  430 + rx_Ga_128_t = raw_rx_dec(Ga_128_t_start_ind:Ga_128_t_start_ind+16*128-1);
  431 + rx_cfo_est = zeros(1,15);
  432 + for iGa = 1:15
  433 + rx_Ga_128_1 = rx_Ga_128_t(1+(iGa-1)*128:iGa*128);
  434 + rx_Ga_128_2 = rx_Ga_128_t(1+iGa*128:(iGa+1)*128);
  435 + rx_cfo_est(1,iGa) = mean(unwrap(angle(rx_Ga_128_2 .* conj(rx_Ga_128_1))));
  436 + rx_cfo_est(1,iGa) = rx_cfo_est(1,iGa)/(2*pi*128);
  437 + end
  438 + rx_cfo_est_final = mean(rx_cfo_est);
  439 + %antenna speed estimation
  440 + CFO_v_estimate = -rx_cfo_est_final/sample_time*v_c/center_freq;
  441 + if CFO_v_estimate-v_ant>100 || CFO_v_estimate<0
  442 + velo_avg(num_avgpkt_i) = ant_est_velo(1,max( num_est-1, 1));
  443 + else
  444 + velo_avg(num_avgpkt_i) = CFO_v_estimate;
  445 + end
  446 + %average smoothing
  447 + if num_avgpkt_i == 5
  448 + ant_est_velo(1,num_est) = mean(velo_avg) ;
  449 + end
  450 +
  451 + %derive h: function for theda estimation
  452 + rx_cfo_corr_t = exp(-1i*2*pi*rx_cfo_est_final*[2176-1:2176+length(corr_target)-2]);
  453 + Gv_gin_idx = min( max( lts_peaks-512, 1) , length(raw_rx_dec) -1024 );
  454 + rx_dec_cfo_corr = raw_rx_dec(Gv_gin_idx:Gv_gin_idx+1024-1).*rx_cfo_corr_t;
  455 +
  456 + %AoA estimation
  457 + v_est_walker = sqrt(100+v_obj^2); %m/s
  458 + lambda = v_c/center_freq; % wavelength
  459 + delta = v_est_walker * sample_time; % spatial separation between successive antennas in the array
  460 + % (twice the one-way separation to account for the round-trip time)
  461 + Length_window = 15;
  462 +
  463 + yy = rx_dec_cfo_corr;
  464 + xx = corr_target;
  465 + Length_N = length(yy);
  466 + hh = fft(yy)./fft(xx);
  467 + A = zeros(19, Length_N);
  468 + A_max_ind = zeros(1, Length_N);
  469 + A_temp = zeros(1, Length_N);
  470 + theta_est = zeros(1, Length_N);
  471 +
  472 + for n = 1:(Length_N-Length_window)
  473 +
  474 + for theta = (-pi/2):pi/18:(pi/2)
  475 +
  476 + theta_index = round((theta+pi/2)/(pi/18)+1);
  477 +
  478 + for i0 = 1:Length_window
  479 + A(theta_index,n) = A(theta_index,n) + hh(n+i0)*exp(1j*2*pi/lambda*i0*delta*sin(theta));
  480 + end
  481 +
  482 + end
  483 + [ A_temp(n), A_max_ind(n)] = max(abs(A(:,n)));
  484 +
  485 + if A_max_ind(n)==1||A_max_ind(n)==19
  486 + theta_est(n)=0;
  487 + else
  488 + theta_est(n) = (A_max_ind(n)-1)*(pi/18)-(pi/2);
  489 + end
  490 +
  491 + end
  492 +
  493 + theta_est_max = max(abs(theta_est));
  494 + alpha_real = atan(abs(y_ant)/(x_obj));
  495 + beta_real = atan(v_obj/v_ant);
  496 +
  497 + alpha_est = theta_est_max - beta_real;
  498 +
  499 + alpha_real_list(1,num_est) = alpha_real;
  500 + gridx_real(1,num_est) = x_obj;
  501 + gridy_real(1,num_est) = y_ant;
  502 + alpha_avg(num_avgpkt_i) = alpha_est;
  503 + if num_avgpkt_i == 5
  504 + alpha_est_list(1,num_est) = mean(alpha_avg) ;
  505 + end
  506 + num_avgpkt_i = num_avgpkt_i +1;
  507 + end
  508 + end%end of if
  509 + end%end of for
  510 + %% plot
  511 +
  512 + smooth_window = 20;
  513 + pkt_timex = 3:est_pktwindow:length(distance_esti)*est_pktwindow;
  514 + distance_esti_smooth = distance_esti;
  515 + alpha_est_list_smooth = alpha_est_list;
  516 + ant_est_velo_smooth = ant_est_velo;
  517 + for i = 1:length(distance_esti_smooth)
  518 + if distance_esti_smooth(i)>100
  519 + distance_esti_smooth(i) = distance_esti_smooth(max( i - 1, 1 ));
  520 + else
  521 + %distance_esti_smooth(i) = mean( distance_esti_smooth( max( i - smooth_window, 1 ) : i ) );
  522 + end
  523 + if abs(alpha_est_list_smooth(i)-alpha_est_list_smooth(max(i-1,1)))/alpha_est_list_smooth(max(i-1,1)) > 10
  524 + alpha_est_list_smooth(i) = mean( alpha_est_list_smooth( max( i - smooth_window, 1 ) : max( i - 1, 1 ) ) );
  525 + else
  526 + alpha_est_list_smooth(i) = mean( alpha_est_list_smooth( max( i - smooth_window, 1 ) : i ) );
  527 + end
  528 + if ant_est_velo_smooth(i) > 500
  529 + ant_est_velo_smooth(i) = ant_est_velo_smooth(max(i-1,1));
  530 + else
  531 + ant_est_velo_smooth(i) = mean( ant_est_velo_smooth( max(i-smooth_window,1): max( i - 1, 1 )));
  532 + end
  533 + end
  534 + ant_mov_ppkt_r = pck_interval*v_ant;
  535 + ant_mov_ppkt = pck_interval*ant_est_velo_smooth;%ant_est_veloc_record
  536 + obj_est_gridx = distance_esti_smooth.*cos(alpha_est_list_smooth);
  537 + %obj_est_gridy = distance_esti_smooth.*sin(alpha_est_list_smooth);
  538 + obj_est_gridy = distance_esti_smooth.*sin(alpha_est_list_smooth) + ant_mov_ppkt.*pkt_timex;
  539 + gridy_real_cali = abs( gridy_real ) + ant_mov_ppkt_r.*pkt_timex ;
  540 + c = linspace(1,10,length(obj_est_gridx));
  541 + K = ant_mov_ppkt.*pkt_timex;
  542 +
  543 + ant_est_veloc_record(snr_i,:) = ant_est_velo_smooth;
  544 + obj_est_gridx_record(snr_i,:) = obj_est_gridx;
  545 + obj_est_gridy_record(snr_i,:) = obj_est_gridy;
  546 + %x = x_ori_obj -
  547 + x = 1:length(distance_real);
  548 + figure(10+snr_i)
  549 + plot(x,distance_real,x,distance_esti_smooth);
  550 + title(['SNR = ' num2str(snr) ' db']);
  551 + xlabel('time(packet)');
  552 + ylabel('distance');
  553 + ylim([0,100]);
  554 + figure(20+snr_i)
  555 + scatter(obj_est_gridx,obj_est_gridy,[],c);
  556 + hold;
  557 + scatter(gridx_real,gridy_real_cali);
  558 + title(['SNR = ' num2str(snr) ' db']);
  559 + xlim([0,5]);
  560 + ylim([0,20]);
  561 + xlabel('m');
  562 + ylabel('m');
  563 + figure(30+snr_i)
  564 + title(['SNR = ' num2str(snr) ' db']);
  565 + plot(x,alpha_real_list,x,alpha_est_list);
  566 + xlabel('time(packet)');
  567 + ylabel('theda');
  568 +
  569 +end