%矢量水聽器頻率細化方位估計——利用增大nfft點數方法

clc
clear all

%參數初始化
parameter_initial;

for T=1:time_length
    
    %矢量水聽器信號模擬(遠場條件，球面波衰減)
    [p,vx,vy]=vector_locomotion(x_a,y_a,target_x,target_y,target_vx,target_vy,T,r_0,len,fs,f_0,An,SNR);
    %改變fft點數
    nfft=4096*10;
    win=hanning(nfft); %窗函數
    ind_up=round(frequence_up/(fs/nfft));	%頻率下限下標
    ind_down=round(frequence_down/(fs/nfft));	%頻率上限下標
    %計算功率譜
	[sp,f]=psd(p,nfft,fs,win);
	[svx,f]=psd(vx,nfft,fs,win);
	[svy,f]=psd(vy,nfft,fs,win);
	[spvx,f]=csd(p,vx,nfft,fs,win);
	[spvy,f]=csd(p,vy,nfft,fs,win);
    %聲強方法方位估計
    [amplitude_theta,theta,result]=azi_estimate_max(sp,svx,svy,spvx,spvy,nfft,fs,ind_up,ind_down);
    theta1_temp(T)=result;

    
    %改變fft點數
    nfft=4096;
    win=hanning(nfft); %窗函數
    ind_up=round(frequence_up/(fs/nfft));	%頻率下限下標
    ind_down=round(frequence_down/(fs/nfft));	%頻率上限下標
    %計算功率譜
	[sp,f]=psd(p,nfft,fs,win);
	[svx,f]=psd(vx,nfft,fs,win);
	[svy,f]=psd(vy,nfft,fs,win);
	[spvx,f]=csd(p,vx,nfft,fs,win);
	[spvy,f]=csd(p,vy,nfft,fs,win);
    %聲強方法方位估計
    [amplitude_theta,theta,result]=azi_estimate_max(sp,svx,svy,spvx,spvy,nfft,fs,ind_up,ind_down);
    theta2_temp(T)=result;

    %改變fft點數
    nfft=1024;
    win=hanning(nfft); %窗函數
    ind_up=round(frequence_up/(fs/nfft));	%頻率下限下標
    ind_down=round(frequence_down/(fs/nfft));	%頻率上限下標
    %計算功率譜
	[sp,f]=psd(p,nfft,fs,win);
	[svx,f]=psd(vx,nfft,fs,win);
	[svy,f]=psd(vy,nfft,fs,win);
	[spvx,f]=csd(p,vx,nfft,fs,win);
	[spvy,f]=csd(p,vy,nfft,fs,win);
    %聲強方法方位估計
    [amplitude_theta,theta,result]=azi_estimate_max(sp,svx,svy,spvx,spvy,nfft,fs,ind_up,ind_down);
    theta3_temp(T)=result;

    %改變fft點數
    nfft=512;
    win=hanning(nfft); %窗函數
    ind_up=round(frequence_up/(fs/nfft));	%頻率下限下標
    ind_down=round(frequence_down/(fs/nfft));	%頻率上限下標
    %計算功率譜
	[sp,f]=psd(p,nfft,fs,win);
	[svx,f]=psd(vx,nfft,fs,win);
	[svy,f]=psd(vy,nfft,fs,win);
	[spvx,f]=csd(p,vx,nfft,fs,win);
	[spvy,f]=csd(p,vy,nfft,fs,win);
    %聲強方法方位估計
    [amplitude_theta,theta,result]=azi_estimate_max(sp,svx,svy,spvx,spvy,nfft,fs,ind_up,ind_down);
    theta4_temp(T)=result;

T

end;
error_1=abs(theta1_temp-45);
error_2=abs(theta2_temp-45);
error_3=abs(theta3_temp-45);
error_4=abs(theta4_temp-45);
mean(error_1)
mean(error_2)
mean(error_3)
mean(error_4)

std(theta1_temp-45);
std(theta2_temp-45);
std(theta3_temp-45);
std(theta4_temp-45);


figure(1)
plot((1:time_length),theta1_temp,'b',...
    (1:time_length),theta2_temp,'r',...
    (1:time_length),theta3_temp,'y');
title('角度檢測結果');
xlabel('x軸方位（m）');
ylabel('y軸方位（m）');

figure(2)
plot((1:time_length),error_1,'b',...
    (1:time_length),error_2,'r',...
    (1:time_length),error_3,'y');
title('角度檢測誤差');
xlabel('x軸方位（m）');
ylabel('y軸方位（m）');