% 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);
[Ad,Gd]=c2d(A,G,td);

%järgivsüsteemi süntees
% laiendatud olekuvektoriga olekumudeli maatriksite A2d, B2d moodustamine:
A2d=[Ad zeros(4,1); -C*Ad eye(1)];
B2d=[Bd; -C*Bd];
q55=20*(td^2) %väljundi järgi tagasiside muutuja "integraalse" vea kaalutegur
q11=13/(X1s^2) %suurendasime ülereguleerimise vähendamiseks
Q2d=diag([q11 q22 q33 q44 q55])

K2d=dlqr(A2d,B2d,Q2d,R)