% ---------------------------------------------------------------------------------------------------
% TOPS method 
% ---------------------------------------------------------------------------------------------------

clear all

Nsensor=12;
Radius=0.5; %注意這里Radius僅僅是與中心頻率f0對應(yīng)波長的比值
DOA=[10 14 40];
Nsignal=length(DOA);
SNR=13*[1 1 1];
snapshot=20;

fl=200;
f0=300;
fh=400;
Fs=1000;
bw=fh-fl;

Nfft=128;%選取每段的時域數(shù)據(jù)長度，即dft長度
N=Nfft*snapshot;%產(chǎn)生時域信號的總點數(shù)
Pn=0.01;%因為噪聲肯定一樣，所以要令其固定，來確定不同信號源的功率
Ps=Pn*10 .^(SNR/10);
% f_focus=[];
% X_focus=[];
% result=0;

%下面產(chǎn)生高斯白噪聲，并且低通濾波得到寬帶信號，存放在st中
ss=randn(N,Nsignal)*diag(sqrt(Ps));
st=[];
[bb,aa]=butter(10,[fl/(Fs/2),fh/(Fs/2)]);
for r=1:Nsignal
    temp=filter(bb,aa,ss(:,r));
    st=[st,temp];
end

%產(chǎn)生信號的功率譜
window=[];%求功率譜加的窗
noverlap=[];
[Pxx,f]=psd(ss(:,1),Nfft,Fs,window,noverlap);
figure,plot(f,Pxx);
[Pyy,f]=psd(st(:,1),Nfft,Fs,window,noverlap);
figure,plot(f,Pyy);



J1=round(Nfft*fl/Fs+1);%最低頻率fl對應(yīng)DFT的第J1個點
J2=round(Nfft*fh/Fs+1); 
J=J2-J1+1;%選用的頻率點數(shù)目
fll=(J1-1)*Fs/Nfft;%第J1個點對應(yīng)的模擬頻率
fhh=(J2-1)*Fs/Nfft;%第J2個點對應(yīng)的模擬頻率
subband=linspace(fll,fhh,J);%各個子帶頻點

Xfallsnapshot=zeros(Nsensor,J,snapshot);%用于存放多次快拍頻域數(shù)據(jù)，Xfallsnapshot為3維矩陣


for number=1:snapshot%快拍循環(huán)開始
    
    Sf=fft(st((number-1)*Nfft+1:number*Nfft,:));
    %fft是按列進(jìn)行的，每一列都變換，對應(yīng)各個信號
    %Sf的規(guī)模為（Nfft，Nsignal）
    

    Sf=Sf(J1:J2,:);%將Sf頻率分量為零的部分去掉，保留規(guī)模為（J，Nsignal）
    
    %產(chǎn)生出復(fù)噪聲
    randn('state',sum(100*clock));
    nn=sqrt(Pn/2)*randn(Nsensor,J);
    randn('state',sum(100*clock));
    nn=nn+j*sqrt(Pn/2)*randn(Nsensor,J);
    % nn=randn(Nsensor,J);
    
    %下面是把白噪聲變成色噪聲，仍然存在nn中
    % nt=[];
    % for rr=1:Nsensor
    %     temp=filter(bb,aa,nn(rr,:));%濾波函數(shù)filter對于行或者列數(shù)據(jù)都可以，且還保持原來的規(guī)模，即行仍為行，列仍為列
    %     nt=[nt;temp];
    % end
    % nn=nt;
    
    Nf=fft(conj(nn'));%因為fft對列進(jìn)行，而這里需要對nt的各行進(jìn)行fft，所以先對nt進(jìn)行轉(zhuǎn)置，fft后再轉(zhuǎn)置回來
    Nf=conj(Nf');%Nf的規(guī)模為(Nsensor,J)
    
    %下面建立模型得到頻域接受數(shù)據(jù)表達(dá)式，存放在Afsf中
    Afsf=zeros(Nsensor,J);
    for m=1:Nsignal
        a=steerMultiFre(Nsensor,Radius,DOA(m),f0,subband);%a為規(guī)模為（Nsensor，J）
        Sf1=repmat(conj(Sf(:,m))',Nsensor,1);%將每一個信號變成(Nsensor,J)的規(guī)模
        as1=a.*Sf1;
        Afsf=Afsf+as1; %各個信號累加得到頻域信號數(shù)據(jù)  
    end
    % Xf=Afsf;
    Xf=Afsf+Nf;%接收到的頻域數(shù)據(jù)
    Xfallsnapshot(:,:,number)=Xf;
    
end%快拍循環(huán)完畢




%下面是用TOPS法進(jìn)行DOA估計
X0=Xfallsnapshot(:,(J/2),:);
X0=squeeze(X0);%此函數(shù)將維數(shù)為1的去掉，得到單一頻率點(近似f0)處多次快拍頻域接收數(shù)據(jù)矩陣
[W0,F0]=Dmatrix(X0,Nsensor,Nsignal);%對多次快拍用music方法，形參X0為矩陣，每一列為一次快拍，返回噪聲和信號子空間


theta=-90:0.1:90;
result=zeros(1,length(theta));
for m=1:length(theta)
    phi=theta(m);
    
    D=[];
    for n=1:J-1
        X=Xfallsnapshot(:,n,:);
        X=squeeze(X);%此函數(shù)將維數(shù)為1的去掉，得到單一頻率點的多次快拍頻域接收數(shù)據(jù)矩陣
        ff=subband(n);%進(jìn)行估計的頻率點
        [W,F]=Dmatrix(X,Nsensor,Nsignal);
        
        Psensor=(0:1:Nsensor-1)*Radius;
        Q=exp(-j*2*pi*Psensor*(ff-f0)/f0*sin(phi*pi/180));
        Q=diag(Q);
        U=Q*F0;
        a3=exp(-j*2*pi*Psensor'*ff/f0*sin(phi*pi/180));
                
        P=eye(Nsensor)-1/(a3'*a3)*a3*a3';
%         P=eye(Nsensor);
        U=P*U;
        UW=U'*W;
        D=[D,UW];
    end
    [UU,SS,VV] = svd(D);
    SS=diag(SS);
    result(m)=1/min(SS);   %取最小的奇異值的倒數(shù)來度量是不是真實DOA方向
end

result=result/max(result);%歸一化
figure,
plot(theta,result,'k');
grid on;
plotxline(DOA,'m','--')
% result=10*log10(result);
% figure,
% plot(theta,result,'k');
% grid on;