% Motor de Corriente Continua con Excitacion independiente constante 
%------------------------
% Parametros Fisicos
%------------------------
Ra=0.062; % ohms
La=2.5e-3; % Hy
J=30; % Nms^2
b=1.275; % Nms
k=6.6; % Nm/A
%-------------------------
% Funcion Transferencia
% G(s)=num(s)/den(s)
% 
twopsiwn=(b/J) + (Ra/La);
wn_2=(Ra*b+k^2)/(La*J);
Kwn_2=k/(La*J);
num=Kwn_2; 
den=[1 twopsiwn wn_2]; 
sistema=tf(num,den);
% 
% Computo de G(z)=ZOH{G(s)} 
% 
Ts=1e-3; 
%[numd,dend]=c2dm(num,den,Ts,'zoh');
[numd,dend]=c2dm(num,den,Ts,'tustin'); 
sistemad=tf(numd,dend,Ts);
% 
% Generacion de datos de entrada/salida a partir del sistema 
% en tiempo continuo 
% 
randn('state',1137)
t=[0:Ts:8]'; 
u=idinput(length(t),'PRBS',[0 0.01],[-1 1]);
%u=sin(1*pi*t)+0.5*sin(3*pi*t)+0.001*randn(size(t)); 
randn('state',2137)
ruido=0.000001*randn(size(u));
ysinruido=lsim(num,den,u,t); 
y=ysinruido + ruido;
% 
% Computo del regresor 
% 
% 
yy=-y; 
ry=[yy(2) yy(1)]; 
cy=yy(2:length(yy)-1,:); 
phiy=toeplitz(cy,ry); 
ru=[u(2) u(1)]; 
cu=u(2:length(u)-1,:); 
phiu=toeplitz(cu,ru); 
phit=[phiy phiu]; 
% 
% Computo de la estima por minimos cuadrados 
% 
thetahat=phit\y(3:length(y),:); 
% 
% Computo de la salida estimada 
% 
[yhat,x]=dlsim([thetahat(3) thetahat(4)],[1 thetahat(1) thetahat(2)],u); 
% 
% Ploteo de la salida y la salida estimada 
% 
subplot(311) 
plot(t(3:length(t),:),y(3:length(y),:),'g',t(3:length(t),:),yhat(3:length(y)),'r') 
title('Salida (verde) y Salida Estimada (roja)'); 
%axis([0 10 -0.25 0.25]) 
subplot(312) 
plot(t(3:length(t),:),y(3:length(y),:)-yhat(3:length(y))) 
title('Error de Estima') 
subplot(313) 
plot(t(3:length(t),:),u(3:length(u),:)) 
xlabel('Tiempo [s]'),title('Entrada')
%
% Computo de la FT estimada
%
numde=[thetahat(3) thetahat(4)];
dende=[1 thetahat(1) thetahat(2)];
sysd=tf(numde,dende,Ts);
sysc=d2c(sysd,'tustin');
[numc,denc]=tfdata(sysc,'v');
figure
pzmap(sistemad,sysd)
figure
pzmap(sistema,sysc)
SNR=20*log10(norm(ysinruido)/norm(ruido));
%
% Identificacion basada en la Respuesta al Escalon
%
g=step(sistema,t(1:1000))+ruido(1:1000);
figure(4)
plot(t(1:1000),g)


