%program na výpočet stavových zpětných vazeb – diskrétní systém%
g = 9.81;	%tihove zrychleni
L = 0.225;	%vzdalenost od teziste
t = 0.01;	%casova konstanta rychlostniho servopohonu
K = 1;		%zesileni rychlostniho servopohonu
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];
Ts=1/100;
[F,G,H,J]=c2dm (A,B,C,D,Ts,'zoh')
co = ctrb (F,G)
ob = obsv (F,H)
Controllability = rank (co)
Observability = rank (ob)
T=0:0.01:10;
U=0.1*ones(size(T));
x=1;	
y=1;	
Q=[x 0 0 0; 	%vahova matice zastoupeni stavu
   0 0 0 0;
   0 0 y 0;
   0 0 0 0];
R = 1;			%vahova matice zastoupeni rizeni
k = dlqr(F,G,Q,R)
[Y,X]=dlsim(F-G*k,G,H,J,U);
stairs(T,Y)
legend('Vozik (x)','Kyvadlo (phi)')