% Xs=[X1s;0;0;0]
% qii>= 1/(Xi_max^2) & rjj>=1/(Uj_max^2)
q11=3/(X1s^2) % x1 max võtame tema liikumise ulatuse
q22=1/(0.2^2) % x2 max on 0.2 rad
q33=1/(1.3^2) % mittenulliseks et piirata ülereguleerimist
q44=0  % ei arvesta juhtimiskriteeriumis üldse
Q=diag([q11 q22 q33 q44])
R=1/(10^2) % U max on 10 ühikut

K=lqr(A,B,Q,R) % olekuregulaatori maatriksi optimaalne süntees

td=0.2 
[Ad,Bd]=c2d(A,B,td);

Kd=dlqr(Ad,Bd,Q,R)