clear all, close all, clc

load modelo_identifica.mat
u=fuerza;
y=posicion;
Y=y(4:15001);
X=[y(3:15000) y(2:14999) y(1:14998) u(3:15000) u(2:14999)];
T=inv(X'*X)*X'*Y
for k=1:5
ye=X*T;
ye=[y(1); y(2); y(3);ye];
err=y-ye;
Y=y(4:15001);
X=[y(3:15000) y(2:14999) y(1:14998) u(3:15000) u(2:14999) err(3:15000)
err(2:14999)];
T=inv(X'*X)*X'*Y
end
a1=T(1);
a2=T(2);
a3=T(3);
b1=T(4);
b2=T(5);
c1=T(6);
c2=T(7);
%validar
load modelo_valida.mat
yv=posicion;
uv=fuerza;
ye1(1)=yv(1);
ye1(2)=yv(2);
ye1(3)=yv(3);
n=length(yv);
for i=4:n
ye1(i)=a1*ye1(i-1)+a2*ye1(i-2)+a3*ye1(i-3)+b1*uv(i-1)+b2*uv(i-2);
end
ye1=ye1';
%medida estimada
muestras=1:n;
figure, plot(muestras,yv,'b',muestras,ye1,'r')
legend('medida','estimada')
%factor de confiabilidad
FC= (1-norm(yv-ye1)/norm(yv-mean(yv)))*100
%B)

Ts=1/500;
numd=[b1 b2 0];
dend=[1 -a1 -a2 -a3];
sysd=tf(numd,dend,Ts)
sysc=d2c(sysd)
damp(sysc)
figure
pzmap(sysc)
%C)
figure
rlocus(sysc)
figure
margin(sysc)
%E)
K=10^(-5/20)
sysc_mod=sysc*K
% COMO NOS PIDEN 5DB MS PARA AUMENTAR AL SISTEMA
%AL sysc * (-5db CONVERTIDOS A MAGNITUD)
figure
margin(sysc_mod)
%E)
numd2=[b1 b2 0 0];
dend2=[1 -a1 -a2 -a3];
sysd2=tf(numd2,dend2,Ts)
sysc2=d2c(sysd2)
figure
step(sysc2)
%F)adicinar polos dominantes
%break
%polos dominates
OS=9.3
psi=-log(OS/100)/sqrt(pi^2+log(OS/100)^2)
Te=1.23
wn=4/(psi*Te)%te=4/(psi*wn) etocces wn=5.393
num=wn^2;
den=[1 2*psi*wn wn^2];
sys1=tf(num,den)
figure, pzmap(sys1)
%final
%adcionado polos dominanstes
final=sys1*sysc; % se multiplica el sysc (q es el original) por el de
polos que hallamos
figure, pzmap(final)
%G) FT continua de la posicion con el ruido
numg=[1 c1 c2 0];
deng=[1 -a1 -a2 -a3];
Ts=1/500;

sysdg=tf(numg,deng,Ts) %DISCRETA
syscg=d2c(sysdg)
%CONTINUA
figure, step(syscg)
%H) MODLEO ARX

N(A)=3, N(B)=2.

%load modelo_identifica.mat
%y=posicion;
%u=fuerza;
%n=length(u);
%Y=y(4:end);
%X=[y(3:end-1) y(2:end-2) y(1:end-3) u(3:end-1) u(2:end-2)]
%T=inv(X'*X)*X'*Y
%load modelo_valida.mat
%yv=posicion;
%uv=fuerza;
%n=length(yv);
%ye(1)=yv(1);
%ye(2)=yv(2);
%ye(3)=yv(3);
%for k=4:n
% ye(k)=T(1)*ye(k-1)+T(2)*ye(k-2)+T(3)*ye(k-3)+T(4)*uv(k-1)+T(5)*uv(k2);
%end
%muestras=1:n;
%figure, plot(muestras,yv,'r',muestras,ye,'b');
%legend('medidaarx','estimadaarx')
%FC=(1-norm(yv-ye)/norm(yv-mean(yv)))*100

ARMAX
clear all, close all, clc

load ident_rotacional.mat
y=angulo;
u=torque;
n=length(u);
muestras=1:n;
Y=y(3:end);
X0=[y(2:end-1) y(1:end-2) u(2:end-1) u(1:end-2)];
Theta0=inv(X0'*X0)*X0'*Y
ye=X0*Theta0;
ye=[y(1);y(2);ye];
err=y-ye;
for k=1:15
Y=y(3:end);
X=[y(2:end-1) y(1:end-2) u(2:end-1) u(1:end-2) err(2:end-1)];
Theta=inv(X'*X)*X'*Y;
ye=X*Theta;
%salida estimada de ARMAX
ye=[y(1);y(2);ye]; %Completando el vector
err=y-ye;
clear ye

%error del modelo ARMAX

end
Theta
a1=Theta(1);
a2=Theta(2);
b1=Theta(3);
b2=Theta(4);
c1=Theta(5);
% Validando modelo
load val_rotacional.mat
yv=angulo;
uv=torque;
n=length(yv);
muestras=1:n;
ye(1)=yv(1);
ye(2)=yv(2);
for k=3:n
ye(k)=a1*ye(k-1)+a2*ye(k-2)+b1*uv(k-1)+b2*uv(k-2);
end
ye=ye';
FC=(1-norm(yv-ye)/norm(yv-mean(yv)))*100

% PREGUNTA B (salida -salida estimada, a eso se refiere graficar el

error)

figure, plot(muestras,yv,'b',muestras,ye,'r')
legend('medido','estimado')
title('Modelo Medido vs Modelo estimado')
%PREGUNTA C: Funcion de transferencia teta/torque
numd=[b1 b2];
dend=[1 -a1 -a2];
Ts=1/100;
sysd=tf(numd,dend,Ts)
sysc=d2c(sysd)
damp(sysc)
figure,pzmap(sysc)
%PREGUNTA E (lugar geometrico)rlocus
figure,rlocus(sysc)
figure,margin(sysc)
%PREGUNTA F: Para reducir el margen 10 db/cuando quiero reducir
J=10^(10/20)
figure,margin(sysc/J)
figure,bode(sysc/J)
%PREGUNTA G: Funcion de transferencia del ERROR/es con el ruido osea el
%erroor
('FT del ruido')
numd2=[1 c1 0];
dend2=[1 -a1 -a2];
Ts=1/100;
sysd2=tf(numd2,dend2,Ts)
sysc2=d2c(sysd2)
figure, step(sysc2)
damp(sysc2)

ARX/FIR
clear all, close all, clc
%PREGUNTA H:FIR DE N(B)=3
load ident_rotacional.mat

y=angulo;
u=torque;
Y=y(4:end);
X=[u(3:end-1) u(2:end-2) u(1:end-3)];
Theta= inv(X'*X)*X'*Y
b1=Theta(1)
b2=Theta(2)
b3=Theta(3)
% Validacin
load val_rotacional.mat
yv=angulo;
uv=torque;
n=length(yv);
muestras=1:n;

ye(1)=yv(1);
ye(2)=yv(2);
ye(3)=yv(3);
ye=ye';
for k=4:n
ye(k)=b1*ye(k-1)+b2*ye(k-2)+b3*uv(k-3);
end
figure, plot(muestras,yv,'b',muestras,ye,'r')
legend('medido','estimado')
%FACTOR DE CONFIABILIDAD
FC=(1-norm(yv-ye)/norm(yv-mean(yv)))*100

%PREGUNTA I: ARX N(A)=2, N(B)=1

load ident_rotacional.mat
y=angulo;
u=torque;
Y=y(3:end);
X=[y(2:end-1) y(1:end-2) u(2:end-1)];
Theta2= inv(X'*X)*X'*Y
a12=Theta2(1)
a22=Theta2(2)
b12=Theta2(3)
% Validacin
load val_rotacional.mat

yv=angulo;
uv=torque;
n=length(yv);
muestras=1:n;
ye(1)=yv(1);
ye(2)=yv(2);
for k=3:n
ye(k)=a12*ye(k-1)+a22*ye(k-2)+b12*uv(k-1);
end
figure, plot(muestras,yv,'b',muestras,ye,'r')
legend('medido','estimado')
%FACTOR DE CONFIABILIDAD
FC=(1-norm(yv-ye)/norm(yv-mean(yv)))*100