% Exemplo 2

clear all
close all 
clc

%% Sistema do Pendulo linearizado (lista 1-03) em torno do ponto de equilíbrio xe=0 

% Valor dos parametros do sistema
m=0.1; b=0.015; L=0.2; g=9.81; u=0;

% Matriz linearizada do sistema em torno de xe=[0  0]
A1=[0 1;(-g/L) -(b/(m*L^2))];
x0=[pi/6 0];
lambda=eig(A1);
lambdamax=max(lambda);

%% Utilizando o Yalmip 
% 
% nA=size(A1,2); % Definindo a dimensão de A1 
% X1 = sdpvar(nA,nA);
% sigma=sdpvar(1,1);
% % mi=real(lambdamax);
% Restr = [X1>0 sigma>0];
% Restr=[Restr A1'*X1+X1*A1<sigma*eye(2)];
% % Configurando o YALMIP para usar o solver LMILAB.
% opts=sdpsettings;
% opts.solver='lmilab';
% opts.verbose=0;
% sol = solvesdp(Restr,sigma,opts);
% 
% che=min(checkset(Restr));
%     if che > 0
%     disp('O sistema pode ser controlado')
%     P1 = double(X1)
%     sigma=double(sigma)
%     Autovalor_AN = [];
%         else
%         disp('o sistema não pode ser controlado')
%     end
% 
% Q1=A1'*P1+P1*A1;
% disp('Matriz Q1 obtida pelo Yalmip')
% disp(Q1)

%% Utilizando o Yalmip 

mimin=0;
mi=0.1:0.1:3;


for i=1:length(mi)
    
nA=size(A1,2); % Definindo a dimensão de A1 
X2 = sdpvar(nA,nA);

Restr = [X2>0];
Restr=[Restr (A1+mi(i)*eye(nA))'*X2+X2*(A1+mi(i)*eye(nA))<0];
% Configurando o YALMIP para usar o solver LMILAB.
opts=sdpsettings;
%opts.solver='lmilab';
opts.verbose=0;
sol = solvesdp(Restr,[],opts);

che=min(checkset(Restr));
    if che > 0
    disp('O siste]ma pode ser controlado')
    Pmin = double(X2);
    Autovalor_AN = [];
    if mi(i)>mimin
    mimin=mi(i);
    P2=Pmin;
    end
    else
    break
    end
end

disp(P2)
disp(mimin)


Q2=A1'*P2+P2*A1;
disp('Matriz Q2 obtida pelo Yalmip')
disp(Q2)

save parameters  m b L g u mimin

%% Para solução do Sistema
t=0:0.001:5;
[t,y] = ode45('funcao1',t,x0);
figure(1)
plot(t,y)
axis([0 5 -2.8 1.5])

[t,y] = ode45('funcao2',t,x0);
figure(2)
plot(t,y)
axis([0 5 -2.8 1.5])