% MUSIC AOA estimation for a M = 6 element array with noise variance = .1
% use time averages instead of expected values by assuming ergodicity of the mean and 
% ergodicity of the correlation.


M=6;
D = 2;  % number of signals
sig2=.1;
th1=-5*pi/180;
th2=5*pi/180;

a1=[1];
a2=[1];
for i=2:M
    a1=[a1 exp(-1j*i*pi*sin(th1))];
    a2=[a2 exp(-1j*i*pi*sin(th2))];
end
A=[a1.' a2.'];

K=100;          % K = length of time samples
s=sign(randn(2,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(6,K);     % calculate the K time samples of the noise for the 6 array
                            % elements
                    
Rnn=(n*n')/K;         % calculate the noise correlation matrix (which is no longer diagonal)
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

[E,Dia]=eig(Rrr);
[Y,Index]=sort(diag(Dia));   % sorts the eigenvalues from least to greatest
 EN=E(:,Index(1:M-D));      % calculate the noise subspace matrix of eigenvectors
                           % using the sorting done in the previous line
for k=1:180;
   th(k)=-pi/6+pi*k/(3*180);
   clear a
a=[1];
   for jj=2:M
      a = [a exp(-1j*jj*pi*sin(th(k)))];
   end
  a=a.';
P(k)=1/abs(a'*EN*EN'*a); 
end
figure;
plot(th*180/pi,10*log10(P/max(P)),'k')
grid on
xlabel('Angle')
ylabel('|P(\theta)|')
axis([-30 30 -30 10])