% root-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.
% First solve the regular MUSIC problem, calculate polynomial coefficients for root-MUSIC, 
%  and plot comparing the two methods.
%  
% randn('state',0)

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

ii=1:M;
  a1=exp(1j*(ii-1)*pi*sin(th1));
  a2=exp(1j*(ii-1)*pi*sin(th2));
A=[a1.' a2.'];
K=300;          % K = length of time samples
K=600
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 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

[V,Dia]=eig(Rrr);
[Y,Index]=sort(diag(Dia));   % sorts the eigenvalues from least to greatest
 EN=V(:,Index(1:M-D));      % calculate the noise subspace matrix of eigenvectors
                           % using the sorting done in the previous line
 ES=V(:,Index(M-D+1:M));       % subspace of signal eigenvectors                
for k=1:360;
   %th(k)=-pi/12+pi*k/(6*360);
    th(k)=-pi/6+pi*k/(3*360);
   clear a
    a=exp(1j*(ii-1)*pi*sin(th(k)));
    a=a.';
P(k)=1/abs(a'*EN*EN'*a); 
end

C=EN*EN';           % calculate the matrix C

%  Find the coefficients for the root-MUSIC polynomial
for kk=-M+1:M-1
    cc(kk+M) = sum(diag(C,kk));
end
rts=roots(cc);                      % find the roots of the 2*(M-1) polynomial

angs=-asin(angle(rts)/pi)*180/pi     % find the angles associated with these root
figure;zplane(rts)
figure;
plot(th*180/pi,P/max(P),'k',angs,abs(rts),'kX','markersize',10)
grid on
xlabel('Angle')
ylabel('|P(\theta)|')
axis([-10 10 0 1.6])
axis([-30 30 0 1.6])