function P_Azimuth_Elevation_f %========空間二維到達角及頻率的估計 2008.4.6 by xray==========
                                   %========采用均勻平行線陣并用ESPRIT算法對到達角進行估計=
clc;
clear;
close all;
tic
M=8;                                  %陣元數目
N=2;                                  %信源數目
c=1500;                               %波速度
f=30000;
lamda=c/f;                            %波長
d=0.5*lamda;                          %陣元間距
snap=100;                             %快拍數目
tao=1/5/f
format short;
fs=100;
ts=1/fs;
t=(0:snap-1)*ts;
%======================================================================
theta_in=[40,30,0.3;150,60,0.1]; %輸入信號
X1=X_Signal(t,M,N,snap,0.5,theta_in,20,f,0,0);
X2=X_Signal(t,M,N,snap,0.5,theta_in,20,f,1,0);
Y1=Y_Signal(t,M,N,snap,0.5,theta_in,20,f,0);
Y2=Y_Signal(t,M,N,snap,0.5,theta_in,20,f,1);
Xt=X_Signal(t,M,N,snap,0.5,theta_in,20,f,0,tao);
   
X1t=[X1;Xt];
Rxt=X1t*X1t'/length(t);
a=[0:M-1]';
[Ut,R1t]=eig(Rxt);
 for iii=1:2*M
    bt(iii)=abs(R1t(iii,iii));
 end
   [cct,et]=sort(bt);
     for iii=1:2*M-N
         Unt(:,iii)=Ut(:,et(iii));
     end;
         for iii=1:N
             Ust(:,iii)=Ut(:,et(2*M-N+iii));
         end;
            Uxts1=Ust(1:M,:);
             Uxts2=Ust(M+1:2*M,:);
   
    X=[X1;X2];
     Rx=X*X'/length(t);
      a=[0:M-1]';
      [U,R1]=eig(Rx);
 for iii=1:2*M
    b(iii)=abs(R1(iii,iii));
 end
   [cc,e]=sort(b);
     for iii=1:2*M-N
         Un(:,iii)=U(:,e(iii));
     end;
         for iii=1:N
             Us(:,iii)=U(:,e(2*M-N+iii));
         end;
            Uxs1=Us(1:M,:);
             Uxs2=Us(M+1:2*M,:);
             
              Y=[X1;Y1];
               Ry=Y*Y'/length(t);
                a=[0:M-1]';
                 [U,R2]=eig(Ry);
 for iii=1:2*M
     b(iii)=abs(R2(iii,iii));
 end
  [cc,e]=sort(b);
    for iii=1:2*M-N
        Un(:,iii)=U(:,e(iii));
    end;
     for iii=1:N
         Us(:,iii)=U(:,e(2*M-N+iii));
     end;
Uys1=Us(1:M,:);
 Uys2=Us(M+1:2*M,:);
 
  Fre=inv(Uxts1'*Uxts1)*Uxts1'*Uxts2;
  at=eig(Fre)
  Frequency=angle(at)/2/pi/f/tao
  Fei=inv(Uxs1'*Uxs1)*Uxs1'*Uxs2;
   ax=eig(Fei)
    Fei=inv(Uys1'*Uys1)*Uys1'*Uys2;
     ay=eig(Fei)
      theta=atan(angle(ay)./angle(ax))*180/pi;
         fei=acos(1/pi*sqrt(angle(ax).^2+angle(ay).^2)./Frequency)*180/pi;
         disp('實際信號入射方向角分別為');
          disp(theta_in);
           disp('信號入射俯仰角度分別為');
            disp([theta,fei,Frequency]);
             
toc
return
% %========================================信號源====均勻平行線陣====================
% %M 陣元數 ,N信源數 ,snap快拍數 ,R陣元間距與波長比 ,theta 信源方位角  ,SNR 信噪比
function out=X_Signal(t,M,N,snap,R,theta,SNR,f0,Type,tao)   
 a=[0:(M-1)]';
for ii=1:N
 S(ii,:)=exp(j*2*pi*(f0*t+0.5*2^(ii-1)*t.^2));
end
 for i=1:N
     if Type==0 & tao==0
   A(:,i)=exp(j*2*pi*R*theta(i,3)*(a+Type)*cos(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)+j*2*pi*f0*theta(i,3)*tao);
else
   A(:,i)=i*exp(j*2*pi*R*theta(i,3)*(a+Type)*cos(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)+j*2*pi*f0*theta(i,3)*tao);
 end 
 end
  X0=A*S;
   randn('state',0);
    real_noise=randn(size(X0));
     randn('state',3);
      imag_noise=randn(size(X0));
       noise0=(real_noise+j*imag_noise)/2^0.5;
         noise=10^(-SNR/20)*noise0;
    out=X0+noise;
return

function out=Y_Signal(t,M,N,snap,R,theta,SNR,f0,Type)
 a=[0:(M-1)]';
for ii=1:N
  S(ii,:)=exp(j*2*pi*(f0*t+0.5*2^(ii-1)*t.^2));
end
 for i=1:N
     if Type==0
   A(:,i)=i*exp(j*2*pi*R*theta(i,3)*((a+Type)*cos(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)+sin(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)));
else
   A(:,i)=i*exp(j*2*pi*R*theta(i,3)*((a+Type)*cos(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)+sin(theta(i,1)/180*pi)*cos(theta(i,2)/180*pi)));
 end 
 end
  X0=A*S;
   randn('state',0);
    real_noise=randn(size(X0));
     randn('state',3);
      imag_noise=randn(size(X0));
       noise0=(real_noise+j*imag_noise)/2^0.5;
         noise=10^(-SNR/20)*noise0;
    out=X0+noise;
return