% ESPRIT AOA estimation for a M = 4 element array with noise variance = .1
% use time averages instead of expected values by assuming ergodicity of the mean and 
% ergodicity of the correlation.
%  
% randn('state',0)

M=4;    % number of array elements
D = 2;  % number of signals
sig1=1; % signal amplitude
sig2=.1;    % noise variance
th1=-5*pi/180;      % angle of arrival 1
th2=10*pi/180;       % angle of arrival 2
ii=1:M;
 a1=exp(1j*(ii-1)*pi*sin(th1));
 a2=exp(1j*(ii-1)*pi*sin(th2));
A=[a1.' a2.'];          % calculate matrix of steering vectors for the D signals
K=300;                 % K = length of time samples
s=sqrt(sig1)*sign(randn(D,K));      % calculate the K time samples of the signals for the 
                                    % two arriving directions
                    
Rss=s*s'/K;      % source correlation matrix with uncorrelated signals
n=sqrt(sig2)*randn(M,K);     % calculate the K time samples of the noise for the M array
                            % elements
Rnn=(n*n')/K;
Rns=(n*s')/K;           % calculate the noise/signal correlation matrix
Rsn=(s*n')/K;           %  calculate the signal/noise correlation matrix
Rrr=A*Rss*A'+A*Rsn+Rns*A'+Rnn;        % combine all to get the array correlation matrix
%Rss=[1 0;0 1];
%Rrr=A*Rss*A'+sig2*eye(M);      % correlation matrix if we assume that noise is uncorrelated with signals
%Rrr=A*A'+sig2*eye(M);      % correlation matrix if we assume that noise is uncorrelated with signals
  
[V,Dia]=eig(Rrr);           % find eigenvalues and eigenvectors of Rrr
[Y,Index]=sort(diag(Dia));   % sorts the eigenvalues from least to greatest
ES=V(:,Index(M-D+1:M));      % calculate the signal subspace matrix of eigenvectors
                              % using the sorting done in the previous line
          
E1=ES(1:M/2+1,:);           % construct the signal subspace form the first K rows of ES (K = M/2+1)
E2=ES(M/2:M,:);             % construct the signal subspace from the last K rows of ES
C=[E1';E2']*[E1 E2];
[EC,tmp]=eig(C);            %Perform the eigendecomposition on the matrix C
E12=EC(1:2,3:4);            % construct the E12 submatrix
E22=EC(3:4,3:4);            % construct the E22 submatrix
PSI=-E12*inv(E22);          % estimate PSI, the rotation operator
[Vp,Ep]=eig(PSI);           % find the eigenvalues and eigenvectors of rotation operator PSI
e=diag(Ep);

angs=asin(angle(e)/pi)*180/pi     % find the angles associated with the roots


for k=1:180;
   th(k)=-pi/6+pi*k/(3*180);
   a =exp(1j*(ii-1)*pi*sin(th(k)));
   a=a.';
  VN=eye(M)-A*Ep*A';          % construct the noise subspace which is perpindicular matrix of steering vectors
P1(k)=abs(1./(a'*VN*VN'*a)^2); 
P2(k)=abs((a'*A*Ep*A'*a)^2);
end
figure; 
plot(th*180/pi,log10(P1/max(P1)),'k',th*180/pi,log10(P2/max(P2)),'k:')      % plot pseudospectrum for ESPRIT
grid on
xlabel('Angle')
ylabel('|P(\theta)|')
axis([-30 30 -20 5])