%--------------SVD-TLS算法-----------------%
clear
loops = 20;        % loops 是獨立實驗次數
M = 100;
pe =30;
p=0;
for loop=1:loops
    % while p~=4
    for k = 1:50
   % 生成觀測數據
          n = [1:128];
          w = randn(1,128);
          x = sqrt(20)*sin(2*pi*0.2*n) + sqrt(2)*sin(2*pi*0.213*n)+w; % 不要直接加上randn(1,128) x = sqrt(20)*sin(2*pi*0.2*n) + sqrt(2)*sin(2*pi*0.213*n)+randn(1,128)
          %w=randn(2000,1);
          %for n=1:1:128
              %x(n)=(20^(1/2))*sin(2*pi*0.2*n)+(2^(1/2))*sin(2*pi*0.213*n)+w(n+loop*50); 
          %end
          Rxx = xcorr(x,'unbiased');    % 相關矩陣
          for i = 1:M
              for j = 1:pe+1
                  Re(i,j) = Rxx(pe+i+1-j);
              end
          end                         % Re是修正方程的增廣矩陣(M,pe+1)
          %求Re的有效秩p
          [U,S,V]=svd(Re);       
          Ak = 0;
          for i=1:pe+1
              Ak = Ak + S(i,i)^2;
          end
          Akf=0;
          v = Akf/Ak;
          p=0;
          while v<0.9979                                   % 選擇0.9979能夠保證有效秩大部分時間為4
                p = p + 1;
                Akf = Akf + S(p,p)^2;
                v = Akf/Ak;
          end
          P(k) = p;
    end
    P;
    p = fix(mean(P));                                       % p即為有效秩
    %計算矩陣Sp
    Sp = 0;
    for j=1:p
        for i=1:pe+1-p
            Vj=V(:,j);
            Sp=Sp+S(j,j)^2*(Vj([i:i+p]))*(Vj([i:i+p]))';
        end
    end
    invSp=inv(Sp);
    for i = 1:p
        a(i)=invSp(i+1,1)/invSp(1,1);
    end
    a;
    ARMA_LS(:,loop) = a;
    Az(1) = 1;
    for i = 2:p+1
        Az(i) = a(i-1);
    end
    z = roots(Az);
    num = 1;
    for i = 1:2:p
        f_ls(num) = atan(imag(z(i))/real(z(i)));
        f_ls(num) = abs(f_ls(num))/(2*pi);
        num = num +1;
    end
    f_ls = sort(f_ls);
    Fz(:,loop) = f_ls;
end
p
ARMA_LS
ARMA_LS_mean = mean(ARMA_LS,2)
ARMA_LS_std  = std(ARMA_LS')'
Fz
Fz_mean = mean(Fz,2)
Fz_var  = var(Fz')'