AOA
常用的角度估计方法（已测试）
This commit is contained in:
Nicole
272
AOA/AOA.m
Normal file
272
AOA/AOA.m
Normal file
@@ -0,0 +1,272 @@
|
||||
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)
|
||||
Reference in New Issue
Block a user