DSB最简单DOA估计算法

哪位大神帮忙看一下错误在哪里？60度的入射角度，结果总是不对。
%==========================================================================
%   1. close all
%==========================================================================
clc;
clear all;
close all;
%==========================================================================
%   1. 
%==========================================================================
fs=16e3;
d=0.035;
c=340;
angle=60;
Ts=1/fs;
M=6;
% construct wav
tao=d*cos(60*pi/180)/c;
f0=50;
n=1:2000;
y1=sin(2*pi*f0*n*Ts);% reference
y2=sin(2*pi*f0*n*Ts-2*pi*f0*tao);
y3=sin(2*pi*f0*n*Ts-2*pi*f0*2*tao);
y4=sin(2*pi*f0*n*Ts-2*pi*f0*3*tao);
y5=sin(2*pi*f0*n*Ts-2*pi*f0*4*tao);
y6=sin(2*pi*f0*n*Ts-2*pi*f0*5*tao);
nfft=2048;
% fft
fft1=fft(y1,nfft);
fft2=fft(y2,nfft);
fft3=fft(y3,nfft);
fft4=fft(y4,nfft);
fft5=fft(y5,nfft);
fft6=fft(y6,nfft);
fft_res=[fft1;fft2;fft3;fft4;fft5;fft6];
theta=[0:1:180]*pi/180;

% 生成导向矢量
tmp_vec=zeros(nfft/2+1,M,length(theta));
for k=1:(nfft/2+1)
    for m=1:M
      tmp_vec(k,m,:)=exp(j*2*pi*k*fs/nfft*(m-1)*d*cos(theta)/c);
    end
end

% 与频域信号相乘
tmp_res=zeros(nfft/2+1,length(theta));
for k=1:nfft/2+1
    for ang=1:length(theta)
        tmp_res(k,ang)=abs(fft_res(:,k).'* tmp_vec(k,:,ang).')^2;% 计算能量
    end
end

[a b]=max(tmp_res(7,:)) % 找最大的位置b