clear
close all

% DEFINICION DE PARAMETROS DE SIMULACION
fm=10;                % FRECUENCIA DE MUESTREO
Tm=1/fm;              % PERIODO DE MUESTREO
Tf=1;                 % TIEMPO TOTAL DE SIMULACION
na=Tf/Tm;             % NUMERO DE TOTAL DE MUESTRAS EN LA SIMULACION
z=tf('z',Tm);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Matrices de estado del sistema discreto
G=[  0     1;
   -0.16  -1];
H=[0;1];
C=[1 0];
D=0;

% Determinación de la controlabilidad del sistema
rango_planta=rank(ctrb(G,H));        % rango debe ser igual al orden del sistema

% Matrices G y H circunflejas
Gc=[G H; zeros(1,2) 0]; Hc=[zeros(2,1);1]; Cc=[C 0];
% Controlabilidad del sistema aumentado
Mc=ctrb(Gc,Hc);
rango_c=rank(Mc);

% Proyecto usando reubicación de polos
% Especificaciones de proyecto
pp=0;  % se eligen los polos deseados
db=1;
if pp == 0 && db == 1
    z1=0.0;         % z1, z2 y z3, son los polos dominantes 
    z2=0.000001;    % en el dominio discreto para respuesta deadbeat 
    z3=0.00000001;
    Tf=1; t=0:Tm:Tf-Tm;
    m = 10;
end

if pp == 1
    z1=0.5 + 1i*0.5;      % Polos dominantes de lazo cerrado
    z2=0.5 - 1i*0.5;      % complejos conjugados en el dominio discreto
    z3=0.01;              % z3 es el polo no dominante
    Tf=2.5; t=0:Tm:Tf-Tm;
    m = 20;
end

if pp == 0 && db == 0
    z1=0.55;   % z1 y z2 son los polos dominantes en el dominio discreto
    z2=0.401; 
    z3=0.01;  % z3 es el polo no dominante
    Tf=5; t=0:Tm:Tf-Tm;
    m = 50;
end
    
% Polinomio caracteristico deseado
Pcd=(z - z1)*(z - z2)*(z - z3);
Coef=Pcd.num;
n=Coef{:,:};
alfa3=n(4); alfa2=n(3); alfa1=n(2);

% Método 1: Transformacion T = M*W
pc=poly(Gc);
Pc=pc(1)*z^3 + pc(2)*z^2 + pc(3)*z + pc(4);
a3=pc(4); a2=pc(3); a1=pc(2);

W=[a2 a1 1;a1 1 0;1 0 0];
T=Mc*W;
 
Kc1=[(alfa3 - a3) (alfa2 - a2) (alfa1 - a1)]*inv(T);

% Método 2: Formula de Ackermann
fi=Gc^3 + alfa1*Gc^2 + alfa2*Gc + alfa3*eye(3);
Kc2=[0 0 1]*inv(Mc)*fi;

% Método 3: Funcion Place
polos=[z1 z2 z3];
Kc3=place(Gc,Hc,polos);

Ms=[G-eye(2) H;C*G C*H];
K21=(Kc3 + [zeros(1,2) 1])*inv(Ms);

Kre1=K21(1);     % Ganancias para realimentacion de estados
Kre2=K21(2);
Ks=K21(3);

% Matriz de lazo cerrado para simular el sistema

Glc=[(G - H*[Kre1 Kre2]) H*Ks;(C*H*[Kre1 Kre2] - C*G) (1 - C*H*Ks)];
Hlc=[zeros(2,1);1];

% Vector de condiciones iniciales
x0=[10;-5;2.5];
sys_c=ss(Glc,Hlc,Cc,D,Tm);

[Y,t,X]=initial(sys_c,x0,t);
figure; stairs(t,Y)
legend('Salida = x1')

figure; stairs(t,X(:,1)); legend('Estado x1')
figure; stairs(t,X(:,2)); legend('Estado x2')
figure; stairs(t,X(:,3)); legend('Estado x3')

figure; step(sys_c,t)
legend('Respuesta al escalón unitario')
axis([0 max(t) 0 1.1])
% pause
close all
%% ------------------------------------------------------------------------
% Simulación digital
td=zeros(1,m);
r=zeros(1,m);
e=zeros(1,m);
y=zeros(1,m);
u=zeros(1,m);
v=zeros(1,m);
x=zeros(2,m);

for k = 2:m
    td(k) = (k-2)*Tm;
    
    r(k) = 1;
    y(k) = C*x(:,k);
    e(k) = r(k) - y(k);
    
    v(k) = v(k-1) + e(k);
    u(k) = Ks*v(k) - [Kre1 Kre2]*x(:,k);
    
    x(:,k+1) = G*x(:,k) + H*u(k);
end

figure; stairs(td(2:k),r(2:k),'ok'); hold on;
stairs(td(3:k),y(3:k),'r'); 
axis([Tm max(td) 0 1.1])
legend('referencia','salida')

figure; stairs(td(2:k),r(2:k),'ok'); hold on;
stairs(td(1:k),e(1:k),'r'); 
axis([Tm max(td) 0 1.1])
legend('referencia','error')

% figure; stairs(td,x(1,1:k)); hold on
% stairs(td,x(2,1:k));
% legend('estado x1','estado x2')

figure; stairs(td,u,'r');
legend('acción de control')