% kaalumaatriksid optimaalsele sünteesile
q11 = 1.5/(X1s^2); % asendi kaalutegur, valitud max järgi
q22 = 1/(0.2^2); % nurga kaalutegur, 
q33 = 0.01; %x3 kaalutegur, 0 sest pole piiranguid
q44 = 0;
Q=diag([q11 q22 q33 q44]) %olekumuutujate kaalumaatriks
R=1/(Umax^2) % sisendmuutuja kaalutegur
K=lqr(A,B,Q,R) % olekuregulaatori optimaalne süntees

% diskreetaja olekumudeli arvutamine
td=0.5 % diskreetimistakt
[Ad,Bd]=c2d(A,B,td)
[Ad,Gd]=c2d(A,G,td)

% Diskreetne süntees
Kd=dlqr(Ad,Bd,Q,R) % olekuregulaatori optimaalne süntees diskreetajas

% laiendatud olekumudeli maatriksite A2d, B2d moodustamine:
A2d=[Ad zeros(4,1); -C*Ad eye(1)];
B2d=[Bd; -C*Bd];
rank(ctrb(A2d,B2d)) % juhitavuse maatriksi astak

q55= 0.1 % väljundi "integraalse" vea kaalutegur
Q2d=diag([5*q11 q22 q33 q44 q55*td*td]) %
K2d=dlqr(A2d,B2d,Q2d,R)%

%Olekutaastajaga
rank(obsv(Ad,C)) %
Zot=Z*0.5 %
Ld=place(Ad',C',Zot)'
