% -----------------------------------------------------------------
%------------------王程  2008-12-10--------------------------------
%------------------仿真作業一：總體最小二乘-------------------------
%-----------------------------------------------------------------
q = 1 ;
num = 1000 ;        %重復計算次數
lumda = 1 ;
T = 128 ;
M = 40 ;
p = 7 ;            %取未知參數個數為14個,即2p=14
n = 1 : T ;
delta = 0.008 ;    %求解有效秩使用的域值
A = zeros(40, 2*p) ;
b = zeros(40, 1) ;

while q <= num                       %重復1k次

x = sqrt(20) * sin(2*pi*0.2*n) + sqrt(2) * sin(2*pi*0.213*n) + randn(1, T);  %  generate signals
r = xcorr(x, 'biased');             %有偏的自相關函數
for m = 1 : M
    A(m, : ) = r(T+m+2*p-1 : -1 : T+m) ;
    b(m) = -r(T+2*p+m) ;
end
%------------以下使用SVD-TLS---------------
B = [-b, A] ;
[U, S, V] = svd(B) ;                %對B進行奇異值分解
R = 0 ;                             %有效秩
for j = 1 : 2*p+1
    sigma(j) = S(j, j) / S(1, 1) ;  %計算歸一化奇異值
    if (sigma(j) > delta)
        R = R + 1 ;
    end
end
Sp = zeros(R+1, R+1) ;
y = zeros(1, R) ;
v = zeros(R+1, 1) ;
for j = 1 : R
    for i = 1 : 2*p+1-R
        v(1 : R+1,1) = V(i : i+R, j) ;
        Sp = Sp + S(i, i) * S(i, i) * v * v' ;
    end
end
if (rank(Sp) < R+1)
    continue ; 
else    
    Sp_inv = inv(Sp) ;
end
for i = 1 : R
    y(i) = Sp_inv(i+1, 1) / Sp_inv(1, 1) ;
end 
%------------------------------------------
a = zeros(1, R+1) ;
for i = 1 : R
    a(i) = y(R-i+1) ;
end
a(R+1) = 1 ;
z = roots(a) ; 
for i = 1 : R
    z(i) = 1 / z(i) ;               %求出極點

end

for i = 1 : floor(R/2)
    fi(i) =  atan( abs( imag(z(2*i-1)) / real(z(2*i-1)) ) ) / (2*pi) ;        %求解諧波頻率
    if fi(i) ~= 0 
        f(lumda) = fi(i) ;
        lumda = lumda + 1 ;
    end
end

q = q + 1 ;
end

figure, hist(f, 100) ;