%% 3
A=[0,1,0,0;
    -5000,-100/3,500,100/3;
    0,-1,0,1;
    0,100/3,-4,-60];
Bu=[0;25/3;0;-1];
Bz=[0;0;0;1];
Bksi=[1;0;0;0];
B=[Bu,Bz];
C=[0,0,1,0];
D=[0,0];
Du=[0];
Qf=7*1e-4;
Rf=1e-8;
Q=diag([1,1,50000,1]);
R=0.001;

K=lqr(A,Bu,Q,R);
H=lqe(A,Bksi,C,Qf,Rf);
Af=A-Bu*K-H*C;
Bf=-H;
Cf=-K;
Df=0;