w=0.7291;
r=42,2;


As=[0 1 0 0;3*w*w 0 0 2*r*w;0 0 0 1;0 -(2*w)/r 0 0]
Bs=[0 0 ;1 0; 0 0 ; 0 1/r]
Cs=[1/r 0 0 0; 0 0 1 0]
%Cs=eye(4)

[p, n]=size(Cs);
Ag=[As zeros(n,p); -Cs zeros(p,p)]
Bg=[Bs; zeros(p,p)]
Cg=[Cs zeros(p,p)]


PO=10;
ts=5;
zita = abs(log(PO/100))/(sqrt(pi^2 + (log(PO/100))^2)); % scelgo ts e po = sovraelongazione
wn = 4/(zita*ts);
alpha = -zita*wn;
wd = wn*sqrt(1-zita^2);

p1 = alpha ;
p2 = alpha-0.00001;
p3=10*real(p1)
p4=10*p2;
p5=p3-2;
p6=p4-2;


K=place(Ag,Bg,[p1 p2 p3 p4 p5 p6]);





Kf=K(:,1:4)
Ki=K(:,5:end)



Q=diag([1 10 10 10 110 100]);
R=diag([10 10]);
Kottimo=lqr(Ag,Bg,Q,R)

Kfottimo=Kottimo(:,1:4);
Kiottimo=Kottimo(:,5:end);