clc
% Tinh du lieu vao - ra de nhan dang mo hinh tuyen tinh
Teta2_bar=pi/6;
To1_bar = 17.9661
Teta2_nga=Teta2(:,2)-Teta2_bar;
To1_nga=To1(:,2)-To1_bar;

%Nhan dang 
alpha=100;               
N=length(Teta2);
Teta2_ = Teta2_nga;             %Tin hieu ra
To1_ = To1_nga;             %Tin hieu vao

Ryu=zeros(N,1);
for T = 1:N-1
    R = 0;
    for k = (T+1):(N)
        R = R + Teta2_(k)*To1_(k-T);  %tinh Ryu
    end
    Ryu(T+1)=R/(N-T);
end
ghat=Ryu/alpha;



%So sanh voi dap ung xung dung cua he thong;
num=[42.58];
den=[1 5.025 23 56.01 20.04];
G0s=tf(num,den)    % Ham truyen dung cua he thong can nhan dang
G0z=c2d(G0s,0.1)
N=length(Teta2);
g0=impulse(G0z,22);
index=1:200;
err=(g0(index)-ghat(index))'*(g0(index)-ghat(index))/N       % Sai so nhan dang 
plot(index,ghat(index),'r',index,g0(index),'b');    % So sanh sai so truc quan dung do thi 
legend('ghat','g0');