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 = 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'); 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