yemai/Algorithm
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Files
d567035746b499b51b494fbeebc26d9cca6db9b0
Algorithm/AOA/AOA.m

273 lines
9.8 KiB
Mathematica
Raw Normal View History

AOA 常用的角度估计方法（已测试）
2020-09-06 15:57:26 +08:00
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<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼƵ<EFBFBD><EFBFBD>58.4771<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>ʼƵ<EFBFBD><EFBFBD>57.5,Unit: GHz
f_stop = 63.4997; % ADC<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ν<EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>63.4997<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>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);
%% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD>
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('ʱ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>')
xlabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>')
% 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('ʱ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>')
% xlabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>')
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('ʱ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>')
xlabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>')
%% 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('Ŀ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> - m')
ylabel('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> - <EFBFBD><EFBFBD>')
title('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>ͼ')
% frequency spectrum plot
subplot(2,2,4) % [3,4]
plot(xdata,scaled_FFT1);
hline = refline([0 Threshold_value]);
hline.Color = 'r';
caption = sprintf('Ŀ<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD> %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('Ŀ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> - m')
ylabel('<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> - dBm')
title('Ƶ<EFBFBD><EFBFBD><EFBFBD>ź<EFBFBD>')
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
%% <EFBFBD><EFBFBD><EFBFBD>ƽǶ<EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD>仯
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('<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>','<EFBFBD><EFBFBD><EFBFBD><EFBFBD>')
axis([1 N_Chirp*N_Frame -70 70])
xlabel('֡<EFBFBD><EFBFBD>')
ylabel('<EFBFBD>Ƕ<EFBFBD>/<EFBFBD><EFBFBD>')
% hold on
% pause(3)
Reference in New Issue Copy Permalink