% Optimaaljuhtimse sümtees
% Kaalutegurite valikul qii > 1/ Xi_max^2, rjj > 1/Uj_max^2
q11=3/(X1s^2) % x1 siirde maks
q22=1/(0.2^2) % x2 maks on 0.2 rad
q33=1/(1.3^2) % x3 mittenulliseks ülereguleerimise piiramiseks 2 cm
q44=0
Q=diag([q11 q22 q33 q44]) %olekumuutujate kaalumaatriks
R=1/(10^2) % U_max on 10
K=lqr(A,B,Q,R) % olekuregulaatori maatriksi optimaalne süntees

td=0.2 % diskreetimistakt
[Ad,Bd]=c2d(A,B,td) %olekumudeli teisendus diskreetaega
[Ad,Gd]=c2d(A,G,td) %olekuhäiringu maatriksi teisendus diskreetaega

%diskreetajas
A2d=[Ad zeros(4,1); -C*Ad eye(1)];
B2d=[Bd; -C*Bd];
q55=1*(td^2) % asendi integraalse vea kaalutegur
Q2d=diag([q11 q22 q33 q44 q55]) %olekumuutujate kaalumaatriks
K2d=dlqr(A2d,B2d,Q2d,R)