%SVD-TLS方法（未引入隨機相位）
clear all;

N = 128;
Pi = 3.1415926;
n = [1:1:N];
pe= 58;
M = 68;

Times=100;
f=zeros(Times,2);
for k=1:Times
    
w = randn(1,N);
x = sqrt(20)*sin(2*Pi*0.2*n)+sqrt(2)*sin(2*Pi*0.213*n)+w;

%r=R(N,x,T)       %N is the data number,T is the time delay
Re=zeros(M,pe+1);
for i=1:M
    for j=1:pe+1
        T=pe+i-(j-1);
        Re(i,j)=R(N,x,T);
    end
end
%[U,d,V]=svd(R);U為左奇異矩陣，d為對角陣
[U,d,V]=svd(Re);
for i=1:pe+1 
    P(i) = d(i,i);
end
for i=1:pe+1 
    if (P(i)/P(1))<=0.05
        p=i-1+mod(i-1,2);
        break;
    end
end

nn=pe;
S = zeros(p+1,p+1);
for j=1:p
    for i=1:nn+1-p
        S=S+(P(j))^2*V(i:i+p,j)*(V(i:i+p,j))';
    end
end
Sn=S^(-1);
for i=1:p
    a(k,i)=Sn(i+1,1)/Sn(1,1);
end

z=roots([1,a(k,1:p)]);
num=1;
for i=1:p
    if abs(z(i))<1.05 & abs(z(i))>0.95
        temp=atan(imag(z(i))/real(z(i)))/(2*Pi);
        if temp>0
            f(k,num)=temp;
            num=num+1;
        end
    end
end

end

%讓f從大到小排序
f=sort(f,2,'ascend');

ua=sum(a)/Times;
da=0;
for i=1:Times
    da=da+((a(i,:)-ua).^2);
end
da=da/Times;

uf=sum(f)/Times;
df=0;
for i=1:Times
    df=df+((f(i,:)-uf).^2);
end
df=df/Times;

ua
da
uf
realf=[0.2 0.213]
offset=abs(realf-uf)
f;
df