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

%Nhan dang 
alpha=50;
ghat=Teta2_nga/alpha;                


%tim ham truy阯
%A=[0 1 0 0;-9.5 -4.545 9.5 0;0 0 0 1;9.21 0 -11.32 -0.48]
%B=[0 0 ;4.623 0 ;0 0 ;0 4.485]
%C=[0 0 1 0 ]
%D=[0 0 ]
%Ham truyen dung cua he thong can nhan dang  
%Pttt=ss(A,B,C,D);
%G0s=tf(Pttt)   
num=[42.58]
den=[1 5.025 23 56.01 20.04]
G0s=tf(num,den)
G0z=c2d(G0s,0.1)
%So sanh voi dap ung xung dung cua he thong;
N=length(Teta2);
g0=impulse(G0z,200);
m=length(g0);
error=(g0(1:N)-ghat(1:N))'*(g0(1:N)-ghat(1:N))/N ;      
plot(1:N,ghat,'g',1:N,g0(1:N),'r');    % So sanh sai so truc quan dung do thi 
legend('ghat','g0');