%TLS-ESPRIT算法(加入隨機(jī)相位以后)
clear all;

N = 128;
Pi = 3.1415926;
n = [1:1:128];
m=80;

Times=100; %獨(dú)立仿真次數(shù)
%f=zeros(Times,2);
for k=1:Times
    
w = randn(1,N);   
fai1=rand*2*Pi-Pi;
fai2=rand*2*Pi-Pi;
x = sqrt(20)*sin(2*Pi*0.2*n+fai1)+sqrt(2)*sin(2*Pi*0.213*n+fai2)+w;
%r=R(N,x,T)       %N is the data number,T is the time delay
Rxx = zeros(m,m);
for i=1:m
    for j=1:m
        if (i-j)>=0
            Rxx(i,j)=R(N,x,(i-j));
        else
            Rxx(i,j)=(R(N,x,(j-i))').';
        end
    end
end
Rxy = zeros(m,m);
for i=1:m
    for j=1:m
        if (i-j)>0
            Rxy(i,j)=R(N,x,(i-j-1));
        else
            Rxy(i,j)=(R(N,x,(j-i+1))').';
        end
    end
end
[V,D] = eig(Rxx);
for i=1:m
    d(i)=D(i,i);
end
%讓d從大到小排序
d=sort(d,'descend');
for i=1:m
    if d(i)/d(1)<0.05
        p0=i-1;
        break;
    end
end
%sigma=min(d);
sigma=sum(d(p0+1:m))/(m-p0);

I=diag(ones(1,m));
Z=zeros(m,m);
for i=1:m
    for j=1:m
        if (i-j)==1
            Z(i,j)=1;
        end
    end
end
Cxx=Rxx-sigma*I;
Cxy=Rxy-sigma*Z;

%[U,d,V]=svd(R);U為左奇異矩陣，d為對角陣
[UU,DD,VV]=svd(Cxx);
for i=1:m
    dd(i)=DD(i,i);
end
%讓d從大到小排序
%d=sort(d,'descend');
for i=1:m
    if dd(i)/dd(1)<0.05
        p=i-1;
        break;
    end
end
E1 = DD(1:p,1:p);
U1 = UU(:,1:p);
V1 = VV(:,1:p);

T=U1'*Cxy*V1;
%[V,D] = EIG(A,B) 
ddd = eig(E1,T);

num=1;
W=0;
for i=1:p
    if abs(ddd(i))>0.95 & abs(ddd(i))<1.05
        temp=atan(imag(ddd(i))/real(ddd(i)));
        if real(ddd(i))>=0 & imag(ddd(i))>=0
            f(k,num)=temp;
            num=num+1;
        end
        if real(ddd(i))<0 & imag(ddd(i))>=0
            f(k,num)=temp+Pi;
            num=num+1;
        end
    end
end
end

f=f/(2*Pi);
%f=sort(f,'ascend');
uf=sum(f)/Times;
df=0;
for i=1:Times
    df=df+((f(i,:)-uf).^2);
end
df=df/Times;

uf
df
f;
p;