clear
clc
n = 0:1:199;
dt = 0.001;
x = exp(-n*dt/0.01).*sin(2*pi*0.12*n*dt)+0.5*exp(-n*dt/0.01).*sin(2*pi*0.24*n*dt);
plot(n,x)
% % 
% load matlab.mat;
s=x;
L = length(s(1:199));
pe = ceil(L/2);
R  = [];
k = 0;   
% 形成擴展階矩陣
while k < pe+1
    p = 1;
    Re = [];
    while p < pe+1
        u = pe+1 - p; v = L  - p;
        m = pe+1 - k; l = L  - k;
        r = sum(s(u:v).*conj(s(m:l)));
        Re = [Re,r];
        p = p + 1;
    end
    R = [R,Re'];
    k = k + 1;
end
[U,S,V] = svd(R);  % 分解
[m,n] = size(R);
h = min(m,n);
cro = [];
for ks = 1:h;
    cro = [cro,S(ks,ks)];
end
ccro = cro/cro(1);
% 估計階數
for pp = 1:h
    if ccro(pp)>=0.045
        pp = pp + 1;
    else 
        break;
    end
end
p = pp-1;
RE = R(2:end,2:p+1);
b  = R(:,1);
b = b(2:end);
a = pinv(RE)*(-b);
R1 = R(1,:);
R1 = R1(1:p+1);
a1 = [1 a'];
Ep = sum(R1.*a1);
P = [1 a'];
z = roots(P);
%% 估計序列X
kk = 1:p;
X(kk) = s(kk);
for pp = p+1:L
    ll = 1:p;
  X(pp) = sum(-a'.*s(pp-ll));
    pp = pp+1;
end
Zh = [];
for mm = 0:L-1
    Zh = [Zh,z.^mm];
end
Z = Zh';
Z = conj(Z);
Zhh  = Z';
b = (Zhh*Z)^(-1)*Zhh*X';
A = abs(b)
f  = angle(z)/2/pi/0.001
% a  = log(abs(z))/dt
% theta = angle(b)/2/pi/dt
plot(X,'r');
hold on;
plot(s,'b');
hold on;