From b9d8be5b4b4f766f0c501457e8c01e61065a684a Mon Sep 17 00:00:00 2001 From: Yujen Ku Date: Fri, 15 Jun 2018 20:31:03 -0700 Subject: [PATCH] the new code --- mmwave_objectdetection.m | 570 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 570 insertions(+) diff --git a/mmwave_objectdetection.m b/mmwave_objectdetection.m index e69de29..1af0551 100644 --- a/mmwave_objectdetection.m +++ b/mmwave_objectdetection.m @@ -0,0 +1,570 @@ +clear; +alpha_est_list = []; + +raw_rx_dec_record = 0; +%% Params: + +%control param +Detail_plot = 0; +showdetail = 0; +DETECTION_OFFSET = 50; % to add packet detection error +INTERPOLATE = 0; + +%snr settings (run estimation under different snr values) +snr_value = [0]; %snr_value = [10,20,50,100,200,500,1000]; + +%how many cycles of packets you are going to measure(each cycle: 2 nulling estimation packets andd 5 object detecting packets) +total_num_est = 100; + +%how many packet used in one measurement. (not including 2 packets for nulling) +num_avgpkt = 5; + +%variables for estimated parameters +obj_est_gridx_record = zeros(length(snr_value),total_num_est); +obj_est_gridy_record = zeros(length(snr_value),total_num_est); +ant_est_veloc_record = zeros(length(snr_value),total_num_est); + + +for snr_i = 1:1:length(snr_value) + snr = snr_value(snr_i); + disp([ 'creating case snr = ' num2str(snr)]); +%environment param +% | || +% (0,0)+-------------------- <---v_obj -----o(x_obj) +% | || +% |< x_wall >|| +% | || +% | || +% | || +% ^ || +% | || +%(y_ant) car(ant) || + +%setting the initial position, velocity of object and antenna + x_obj = 3; %initial position of moving object(m) + x_ori_obj = x_obj; + y_ant = -10; %initial position of antenna(m) + v_ant = 10; %velocity of antenna(m/s) + v_obj = -1.5; %velocity of moving object(m/s) + x_wall = 1; %horizontal distance of wall to y axis (m) + thick_wall = 0.01; %2*thickness of wall (m) + +% __/ Tx +% +% __/ Rx +% +% __/ Tx + + dis_ant = 0.1; %distance between antennas + + % Waveform params + N_OFDM_SYMS = 500; % Number of OFDM symbols + MOD_ORDER = 2; % Modulation order in power of 2 (1/2/4/6 = BSPK/QPSK/16-QAM/64-QAM) + TX_SCALE = 1.0; % Scale for Tx waveform ([0:1]) + + % OFDM params + SC_IND_PILOTS = [-150,-130,-110,-90,-70,-50,-30,-10,10,30,50,70,90,110,130,150]+256; % Pilot subcarrier indices + 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 + N_SC = 512; % Number of subcarriers + CP_LEN = 128; % Cyclic prefix length + N_DATA_SYMS = N_OFDM_SYMS * length(SC_IND_DATA); % Number of data symbols (one per data-bearing subcarrier per OFDM symbol) + + channel_coding = .5; % coding rate + trellis_end_length = 8; % bits for trellis to end + + v_c = 299792458; %light speed (m/s) + SC_spacing = 5.165625e6; %sub-carrier spacing(Hz) + center_freq = 60e9; %center frequency + symbol_time = 1/SC_spacing; %duration of 1 OFDM symbol + sample_time = symbol_time/N_SC; %duration of 1 sample + + pck_interval = 200e-6; %interval between two packets + sample_interval = ceil(pck_interval/sample_time); %number of samples between two packets + + distance_real = zeros(1,total_num_est); %record the real distance of each measurement + distance_esti = zeros(1,total_num_est); %record the estimate distance of each measurement + ant_est_velo = zeros(1,total_num_est); %record the estimate antenna velocity of each measurement + ant_est_velo(1,1) = v_ant; %initial the estimation as the true speed + gridx_real = zeros(1,total_num_est); %record the real x position of the object + gridy_real = zeros(1,total_num_est); %record the real y position of the object + alpha_est_list = zeros(1,total_num_est); %record the estiamte AoA of each measurement + alpha_real_list = zeros(1,total_num_est); %record the real AoA of each measurement + est_pktwindow = 30; %do the measurement after every 'est_pktwindow' number of packets + num_est = 0; + + obj_mov_ppkt = pck_interval*v_obj; + for t = 1:total_num_est*est_pktwindow + if mod(t,est_pktwindow) == 0 + disp([ ' creating signal t = ' num2str(t)]); + end + + %update the start index of time, sample and position of object + time_start = (t-1)*pck_interval+1; + sample_start = (t-1)*sample_interval+1; + x_obj = x_obj+v_obj*pck_interval; %new x location of object + y_ant = y_ant+v_ant*pck_interval; %new y location of antenna + + %periodically measurement + if mod(t,est_pktwindow)>( 2+num_avgpkt) || mod(t,est_pktwindow) == 0 + %do nothing + else + + if mod(t,est_pktwindow) == 3 + num_est = num_est+1; + distance_real(1,num_est) = sqrt(x_obj^2+y_ant^2); + end + %% Preamble + if mod(t,est_pktwindow) == 1 + H_tx2 = ones(1,N_SC); + distance_avg = zeros(1,num_avgpkt); + alpha_avg = zeros(1,num_avgpkt); + velo_avg = zeros(1,num_avgpkt); + num_avgpkt_i = 1; + end + % reference [802.11ad PHY] + + 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,... + -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,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,-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]; + 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,... + 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,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,-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]; + Ga_128_t = ifft(Ga_128_f_1,128); + Ga_128_t_neg = ifft(-1*Ga_128_f_1,128); + Gb_128_t = ifft(Gb_128_f_1,128); + Gb_128_t_neg = ifft(-1*Gb_128_f_1,128); + + Gv_512_f = [-Gb_128_f_1, Ga_128_f_1, -Gb_128_f_1, -Ga_128_f_1]; + Gu_512_f = [-Gb_128_f_1, -Ga_128_f_1, Gb_128_f_1, -Ga_128_f_1]; + + %Nulling for preamble + Gv_512_f_2 = H_tx2.*[-Gb_128_f_1, Ga_128_f_1, -Gb_128_f_1, -Ga_128_f_1]; + Gu_512_f_2 = H_tx2.*[-Gb_128_f_1, -Ga_128_f_1, Gb_128_f_1, -Ga_128_f_1]; + + %Fourier transform of preamble from freq domain to time domain + %first antenna + Gv_512_t = ifft(Gv_512_f,512); + Gu_512_t = ifft(Gu_512_f,512); + %second antenna + Gv_512_t_2 = ifft(Gv_512_f_2,512); + Gu_512_t_2 = ifft(Gu_512_f_2,512); + + %produce preamble + 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 + 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 + + %% Generate a payload of random integers + number_of_bits = (N_DATA_SYMS * MOD_ORDER - 2*trellis_end_length) * channel_coding; + tx_data = randi(2, 1, number_of_bits) - 1; + + % Forward Error Correction + tx_data = double([tx_data zeros(1,trellis_end_length) ]); % 8 bits padding + trel = poly2trellis(7, [171 133]); % Define trellis + tx_code = convenc(tx_data,trel); % convultional encoder + + % bits to signal space mapping these are your x_k from the class + tx_syms = mapping(tx_code', MOD_ORDER, 1); + + if Detail_plot + figure(1); + scatter(real(tx_syms), imag(tx_syms),'filled'); + title(' Signal Space of transmitted bits'); + xlabel('I'); ylabel('Q'); + title('Tx data for 64QAM data set') + end + + % Reshape the symbol vector to a matrix with one column per OFDM symbol, + tx_syms_mat = reshape(tx_syms, length(SC_IND_DATA), N_OFDM_SYMS); + + % Define the pilot tone values as BPSK symbols + pilots = [-1, 1, -1, 1, 1, -1, -1, -1, -1, -1, 1, 1, 1, -1, 1, 1].'; + + % Repeat the pilots across all OFDM symbols + pilots_mat = repmat(pilots, 1, N_OFDM_SYMS); + + + %% IFFT + + % Construct the IFFT input matrix + ifft_in_mat = zeros(N_SC, N_OFDM_SYMS); + + % Insert the data and pilot values; other subcarriers will remain at 0 + ifft_in_mat(SC_IND_DATA, :) = tx_syms_mat; + ifft_in_mat(SC_IND_PILOTS, :) = pilots_mat; + + %Perform the IFFT --> frequency to time translation + tx_payload_mat = ifft(ifft_in_mat, N_SC, 1); %payload for Tx1 + tx_payload_mat_2 = ifft(ifft_in_mat.*repmat(H_tx2',1,N_OFDM_SYMS), N_SC, 1); %payload for Tx2 + % Insert the cyclic prefix + if(CP_LEN > 0) + tx_cp = tx_payload_mat((end-CP_LEN+1 : end), :); + tx_payload_mat = [tx_cp; tx_payload_mat]; + tx_cp_2 = tx_payload_mat_2((end-CP_LEN+1 : end), :); + tx_payload_mat_2 = [tx_cp_2; tx_payload_mat_2]; + end + + % Reshape to a vector + tx_payload_vec = reshape(tx_payload_mat, 1, numel(tx_payload_mat)); + tx_payload_vec_2 = reshape(tx_payload_mat_2, 1, numel(tx_payload_mat_2)); + + % Construct the full time-domain OFDM waveform + tx_vec = [preamble_1 tx_payload_vec]; + tx_vec_2 = [preamble_2 tx_payload_vec_2]; + + %% Interpolate, + if INTERPOLATE + % Interpolation filter basically implements the DAC before transmission + % On the receiver's end decimation is performed to implement the ADC + + % Define a half-band 2x interpolation filter response + interp_filt2 = zeros(1,43); + interp_filt2([1 3 5 7 9 11 13 15 17 19 21]) = [12 -32 72 -140 252 -422 682 -1086 1778 -3284 10364]; + 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])); + interp_filt2(22) = 16384; + interp_filt2 = interp_filt2./max(abs(interp_filt2)); + + % Pad with zeros for transmission to deal with delay through the interpolation filter + tx_vec_padded = [tx_vec, zeros(1, ceil(length(interp_filt2)/2))]; + tx_vec_2x = zeros(1, 2*numel(tx_vec_padded)); + tx_vec_2x(1:2:end) = tx_vec_padded; + tx_vec_air = filter(interp_filt2, 1, tx_vec_2x); + + if Detail_plot + figure(2); + plot(abs(tx_vec_2x)); + hold on; + plot(abs(tx_vec_air(22:end))); + xlim([20,50]); + title('Interpolation visualized'); + xlabel('time'); ylabel('amplitude'); + legend('y = pre filtering','y = post filtering') + end + % Scale the Tx vector to +/- 1, becasue ADC and DAC take samples input from + % 1 to -1 + tx_vec_air = TX_SCALE .* tx_vec_air ./ max(abs(tx_vec_air)); + + if Detail_plot + figure(3); + plot(db(abs(fftshift(fft(tx_vec_air))))); + %plot(db(abs(fftshift(fft(tx_vec_2x))))); + %xlim([20000,60000]); ylim([0,65]); + % in this plot, why do see four peaks? + end + else + tx_vec_air = tx_vec; + tx_vec_air_2 = tx_vec_2; + end + %% This part of code is for simulating the wireless channel. + + %send to 2 transmitter: + tx1_vec_air = tx_vec_air; + tx2_vec_air = tx_vec_air_2; + + % AWGN: + TRIGGER_OFFSET_TOL_NS = 3000; % Trigger time offset toleration between Tx and Rx that can be accomodated + SAMP_FREQ = 1/sample_time; % Sampling frequency + + rx_vec_air_tx1 = [tx1_vec_air, zeros(1,ceil((TRIGGER_OFFSET_TOL_NS*1e-9) / (1/SAMP_FREQ)))]; + rx_vec_air_tx2 = [tx2_vec_air, zeros(1,ceil((TRIGGER_OFFSET_TOL_NS*1e-9) / (1/SAMP_FREQ)))]; + + %add noise + noise_power = var(rx_vec_air_tx1) * 10 ^(-snr/20); + 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))); + + noise_power = var(rx_vec_air_tx2) * 10 ^(-snr/20); + 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))); + + % Decimate %DAC in receiver + if INTERPOLATE + raw_rx_dec_tx1 = filter(interp_filt2, 1, rx_vec_air_tx1); + raw_rx_dec_tx1 = [zeros(1,DETECTION_OFFSET) raw_rx_dec_tx1(1:2:end)]; + + raw_rx_dec_tx2 = filter(interp_filt2, 1, rx_vec_air_tx2); + raw_rx_dec_tx2 = [zeros(1,DETECTION_OFFSET) raw_rx_dec_tx2(1:2:end)]; + else + raw_rx_dec_tx1 = rx_vec_air_tx1; + raw_rx_dec_tx2 = rx_vec_air_tx2; + end + + %create four paths and their delay, phase change, doppler + %calculating distance + 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 + 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 + distance_tx1wal = tempa_1*v_c/v_ant; + distance_tx1obj = sqrt(x_obj^2+(y_ant-dis_ant)^2)+sqrt(x_obj^2+(y_ant-tempa_2)^2); + distance_tx2wal = -tempa_2*v_c/v_ant; + distance_tx2obj = sqrt(x_obj^2+(y_ant+dis_ant)^2)+sqrt(x_obj^2+(y_ant-tempa_2)^2); + + %calculating correspoding delay + Delay_tx1wal = distance_tx1wal/v_c; + Delay_tx1obj = distance_tx1obj/v_c; + Delay_tx2wal = distance_tx2wal/v_c; + Delay_tx2obj = distance_tx2obj/v_c; + + %calculating channel condition(phase shift) + Phase_tx1wal = exp(1i*2*pi*distance_tx1wal*center_freq/v_c); + Phase_tx1obj = exp(1i*2*pi*distance_tx1obj*center_freq/v_c); + Phase_tx2wal = exp(1i*2*pi*distance_tx2wal*center_freq/v_c); + Phase_tx2obj = exp(1i*2*pi*distance_tx2obj*center_freq/v_c); + + %attenuation according to distance + Atten_tx1wal = 68.0630 + 20*log10(distance_tx1wal);%dB + Atten_tx1obj_wal = 400*( thick_wall*abs( y_ant/x_obj ) ) ;%dB + Atten_tx1obj_air = 68.0630 + 20*log10(distance_tx1obj-thick_wall*abs(y_ant/x_obj));%dB + Atten_tx2wal = 68.0630 + 20*log10(distance_tx2wal);%dB + Atten_tx2obj_wal = 400*( thick_wall*abs(y_ant/x_obj) ) ;%dB + Atten_tx2obj_air = 68.0630 + 20*log10(distance_tx2obj-thick_wall*abs(y_ant/x_obj));%dB + + %Doppler effect(Frequece shift) + theda_velo = atan(abs(v_ant/v_obj)); + theda_grid = atan(abs(y_ant/x_obj)); + theda_dopr = abs(theda_velo-theda_grid); + doppler_FO = center_freq* sqrt( v_ant^2 + v_obj^2 )* cos(theda_dopr)/v_c ; + + %create 4 signals for each path + raw_rx_dec_tx1wal = raw_rx_dec_tx1*Phase_tx1wal/sqrt( 10^(Atten_tx1wal/10) ); + 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]); + raw_rx_dec_tx2wal = raw_rx_dec_tx2*Phase_tx2wal/sqrt( 10^(Atten_tx2wal/10) ); + 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]); + + %conbine all 4 reflected signal in reciver + raw_rx_dec = 0; + if mod(t,est_pktwindow) ~= 2 %at step 2, there is no signal from TX1 + if showdetail + fprintf('t = %d , Wall Reflc-signal from Tx1 with ', t ); + end + 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 + if showdetail + fprintf('t = %d , Objt Reflc-signal from Tx1 with ', t ); + end + 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 + end + if mod(t,est_pktwindow) ~= 1 %at step 1, there is no signal from TX2 + if showdetail + fprintf('t = %d , Wall Reflc-signal from Tx2 with ', t ); + end + 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 + if showdetail + fprintf('t = %d , Objt Reflc-signal from Tx2 with ', t ); + end + raw_rx_dec = form_raw_rx_dec(1,raw_rx_dec,Delay_tx2obj,sample_time,raw_rx_dec_tx2obj,center_freq,showdetail); + end + + %% Reveiver Ends + if mod(t,est_pktwindow) == 1 || mod(t,est_pktwindow) == 2 %step 1 and step 2 + %packet detection + lts_corr = abs(conv(conj(fliplr(Ga_128_t)), sign(raw_rx_dec))); + lts_corr = lts_corr(32:end-32); + LTS_CORR_THRESH=.8; + lts_peaks = find(lts_corr > LTS_CORR_THRESH*max(lts_corr)); + [LTS1, LTS2] = meshgrid(lts_peaks,lts_peaks); + [lts_last_peak_index,y] = find(LTS2-LTS1 == length(Ga_128_t)); + + channel_training_ind = lts_peaks(max(lts_last_peak_index)) + 128/2; + payload_ind = lts_peaks(max(lts_last_peak_index)) + 128/2 + 9*128; + + %CFO + 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); + rx_Ga_128_t = raw_rx_dec(Ga_128_t_start_ind:Ga_128_t_start_ind+16.5*128-1); + rx_cfo_est = zeros(1,15); + for iGa = 1:15 + rx_Ga_128_1 = rx_Ga_128_t(1+(iGa-1)*128:iGa*128); + rx_Ga_128_2 = rx_Ga_128_t(1+iGa*128:(iGa+1)*128); + rx_cfo_est(1,iGa) = mean(unwrap(angle(rx_Ga_128_2 .* conj(rx_Ga_128_1)))); + rx_cfo_est(1,iGa) = rx_cfo_est(1,iGa)/(2*pi*128); + end + rx_cfo_est_final = mean(rx_cfo_est); + + rx_cfo_corr_t = exp(-1i*2*pi*rx_cfo_est_final*[0:length(raw_rx_dec)-1]); + rx_dec_cfo_corr = raw_rx_dec .* rx_cfo_corr_t; + + % channel estimation: + Gv512_ind_start = channel_training_ind ; + Gv512_ind_end = channel_training_ind + 512 - 1; + Gu512_ind_start = channel_training_ind + 512; + Gu512_ind_end = channel_training_ind + 512*2 - 1; + + rx_Gv512 = raw_rx_dec(Gv512_ind_start:Gv512_ind_end); + rx_Gu512 = raw_rx_dec(Gu512_ind_start:Gu512_ind_end); + + rx_Gv512_f = fft(rx_Gv512); + rx_Gu512_f = fft(rx_Gu512); + + H_vest = rx_Gv512_f./Gv_512_f; + H_uest = rx_Gu512_f./Gu_512_f; + H_est = ( H_vest+H_uest )/2; + if mod(t,est_pktwindow) == 1 + H_1 = H_est; + else + H_tx2 = - H_1./H_est; + end + lts_corr = abs(conv(conj(fliplr(Gu_512_t)), sign(raw_rx_dec))); + lts_corr = lts_corr(32:end-32); + LTS_CORR_THRESH=.8; + lts_peaks = find(lts_corr > LTS_CORR_THRESH*max(lts_corr)); + + else %mod(t,num_avgpkt) == 0 || 3~6: step 3 + %% start extract object information + %packet detection + corr_target = [ Gv_512_t, Gu_512_t]; + lts_corr = abs(conv(conj(fliplr(corr_target)), sign(raw_rx_dec))); + lts_corr = lts_corr(32:end-32); + LTS_CORR_THRESH=.8; + [~,lts_peaks] = max(lts_corr); + + %ToF estimation + D_3 = lts_peaks-128*17-1024+32 ; + distance_avg(num_avgpkt_i) = D_3*sample_time*v_c/2; + %average smoothing + if num_avgpkt_i == 5 + distance_esti(1,num_est) = mean(distance_avg) ; + end + if showdetail + disp(['OBJT delay estimation = ' num2str(D_3)]); + end + + %CFO estimation: + Ga_128_t_start_ind = min ( max(lts_peaks-1024 -128*17,1), length(raw_rx_dec) - 16*128 +1) ; + rx_Ga_128_t = raw_rx_dec(Ga_128_t_start_ind:Ga_128_t_start_ind+16*128-1); + rx_cfo_est = zeros(1,15); + for iGa = 1:15 + rx_Ga_128_1 = rx_Ga_128_t(1+(iGa-1)*128:iGa*128); + rx_Ga_128_2 = rx_Ga_128_t(1+iGa*128:(iGa+1)*128); + rx_cfo_est(1,iGa) = mean(unwrap(angle(rx_Ga_128_2 .* conj(rx_Ga_128_1)))); + rx_cfo_est(1,iGa) = rx_cfo_est(1,iGa)/(2*pi*128); + end + rx_cfo_est_final = mean(rx_cfo_est); + %antenna speed estimation + CFO_v_estimate = -rx_cfo_est_final/sample_time*v_c/center_freq; + if CFO_v_estimate-v_ant>100 || CFO_v_estimate<0 + velo_avg(num_avgpkt_i) = ant_est_velo(1,max( num_est-1, 1)); + else + velo_avg(num_avgpkt_i) = CFO_v_estimate; + end + %average smoothing + if num_avgpkt_i == 5 + ant_est_velo(1,num_est) = mean(velo_avg) ; + end + + %derive h: function for theda estimation + rx_cfo_corr_t = exp(-1i*2*pi*rx_cfo_est_final*[2176-1:2176+length(corr_target)-2]); + Gv_gin_idx = min( max( lts_peaks-512, 1) , length(raw_rx_dec) -1024 ); + rx_dec_cfo_corr = raw_rx_dec(Gv_gin_idx:Gv_gin_idx+1024-1).*rx_cfo_corr_t; + + %AoA estimation + v_est_walker = sqrt(100+v_obj^2); %m/s + lambda = v_c/center_freq; % wavelength + delta = v_est_walker * sample_time; % spatial separation between successive antennas in the array + % (twice the one-way separation to account for the round-trip time) + Length_window = 15; + + yy = rx_dec_cfo_corr; + xx = corr_target; + Length_N = length(yy); + hh = fft(yy)./fft(xx); + A = zeros(19, Length_N); + A_max_ind = zeros(1, Length_N); + A_temp = zeros(1, Length_N); + theta_est = zeros(1, Length_N); + + for n = 1:(Length_N-Length_window) + + for theta = (-pi/2):pi/18:(pi/2) + + theta_index = round((theta+pi/2)/(pi/18)+1); + + for i0 = 1:Length_window + A(theta_index,n) = A(theta_index,n) + hh(n+i0)*exp(1j*2*pi/lambda*i0*delta*sin(theta)); + end + + end + [ A_temp(n), A_max_ind(n)] = max(abs(A(:,n))); + + if A_max_ind(n)==1||A_max_ind(n)==19 + theta_est(n)=0; + else + theta_est(n) = (A_max_ind(n)-1)*(pi/18)-(pi/2); + end + + end + + theta_est_max = max(abs(theta_est)); + alpha_real = atan(abs(y_ant)/(x_obj)); + beta_real = atan(v_obj/v_ant); + + alpha_est = theta_est_max - beta_real; + + alpha_real_list(1,num_est) = alpha_real; + gridx_real(1,num_est) = x_obj; + gridy_real(1,num_est) = y_ant; + alpha_avg(num_avgpkt_i) = alpha_est; + if num_avgpkt_i == 5 + alpha_est_list(1,num_est) = mean(alpha_avg) ; + end + num_avgpkt_i = num_avgpkt_i +1; + end + end%end of if + end%end of for + %% plot + + smooth_window = 20; + pkt_timex = 3:est_pktwindow:length(distance_esti)*est_pktwindow; + distance_esti_smooth = distance_esti; + alpha_est_list_smooth = alpha_est_list; + ant_est_velo_smooth = ant_est_velo; + for i = 1:length(distance_esti_smooth) + if distance_esti_smooth(i)>100 + distance_esti_smooth(i) = distance_esti_smooth(max( i - 1, 1 )); + else + %distance_esti_smooth(i) = mean( distance_esti_smooth( max( i - smooth_window, 1 ) : i ) ); + end + if abs(alpha_est_list_smooth(i)-alpha_est_list_smooth(max(i-1,1)))/alpha_est_list_smooth(max(i-1,1)) > 10 + alpha_est_list_smooth(i) = mean( alpha_est_list_smooth( max( i - smooth_window, 1 ) : max( i - 1, 1 ) ) ); + else + alpha_est_list_smooth(i) = mean( alpha_est_list_smooth( max( i - smooth_window, 1 ) : i ) ); + end + if ant_est_velo_smooth(i) > 500 + ant_est_velo_smooth(i) = ant_est_velo_smooth(max(i-1,1)); + else + ant_est_velo_smooth(i) = mean( ant_est_velo_smooth( max(i-smooth_window,1): max( i - 1, 1 ))); + end + end + ant_mov_ppkt_r = pck_interval*v_ant; + ant_mov_ppkt = pck_interval*ant_est_velo_smooth;%ant_est_veloc_record + obj_est_gridx = distance_esti_smooth.*cos(alpha_est_list_smooth); + %obj_est_gridy = distance_esti_smooth.*sin(alpha_est_list_smooth); + obj_est_gridy = distance_esti_smooth.*sin(alpha_est_list_smooth) + ant_mov_ppkt.*pkt_timex; + gridy_real_cali = abs( gridy_real ) + ant_mov_ppkt_r.*pkt_timex ; + c = linspace(1,10,length(obj_est_gridx)); + K = ant_mov_ppkt.*pkt_timex; + + ant_est_veloc_record(snr_i,:) = ant_est_velo_smooth; + obj_est_gridx_record(snr_i,:) = obj_est_gridx; + obj_est_gridy_record(snr_i,:) = obj_est_gridy; + %x = x_ori_obj - + x = 1:length(distance_real); + figure(10+snr_i) + plot(x,distance_real,x,distance_esti_smooth); + title(['SNR = ' num2str(snr) ' db']); + xlabel('time(packet)'); + ylabel('distance'); + ylim([0,100]); + figure(20+snr_i) + scatter(obj_est_gridx,obj_est_gridy,[],c); + hold; + scatter(gridx_real,gridy_real_cali); + title(['SNR = ' num2str(snr) ' db']); + xlim([0,5]); + ylim([0,20]); + xlabel('m'); + ylabel('m'); + figure(30+snr_i) + title(['SNR = ' num2str(snr) ' db']); + plot(x,alpha_real_list,x,alpha_est_list); + xlabel('time(packet)'); + ylabel('theda'); + +end + -- 1.9.1