simulation script
#29
by
Yujen Ku
mmwave_objectdetection.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
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