%
% Cálculo do compensador LQG/LTR - veja arquivo Aula8_ExemploLQG.doc
%
% SEM 5928 - Sistemas de Controle 
% Escola de Engenharia de São Carlos - USP
%
% Adriano Siqueira - 2016
%

clear all;close all;
%Sistema nominal
Ap=[ -0.02 0.005 2.4 -32;
    -0.014 0.44 -1.3 -30; 
    0 0.018 -6 1.2;
    0 0 1 0];
Bp=[0.14 -0.12 ;
   0.36 -8.6 ; 
   0.35 0.009 ;
   0 0 ];
Cp=[0 1 0 0;
    0 0 0 57.3];
Dp=[0 0;0 0];

% %Valores singulares do sistema nominal 
% sysG=ss(Ap,Bp,Cp,Dp);
% w=logspace(-2,3,100);
% [sv,w]=sigma(sysG,w);
% semilogx(w,20*log10(sv));
% grid;
% title('Sistema nominal');
% xlabel('frequência');
% ylabel('dB');

%Sisema aumentado com integradores
A=[ 0 0 0 0 0 0;
   0 0 0 0 0 0;
   0.14 -0.12 -0.02 0.005 2.4 -32;
   0.36 -8.6 -0.014 0.44 -1.3 -30; 
   0.35 0.009 0 0.018 -6 1.2;
   0 0 0 0 1 0];
B=[1 0;
   0 1;
   0 0;
   0 0;
   0 0;
 	0 0];
C=[0 0 0 1 0 0;
   0 0 0 0 0 57.3];
D=[0 0; 0 0];

% %Valores singulares do sistema aumentado
% sysG=ss(A,B,C,D);
% [sv,w]=sigma(sysG,w);
% figure;
% semilogx(w,20*log10(sv));
% grid;
% title('Sistema nominal com integradores');
% xlabel('frequência');
% ylabel('dB');
%  

% %Determinação da barreira de estabilidade e da barreira de desempenho 
figure; 
alfar=20*log10(1/0.10);  %sinal de referência
alfad=20*log10(1/0.10);  %rejeição de perturbações
alfas=20*log10(1/0.15);  %sensibilidade a variações da planta
w=linspace(0.01,0.5,1000);
semilogx(w,alfar,'r.');%barreira do desempenho
hold on;
t=linspace(-50,alfar);
semilogx(0.5,t,'r.');
w=linspace(0.01,0.7,1000);
semilogx(w,alfas,'r.');%barreira do desempenho
t=linspace(-50,alfas);
semilogx(0.7,t,'r.');

sysG=ss(A,B,C,D);
w=logspace(-2,3,100);
[sv,w]=sigma(sysG,w);   %valores singulares da planta nominal.
semilogx(w,20*log10(sv));

load barreira inverro;  %inverso do erro 
semilogx(w,20*log10(inverro),'r');%barreira da estabilidade
title('Sistema com integradores e barreiras de desempenho e estabilidade');
xlabel('frequência');
ylabel('dB');
grid;



%Determinação da malha objetivo

Ll = -inv(Cp*inv(Ap)*Bp);
Lh = -inv(Ap)*Bp*Ll;
%L = [Ll;Lh];

L =[-0.0527   -0.0594; % L =[-0.0527   -0.0594;
    0.0496   -0.0174;  %      0.0496   -0.0174;
   -0.4167  -28.2345;  %     -0.4167  -28.2345;
   1.0000       0;     %      1.0000         0;
   0            0;     %       0         0;
   -0.0000    0.0175]; %      -0.0000    0.0175];


w=logspace(-2,3,100);
w_ = i*w;

raizmi = 0.18; 
mi = raizmi^2;

for j=1:100,   
	Gml = (1/raizmi)*C*inv(w_(j)*eye(size(A))-A)*L;
	ML(:,j)=svd(Gml);
end;	
figure;
semilogx(w,20*log10(ML));
hold on;
w=linspace(0.01,0.5,1000);
semilogx(w,alfar,'r.');%barreira do desempenho
t=linspace(-50,alfar);
semilogx(0.5,t,'r.');
w=linspace(0.01,0.7,1000);
semilogx(w,alfas,'r.');%barreira do desempenho
t=linspace(-50,alfas);
semilogx(0.7,t,'r.');
w=logspace(-2,3,100);
semilogx(w,20*log10(inverro),'r');%barreira da estabilidade
title('G = C*inv(sI-A)*L e barreiras de desempenho e estabilidade');
xlabel('frequência');
ylabel('dB');
grid;


A_=A';
B_=(1/mi)*C'*C;
C_=L*L';
S = are(A_,B_,C_); %S é a matriz sigma maiúscula

H = (1/mi)*S*C';
for j=1:100,   
	Gkf = C*inv(w_(j)*eye(size(A))-A)*H;
   MO(:,j)=svd(Gkf);
   MOf(:,j)=svd(inv(eye(size(Gkf))+Gkf)*Gkf);
end;
figure;
semilogx(w,20*log10(MO));%valores singulares da malha objetivo
hold on;
w=linspace(0.01,0.5,1000);
semilogx(w,alfar,'r.');%barreira do desempenho
t=linspace(-50,alfar);
semilogx(0.5,t,'r.');
w=linspace(0.01,0.7,1000);
semilogx(w,alfas,'r.');%barreira do desempenho
t=linspace(-50,alfas);
semilogx(0.7,t,'r.');
w=logspace(-2,3,100);
semilogx(w,20*log10(inverro),'r');%barreira da estabilidade
title('Malha Objetivo e barreiras de desempenho e estabilidade');
xlabel('frequência');
ylabel('dB');
grid;


% % %Verificação da malha objetivo em malha fechada quanto a barreira de estabilidade
% % figure;
% % semilogx(w,20*log10(MOf));
% % hold on;
% % semilogx(w,20*log10(inverro),'r');%barreira da estabilidade
% % title('Malha Objetivo em malha fechada e barreira estabilidade');
% % xlabel('frequência');
% % ylabel('dB');
% % grid;



% Determinação do regulador linear quadrático 
ro=.00000001;%(1e-12);
Cr=C'*C;
Br=(1/ro)*B*B';
Ar=A;
K=are(Ar,Br,Cr);


G=(1/ro)*B'*K;
A_1=A-B*G-H*C;

sysK = ss(A_1,H,G,D);

sysGK = series(sysK,sysG);
%valores singulares de GK 
[AR,BR,CR,DR]=ssdata(sysGK);
for j=1:100,   
	GK = CR*inv(w_(j)*eye(size(AR))-AR)*BR+DR;
   Gnk(:,j)=svd(GK);
   CN(:,j)=svd(inv(eye(size(GK))+GK)*GK);
end;	
%Verificação da aproximação entre GK e Malha Objetivo
figure;
semilogx(w,20*log10(MO),'m');
hold on;
semilogx(w,20*log10(Gnk));

w=linspace(0.01,0.5,1000);
semilogx(w,alfar,'r.');%barreira do desempenho
t=linspace(-50,alfar);
semilogx(0.5,t,'r.');
w=linspace(0.01,0.7,1000);
semilogx(w,alfas,'r.');%barreira do desempenho
t=linspace(-50,alfas);
semilogx(0.7,t,'r.');
w=logspace(-2,3,100);
semilogx(w,20*log10(inverro),'r');%barreira da estabilidade

title('Malha Objetivo e GK');
xlabel('frequência');
ylabel('dB');
grid;

% %Regulador utilizando LTRY
% figure;
% q=[0];
% %a função LTRY gera o gráfico de GK mas não gera o da MO
% [Af,Bf,Cf,Df,svl]=ltry(A,B,C,D,H,Cr,[ro 0;0 ro],q,w);
% sysKf = ss(Af,Bf,Cf,Df);
% sysGKf = series(sysKf,sysG);
% figure;grid;sigma(sysGKf,w);


%Verificação do sistema nominal com integradores em malha fechada quanto a barreira de estabilidade
% % figure;
% % semilogx(w,20*log10(inverro),'r');%barreira da estabilidade
% % hold on;grid;
% % semilogx(w,20*log10(CN));%valores singulares da malha fechada
% % title('Sistema nominal com compensador GK em malha fechada e barreira da estabilidade');
% % xlabel('frequência');
% % ylabel('dB')