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 Inline 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