%%%%%%%%%%%%%%%%%%%%%%%  simulatation of the Pisarenko method          %%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%%%%%%%%%%%%%%  using ESPRIT algorithms  P156:3.18       %%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%          initiatation        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
n=1:1280;                        %the length of data,it is larger than N,because of Rx
number=1000;                     %the iteration number
for pp=1:number
v1=sqrt(20)*sin(2*pi*0.2*n);   
v2=sqrt(2)*sin(2*pi*0.213*n);
w=randn(1,10000);
x=v1+v2+w(1:length(n));                %generate obervated data, which is in added gaussian white noise

m=30;                       %the number of the receivers
p=4; 
rx=xcorr(x,m,'unbiased');
for i=1:m                      % self-correlative matrix Rxx, and mutual correlative matrix Rxy
    Rxx(i,:)=rx(m+2-i:2*m+1-i);
end
for i=1:m
    Rxy(i,:)=rx(m+3-i:2*(m+1)-i);
end

%%%%%%%%%%%%%% the first method (in the textbook) %%%%%%%%%%%%%%%%%%%%
% for i=1:m                    % generate the special matrix Z
%     for j=1:m
%         if (i-j==1)
%             Z(i,j)=1;
%         else
%             Z(i,j)=0;
%         end
%     end
% end
%     
% 
 eigenvalue=eig(Rxx);        
 sigma=mean(eigenvalue(p+1:m));      % calculate the variance of white noise
% Cxx=Rxx-sigma*eye(m);
% Cxy=Rxy-mean(eigenvalue(p+1:m))*Z;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%  the second method (improved) %%%%%%%%%%%%%%%%%%%%%%%%%
 [U,S]=eig(Rxx);
 for i=1:m                     % 經過特征值分解后，令后m-p個對角元素的值的平均為噪聲方差，構造新的對角矩陣A
     if (i<=p)                 % 其前p個對角元素為S中的前p個對角元素減去噪聲方差，后m-p個元素直接置零
         A(i,i)=S(i,i)-sigma;
     else
         A(i,i)=0;
     end
 end
 Cxx=U*A*U';                         % not use the mean of inferior eigenvalue, but use the left eig matrix
%%% 這樣就得到了新的關于Cxx的計算公式

% 使用類似的方法構造出Cxy矩陣。由于sigma*Z表示的是E（w（n）w（n+1）’），所以可以首先構造對角陣，使其前
%% p個元素為平均值sigma，后m-p個元素為上面得到的S的后m-p個元素，然后分別用U左乘和U’右乘該對角陣得到E（w（n）w（n）’）
%% 的估計，再通過適當拼接（補零或者刪去某一行）來得到E（w（n）w（n+1）’）的估計
 for i=1:m                    
     if (i<=p)
         k(i)=sigma;
     else
         k(i)=eigenvalue(i);
     end
 end
 B=U*diag(k)*U';
 B=[B(:,2:m) zeros(m,1)];   
 Cxy=Rxy-B;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




d=eig(Cxx,Cxy);                     % eigenvalue
d_nonzero=0;                        % nonzero eigenvalue
for i=1:m
    if (abs(d(i))>=0.9 & abs(d(i)<=1.1))
        d_nonzero=[d_nonzero d(i)];
    end
end

frequency=angle(d_nonzero(2:length(d_nonzero)))/2/pi;
frequency=sort(frequency);            % order the turn
for i=1:length(frequency)
    if (frequency(i)>0.15)
        k=i;
        break
    end
end
result(pp,:)=frequency(k:k+1);       % each of its row is the result of one run
end