% root-Min-Norm 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.
% 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=.3;
th1=-2*pi/180;
th2=4*pi/180;
temp=eye(M);
u1=temp(:,1);
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=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 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
            
for k=1:360;
   th(k)=-pi/12+pi*k/(6*360);
   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'*u1)^2; 
end
c1=(EN*EN')*u1;
C=c1*c1';           % 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])