273 lines
9.8 KiB
Matlab
273 lines
9.8 KiB
Matlab
function AOA(RawData)
|
||
RawData = permute(RawData,[2 1 3 4]);
|
||
%% Radar Parameters
|
||
N_Chan = 3;
|
||
N_adc = 32;
|
||
N_Chirp = 64;
|
||
Fs = 2e6;
|
||
|
||
f_start = 58.4771; % ADC采样段起始频率58.4771，调频起始频率57.5,Unit: GHz
|
||
f_stop = 63.4997; % ADC采样段结束频率63.4997，调频结束频率63.5,Unit: GHz
|
||
% c0 = physconst('LightSpeed'); % speed of light in m/s 2.99817e8
|
||
% % fc = (57.5 + 63.5)*1e9/2; % center frequency
|
||
% fc = (f_start + f_stop)/2 * 1e9;
|
||
% lamta = c0/fc;
|
||
% d_res = c0/2/B;
|
||
|
||
B = (f_stop - f_start) * 1e9; % Radar bandwidth in Hz
|
||
Tadc = N_adc/Fs; % chirp time in seconds
|
||
Ks = B/Tadc; % slope rate in Hz/s;
|
||
c0 = 2.99817e8; % speed of light im m/s;
|
||
fc = (56.5 + 64.5)*1e9/2; % maximum center frequency
|
||
lamta_max = c0 / fc; % maximum wavelength
|
||
rxSpace = 0.5*lamta_max; % spacing between the receivers
|
||
d_max = c0 * Fs / (2 * Ks);
|
||
Zeropad = N_adc*1; % Zeropadding to multiple of N where N*4 is good choice
|
||
xdata = linspace(0,1-1/Zeropad,Zeropad)*d_max; % scaling the x-axis to proper range to frequency
|
||
xdata1= 1:1:N_adc;
|
||
|
||
Hann_window = hann(N_adc,'periodic'); % hann(Length of window, 'periodic');
|
||
ScaleHannWin = 1/sum(Hann_window); % replaces the scaling of the fft with length of spectrum with sum(window)
|
||
Threshold_value = -60;
|
||
|
||
%%% Suppress Warnings
|
||
id = 'signal:findpeaks:largeMinPeakHeight';
|
||
warning('off',id);
|
||
|
||
%% start Angle of Arrival estimation
|
||
% starting the while-loop where the process of AoA is computed and
|
||
% collecting raw_data from the board
|
||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
||
N_Frame = size(RawData,4);
|
||
%% 参数初始化
|
||
history_Rx1 = zeros(N_adc,N_Chirp);
|
||
history_Rx2 = zeros(N_adc,N_Chirp);
|
||
history_Rx3 = zeros(N_adc,N_Chirp);
|
||
%% AOA
|
||
aoa_H_fram = zeros(1,N_Chirp*N_Frame);
|
||
aoa_V_fram = zeros(1,N_Chirp*N_Frame);
|
||
|
||
for kk = 1 : N_Chirp % trigger chirp and collect raw data for specific number of chirps
|
||
back = squeeze(RawData(:,:,kk,1)); % fram
|
||
history_Rx1(:,kk) = back(:,1);
|
||
history_Rx2(:,kk) = back(:,2);
|
||
history_Rx3(:,kk) = back(:,3);
|
||
end
|
||
|
||
for fram = 2 : N_Frame
|
||
for chir = 1 %: N_Chirp
|
||
|
||
recent = squeeze(RawData(:,:,chir,fram));
|
||
|
||
%% Static clutter Suppression (CIS)
|
||
c(:,1) = recent(:,1) - mean(history_Rx1,2);
|
||
c(:,2) = recent(:,2) - mean(history_Rx2,2);
|
||
c(:,3) = recent(:,3) - mean(history_Rx3,2);
|
||
|
||
%% plot most recent chirp data and chirp data with removed clutter
|
||
% figure(1)
|
||
% subplot(2,2,1)
|
||
% plot(xdata1,abs(recent(:,1)).','r',xdata1,abs(recent(:,3)).','b',...
|
||
% xdata1,abs(c(:,1)).','g',xdata1,abs(c(:,3)).','m');
|
||
% legend('Rx1','Rx3','calibrated Rx1','calibrated Rx3')
|
||
% grid on
|
||
% title('Time domain data')
|
||
% xlabel('Number of samples')
|
||
% ylim([0,1])
|
||
|
||
figure(1)
|
||
subplot(221)
|
||
plot(xdata1,abs(recent(:,1)).','r', ...
|
||
xdata1,abs(recent(:,2)).','g', ...
|
||
xdata1,abs(recent(:,3)).','b')
|
||
legend('Rx1','Rx2','Rx3')
|
||
title('时域信号')
|
||
xlabel('采样数')
|
||
% subplot(222)
|
||
% plot(xdata1,abs(c(:,1)).','r', ...
|
||
% xdata1,abs(c(:,2)).','g', ...
|
||
% xdata1,abs(c(:,3)).','b')
|
||
% ylim([0 0.03])
|
||
% legend('calibrated Rx1','calibrated Rx2','calibrated Rx3')
|
||
% title('时域信号')
|
||
% xlabel('采样数')
|
||
|
||
|
||
Nbut=4; % Removal of low frequency components
|
||
Wn=0.05; % 0.05
|
||
[b,a]=butter(Nbut,Wn,'high');
|
||
|
||
data1 = filter(b,a,c(:,1).');
|
||
data2 = filter(b,a,c(:,2).');
|
||
data3 = filter(b,a,c(:,3).');
|
||
% data1 = c(:,1).';
|
||
% data2 = c(:,2).';
|
||
% data3 = c(:,3).';
|
||
|
||
% subplot(133)
|
||
% plot(xdata1,abs(data1).','g',xdata1,abs(data3).','m')
|
||
% legend('filtered Rx1','filtered Rx3')
|
||
|
||
subplot(222)
|
||
plot(xdata1,abs(data1),'r', ...
|
||
xdata1,abs(data2),'g', ...
|
||
xdata1,abs(data3),'b')
|
||
ylim([0 0.03])
|
||
legend('calibrated Rx1','calibrated Rx2','calibrated Rx3')
|
||
title('时域信号')
|
||
xlabel('采样数')
|
||
|
||
%% Prepare AoA
|
||
% Computing the fast-fourier-transform of the raw data and finding any
|
||
% peaks for a region of data, here the data has to be in a range of 0.1 to
|
||
% 1.5. Peaks are than used for determing range and signal strength of
|
||
% object
|
||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
||
FFT_dat1 = fft(data1.*Hann_window.',Zeropad);
|
||
scaled_FFT1 = db(abs(FFT_dat1(1:length(xdata))*1*ScaleHannWin));
|
||
|
||
FFT_dat2=fft(data2.*Hann_window.',Zeropad);
|
||
scaled_FFT2=db(abs(FFT_dat2(1:length(xdata))*1*ScaleHannWin));
|
||
|
||
FFT_dat3=fft(data3.*Hann_window.',Zeropad);
|
||
scaled_FFT3=db(abs(FFT_dat3(1:length(xdata))*1*ScaleHannWin));
|
||
|
||
IF_13=[FFT_dat1.' FFT_dat3.']; % Create a matrix with two receivers
|
||
IF_23=[FFT_dat2.' FFT_dat3.']; % Create a matrix with two receivers
|
||
|
||
xmin = 0.1; % Minimum range value
|
||
xmax = 0.3; % Maximum range value 1.5
|
||
region_of_interest = xmax>xdata & xdata>xmin; % Desired range value for peak detection
|
||
start_bin=find(region_of_interest,1)-1;
|
||
[rvPks,rvLocs] = findpeaks(scaled_FFT1(region_of_interest),...
|
||
'MINPEAKHEIGHT',Threshold_value,'MINPEAKDISTANCE',3);%15
|
||
[Tx_scPeakVal,scInd] = max(rvPks);
|
||
TxscPeakBin=rvLocs(scInd);
|
||
iter=length(TxscPeakBin);
|
||
|
||
|
||
if iter>0
|
||
|
||
Desired_bin = start_bin+TxscPeakBin; % Desired bin
|
||
target_range= xdata(Desired_bin); % Target range value
|
||
|
||
%% Angle of Arrival estimation
|
||
% AoA is determined by taking the phased difference between the two
|
||
% receivers and calculating the AoA:
|
||
% AoA = arcsin((delta_phi/2pi)*(max. wavelength/spacing))
|
||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
||
rvPh = unwrap(angle(IF_13(Desired_bin,:)),[]); % get phase information from the desired target bin
|
||
% rvPh = angle(IF_13(Desired_bin,:));
|
||
rvPhDelt = diff(rvPh); % get phase difference
|
||
scAlphSin = (rvPhDelt/(2*pi))*(lamta_max/rxSpace); % Angle of arrival formaula
|
||
aoa_H = asind(scAlphSin); % Angle value in degrees
|
||
|
||
rvPh = unwrap(angle(IF_23(Desired_bin,:)),[]); % get phase information from the desired target bin
|
||
% rvPh = angle(IF_12(Desired_bin,:));
|
||
rvPhDelt = diff(rvPh); % get phase difference
|
||
scAlphSin = (rvPhDelt/(2*pi))*(lamta_max/rxSpace); % Angle of arrival formaula
|
||
aoa_V = asind(scAlphSin); % Angle value in degrees
|
||
%% Plot the range and angle values
|
||
% Finishing with the last 2 plots, the range and angle values and the
|
||
% the frequency spectrum of the raw data plot
|
||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||
|
||
subplot(2,2,3)
|
||
plot(target_range,aoa_H,'Marker','o',...
|
||
'MarkerSize',10,'MarkerFaceColor','blue','MarkerEdgeColor','green')
|
||
xlim([xmin,xmax]) %max(xdata)/2
|
||
ylim([-70,70])
|
||
grid on;
|
||
legend(num2str(aoa_H));
|
||
xlabel('目标距离 - m')
|
||
ylabel('方位角 - °')
|
||
title('距离角度图')
|
||
|
||
% frequency spectrum plot
|
||
subplot(2,2,4) % [3,4]
|
||
plot(xdata,scaled_FFT1);
|
||
hline = refline([0 Threshold_value]);
|
||
hline.Color = 'r';
|
||
caption = sprintf('目标位于 %f m' , target_range); % Make legend caption for this curve
|
||
txt1= num2str(caption);
|
||
text(target_range,scaled_FFT1(Desired_bin),txt1)
|
||
grid on
|
||
ylim([-120,-10])
|
||
xlabel('目标距离 - m')
|
||
ylabel('功率谱 - dBm')
|
||
title('频域信号')
|
||
xlim([0.1,max(xdata)/2])
|
||
drawnow
|
||
|
||
else
|
||
aoa_H=0;
|
||
aoa_V=0;
|
||
range=0;
|
||
|
||
% subplot(2,2,3)
|
||
% plot(range,aoa_H,'Marker','o','MarkerSize',...
|
||
% 10,'MarkerFaceColor','blue','MarkerEdgeColor','green');
|
||
% legend('no target')
|
||
% xlabel('Target range in m')
|
||
% ylabel('Azimuthal angle in degrees')
|
||
% title('Range angle map')
|
||
% % xlim([0.1,max(xdata)/2])
|
||
% xlim([xmin,xmax])
|
||
% ylim([-70,70])
|
||
% grid on;
|
||
%
|
||
% subplot(2,2,4) % [3,4]
|
||
% plot(xdata,scaled_FFT1);
|
||
% hline = refline([0 Threshold_value]);
|
||
% hline.Color = 'r';
|
||
% grid on
|
||
% ylim([-120,-10])
|
||
% xlabel('Target range in m')
|
||
% ylabel('Power spectrum in dbm')
|
||
% title('Frequency domain signal')
|
||
% xlim([0.1,max(xdata)/2])
|
||
% drawnow
|
||
end
|
||
|
||
% aoa_H_fram(fram) = aoa_H;
|
||
% aoa_V_fram(fram) = aoa_V;
|
||
chir_ind = (fram-1)*N_Chirp + chir;
|
||
aoa_H_fram(chir_ind) = aoa_H;
|
||
aoa_V_fram(chir_ind) = aoa_V;
|
||
|
||
% clear temp variables
|
||
rvLocs=[];
|
||
rvPks=[];
|
||
target_range=[];
|
||
Desired_bin=[];
|
||
rvPh = [] ;
|
||
rvPhDelt = [];
|
||
scAlphSin = [];
|
||
aoa_H =[];
|
||
|
||
% pause(0.5)
|
||
end % chirps END
|
||
% if fram > 1
|
||
% aoa_jump = aoa_H_fram((fram-1)*N_Chirp + 1) - aoa_H_fram((fram-1)*N_Chirp);
|
||
% if aoa_jump ~= 0
|
||
% aoa_H_fram((fram-1)*N_Chirp + 1 : fram*N_Chirp) = ...
|
||
% aoa_H_fram((fram-1)*N_Chirp + 1 : fram*N_Chirp) - aoa_jump;
|
||
% end
|
||
% end
|
||
|
||
end
|
||
|
||
%% 绘制角度随时间变化
|
||
figure(2)
|
||
% subplot(2,2,[1,2])
|
||
plot(1: N_Chirp*N_Frame,aoa_H_fram,'r',1: N_Chirp*N_Frame,aoa_V_fram,'g')
|
||
legend('方位角','仰角')
|
||
axis([1 N_Chirp*N_Frame -70 70])
|
||
xlabel('帧数')
|
||
ylabel('角度/度')
|
||
% hold on
|
||
% pause(3)
|