% Find Total Inertia
Jt = Jr*Jp - mp^2*r^2*l^2;
% 
% State Space Representation
A = [0 0 1 0;
     0 0 0 1;
     0 mp^2*l^2*r*g/Jt  -br*Jp/Jt   -mp*l*r*bp/Jt 
     0  mp*g*l*Jr/Jt    -mp*l*r*br/Jt   -Jp*bp/Jt];
%
B = [0; 0; Jp/Jt; mp*l*r/Jt];
C = eye(2,4);
D = zeros(2,1);
% 
% Add actuator dynamics
A(3,3) = A(3,3) - km*km/Rm*B(3);
A(4,3) = A(4,3) - km*km/Rm*B(4);
B = km * B / Rm;

R = 1;

sys = ss(A,B,C,D)

Q = eye(4);
K = lqr(sys,Q,R);

Q = eye(4);
Q(1,1) = 20;
K_theta_high = lqr(sys, Q, R);

Q = eye(4);
Q(2,2) = 20;
K_alpha_high = lqr(sys, Q, R);

Q = eye(4);
Q(1,1) = 0.1;
K_theta_low = lqr(sys, Q, R);