%Laboratorio No 4 - Compensador Sistemas Lineales

clc;
clear all;
close all;
%%

%% definicion de parametros pendulo invertido en carrito

%M=10; m=80; c=0.1; gamma=0.01; g=9.81; l=1; J=100;
M=0.5; m=0.2; c=0.1; gamma=0.1; g=9.81; l=0.3; J=0.0006;

%%
%% matrices de la representacion espacio de estados lineal
% con respecto a x=(p,theta,p_dot,theta_dot)
Mt=m+M;
Jt=J+m*l^2;
mu=Mt*Jt-m^2*l^2;

A=[0,0,1,0;...
0,0,0,1; ...
0, m^2*l^2*g/mu, -c*Jt/mu, -gamma*l*m/mu ; ...
0, Mt*m*g*l/mu, -c*l*m/mu, -gamma*Mt/mu];
%
B=[0;0;Jt/mu;l*m/mu];

%% estabilidad
pA=eig(A);

%% controlabilidad
n=length(A);
Wc = [B A*B A^2*B A^3*B]; %numero de estados n=4
rangoWc = rank(Wc);
%
Wcm = ctrb(A,B);

%% diseño del control por realimentacion de estados

%
%% polos deseados pendulo
Wop=sqrt(m*g*l/Jt);
zp=0.7;
%
%% polos deseados carrito
ts=1;
zc=0.7;
Woc=4/(zc*ts);
%
pc=[-zp*Wop+Wop*sqrt(1-zp^2)*i; ...
-zp*Wop-Wop*sqrt(1-zp^2)*i; ...
-zc*Woc+Woc*sqrt(1-zc^2)*i; ...
-zc*Woc-Woc*sqrt(1-zc^2)*i];

%% polinomio formado por polos deseados

alphac=poly(pc);

%% ganancia de control K
% formula de ackerman - calculo de K --solo sirve para una entrada
Ka=[zeros(1,n-1),1]*inv(Wc)*polyvalm(alphac,A);
%
Kam=acker(A,B,pc);
%
Kp=place(A,B,pc); % sirve para varias entradas de control
%% seguimiento de referencia constante (theta)
%
Cz=[1 0 0 0]; %(sacando p)
Dz=0;
Cy=eye(4); %estados conocidos
Dy=zeros(4,1);
% calculo de Kr (ganancia por alimentación directa)
Kr=-pinv(Cz*inv(A-B*Kp)*B);

%% simulacion
% sistema lineal
sim('pendulo_invertido_carro_lin_cont')
figure(1)
subplot(1,2,1)
plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
title('señal controlada - sistema lineal')
subplot(1,2,2)
plot(u(:,1),u(:,2),'b')
title('señal de control')

% sistema no lineal
sim('pendulo_invertido_carro_nolin_cont')
figure(2)
subplot(1,2,1)
plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
title('señal controlada - sistema nolineal')
subplot(1,2,2)
plot(u(:,1),u(:,2),'b')
title('señal de control')

%% control integral
%planta+integrador
Ai=[A,zeros(n,1);Cz,zeros(1,1)];
Bi=[B;Dz];

%vector de polos del sistema controlado con integrador

pci=[pc;-2];

%usando la funcion place de Matlab (calcula controladores para mas de

una entrada)
Ki=place(Ai,Bi,pci);

%sacando k y ki
kpi=Ki(:,1:4);
kii=Ki(:,5);

%% simulacion
% sistema lineal
sim('pendulo_invertido_carro_lin_cont_int')
figure(3)
subplot(1,2,1)
plot(r(:,1),r(:,2),'k',zi(:,1),zi(:,2),'b')
title('señal controlada / control integral - sistema lineal')
subplot(1,2,2)
plot(ui(:,1),ui(:,2),'b')
title('señal de control')
% %

% sistema no lineal
sim('pendulo_invertido_carro_nolin_cont_int')
figure(4)
subplot(1,2,1)
plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
title('señal controlada / control integral - sistema nolineal')
subplot(1,2,2)
plot(u(:,1),u(:,2),'b')
title('señal de control')
%
%% Control por realimentacion por realimentacion de salidas
%% diseño del observador orden completo

clear Cy Dy
Cy = [1 0 0 0; 0 0 0 1]; %asumiendo se mide p y theta_dot
Dy = zeros(2,1);
%
% observabilidad
Wo=[Cy;Cy*A;Cy*A^2;Cy*A^3];
Wom=obsv(A,Cy);
rangoWo=rank(Wo);

%polos del observador

po=8*pc;

%ganancia del observador

Kd=place(A',Cy',po);
L=Kd';

%observador
Ao=A-L*Cy;
Bo=[B L];
Co=eye(4);
Do=zeros(4,3);
%
% %% simulacion
%
% % sistema lineal
% sim('pendulo_invertido_carro_lin_cont_obs')
% figure(5)
% subplot(1,3,1)
% plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
% title('señal controlada / observador - sistema lineal')
% subplot(1,3,2)
% plot(u(:,1),u(:,2),'b')
% title('señal de control')
% subplot(1,3,3)
% plot(z(:,1),z(:,2),'k',z_hat(:,1),z_hat(:,2),'b')
% title('estado estimado')
% %
% % sistema no lineal
% sim('pendulo_invertido_carro_nolin_cont_obs')
% figure(6)
% subplot(1,3,1)
% plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
% title('señal controlada / observador - sistema nolineal')
% subplot(1,3,2)
% plot(u(:,1),u(:,2),'b')
% title('señal de control')
% subplot(1,3,3)
% plot(z(:,1),z(:,2),'k',z_hat(:,1),z_hat(:,2),'b')
% title('estado estimado')
%
% %% compensador
% Ac=A-B*Kp-L*Cy;
% Bc=[L B*Kr];
% Cc=-Kp;
% Dc=[zeros(1,2), Kr];
% %
% % sistema no lineal
% sim('pendulo_invertido_carro_nolin_comp')
% figure(7)
% subplot(1,3,1)
% plot(r(:,1),r(:,2),'k',z(:,1),z(:,2),'b')
% title('señal controlada / compensador - sistema nolineal')
% subplot(1,3,2)
% plot(u(:,1),u(:,2),'b')
% title('señal de control')
% subplot(1,3,3)
% plot(z(:,1),z(:,2),'k',z_hat(:,1),z_hat(:,2),'b')
% title('estado estimado')
%
%
%
%
%% diseño del observador orden reducido

clear Cy Dy
Cy = [1 0 0 0; 0 0 0 1]; %asumiendo se mide p y theta_dot
Dy = zeros(2,1);
%
[q,n]=size(Cy);
%
R = [0 1 0 0;0 0 1 0];
P =[Cy;R];
%
A_b=P*A*inv(P);
B_b=P*B;
%
A11_b=A_b(1:q,1:q);
A12_b=A_b(1:q,q+1:n);
A21_b=A_b(q+1:n,1:q);
A22_b=A_b(q+1:n,q+1:n);
%
B1_b=B_b(1:q,:);
B2_b=B_b(q+1:n,:);

% REE observador de orden reducido x2_b_dot=A22_bx2_b+u_b; w=A12_bx2_b

% observabilidad
Wor=[A12_b;A12_b*A22_b];
Womr=obsv(A22_b,A12_b);
rangoWor=rank(Wor);

%polos del observador

por=[-40,-40];

%ganancia del observador

Kdr=place(A22_b',A12_b',por);
Lr=Kdr';

%observador orden reducido

Ar=A22_b-Lr*A12_b;
Bru=B2_b-Lr*B1_b;
Bry=(A22_b-Lr*A12_b)*Lr+A21_b-Lr*A11_b;
Br=[Bru Bry];
Cr=eye(q);
Dru=zeros(q,1);
Dry=Lr;
Dr=[Dru Dry];