close all
clear all
%integrador duplo
A=[0 1;0 0];B=[0 1]'; Q=[1 0;0 0];R=1/10;
K=lqr(A,B,Q,R);
L=ss(A,B,K,0);
Lf=tf(L);
figure(1)
nyquist(L);
axis([-4 4 -4 4])
hold on
x=[-2:0.01:0];

plot(x,sqrt(1-(1+x).^2),x,-sqrt(1-(1+x).^2))




%pendulo invertido


clear all

clc

%parametros do sistema
M=2; m=0.1; l=0.5; I=0;g=9.8;

%matrizes do sistema linearizado na forma dx=Ax+Bu

den=(I+(m*(l^2)))-((m*l)^2)/(M+m);
a21=(m*g*l)/den;
b2= -(m*l)/((M+m)*den);
a41=-(m^2)*g*(l^2)/((M+m)*den);
b4=(1/(M+m))+(((m^2)*(l^2))/(((M+m)^2)*den));

A=[0 1 0 0; a21 0 0 0;0 0 0 1; a41 0 0 0];
B=[0; b2; 0; b4];

%matrizes Q e R do indice de desempenho quadratico
Q1=[100 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
R1=[1];

Q2=[1000 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
R2=[0.1];

Q3=[10 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
R3=[10];



%o valor da matriz de ganho K pode ser dado a partir do comando lqr
%K=> matriz de ganho
%P=> soluçao P da equaçao de Riccati
%E=> matriz de autovalores do sistema realimentado

[K,P,E]=lqr(A,B,Q1,R1);

L=ss(A,B,K,0);
figure(2)

nyquist(L);
axis([-4 4 -4 4])
hold on

x=[-2:0.01:0];

plot(x,sqrt(1-(1+x).^2),x,-sqrt(1-(1+x).^2))

%Controle de sistema de suspensão magnética
clear all

clc

%

L0=0.0249;  a=0.00672; ze=0.0045; m=0.0226; 
RR=19.9; L=0.52; g=9.81;

% pontos de equilibrio i0 e v0 para z0=0.005

%mg=-f(ze,ie)

ie=sqrt((m*g*2*a/L0)*(1+ze/a));
%ie=0.447
%k1 e k2
k1=(L0*ie)/(a*(1+(ze/a))^2);
k2=(L0*(ie^2))/(a^2*(1+(ze/a))^3);


%Matriz de estados A e matriz de controle B
A=[0 1 0;k2/m 0 -k1/m;0 0 -RR/L];
B=[0;0;1/L];


% diagonal da matriz Q
d1=1000;
d2=0;
d3=0;

%Matrizes Q e R do índice de desempenho quadrático
%caso 1
R1=0.0001;
Q1=[d1 0 0;0 d2 0;0 0 d3];

%K => Matriz de Ganho de Retroação
%P => Solução P da equação de Riccati
[K,P]=lqr(A,B,Q1,R1);

L=ss(A,B,K,0);
figure(3)
nyquist(L);
hold on
%circulo centrado em (-1.0) de raio 1


x=[-2:0.01:0];
w=logspace(-1,3);
plot(x,sqrt(1-(1+x).^2),x,-sqrt(1-(1+x).^2))
T=feedback(L,1);
Tf=tf(T);
figure(4)
bodemag(Tf,w);delta=1/norm(T,inf)