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;

pc=poly(G);
Pc=pc(1)*z^2 + pc(2)*z + pc(3);
a2=pc(3); a1=pc(2);

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

% Especificaciones de proyecto
pp=0;

if pp == 1
    z1=0.201;
    z2=0.2; Tf=1;
    t=0:Tm:Tf-Tm;
else
    z1=0.7 + 1i*0.2;          % Polos dominantes de lazo cerrado
    z2=0.7 - 1i*0.2;          % complejos conjugados en el dominio discreto
    Tf=2.5; t=0:Tm:Tf-Tm;
end

Pcd=(z - z1)*(z - z2);
Coef=Pcd.num;
n=Coef{:,:};
alfa2=n(3); alfa1=n(2);

% Proyecto usando reubicación de polos
% Método 1: Transformacion T
M=ctrb(G,H);
W=[a1 1;1 0];
T=M*W;

Km1=[(alfa2 - a2) (alfa1 - a1)]*inv(T);

% Método 2: Formula de Ackermann
fi=G^2 + alfa1*G + alfa2*eye(2);
Km2=[0 1]*inv(M)*fi;

% Método 3: Funcion Place
polos=[z1 z2];
Kplace=place(G,H,polos);

K1=Kplace(1);   % Ganancias para realimentacion de estados
K2=Kplace(2);

Km3=[K1 K2];

Gc=G - H*Km1; Hc=H;

% Vector de condiciones iniciales
x0=[10;-5];
sys_c=ss(Gc,Hc,C,D,Tm);

[Y,t1,X]=initial(sys_c,x0,t);
figure; stairs(t,X(:,1),'k'); hold on; stairs(t,X(:,2),'o-r');
legend('x1','x2')

figure; step(sys_c,t)
legend('Respuesta al escalón unitario')

% Ajuste de la ganancia Ko para error nulo de regimen estacionario
gan_cero=dcgain(sys_c);
Ko=1/gan_cero;
figure; step(100*Ko*sys_c,t)

close all

%% Para respuesta deadbeat:
z1=0;           % Polos dominantes de lazo cerrado
z2=0;           % complejos conjugados en el dominio discreto

Pcd=(z - z1)*(z - z2);
Coef=Pcd.num;
n=Coef{:,:};
alfa2=n(3); alfa1=n(2);

% Método 1: Transformacion T
M=ctrb(G,H);
W=[a1 1;1 0];
T=M*W;

Km1d=[(alfa2 - a2) (alfa1 - a1)]*inv(T);

% Método 2: Formula de Ackermann
fi=G^2 + alfa1*G + alfa2*eye(2);
Km2=[0 1]*inv(M)*fi;

% Método 3: Funcion Place
z1=0;          % Polos dominantes de lazo cerrado
z2=0.00001;       % complejos conjugados en el dominio discreto
polos=[z1 z2];
Kplace=acker(G,H,polos);

K1=Kplace(1);   % Ganancias para realimentacion de estados
K2=Kplace(2);

Km3=[K1 K2];

Gcd=G-H*Km3;

% Tf=1;
% t=0:Tm:Tf-Tm';
x0=[10;-5];
sys_cdb=ss(Gcd,Hc,C,D,Tm);

[Yd,t2,Xd]=initial(sys_cdb,x0,t);
figure; stairs(t,Xd(:,1),'k'); hold on; stairs(t,Xd(:,2),'o-r');
legend('x1','x2')

figure; step(sys_cdb,t)
legend('Respuesta al escalón unitario')

%%------------------------------------------------------------------------