g=9.81;
l=0.225;
t=0.01;
k=1;

A = [0      1              0           0;
     0      -1/t           0           0;
     0      0              0           1;
     0      1/(l*t)        g/l            0]
B = [     0; 
        k/t;
          0;
        -k/(l*t)]

C = [1 0 0 0;
     0 0 1 0];
D = [0;0];
x=1;
y=1;
Q=[x 0 0 0;
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];
R = 1;
K = lqr(A,B,Q,R)
Ac = [(A-B*K)];
Bc = [B];
Cc = [C];
Dc = [D];

T=0:0.01:10;
U=1*ones(size(T));
[Y,X]=lsim(Ac,Bc,Cc,Dc,U,T);
plot(T,Y)
legend('Vozik (x)','Kyvadlo (phi)')

