% Finding a LQR controller for the Acrobot
% to control it close to equilibrum

% Definition of the linearized system 
% x' = A x + B u
% x = [q1 - pi/2 , q2 , q1', q2']
%

% The A matrix comes from the Jacobian of the file jacobian_acrobot.m
% The B vector comes from f_torque of the file jacobian_acrobot.m
% Below , they are the results for
% m1 = 1.0;
% m2 = 1.0;
% l1 = 1.0;
% l2 = 1.0;
% And a time step of Ts = 0.01;

A = zeros(4,4);
A(1,1) = 0;
A(1,2) = 0;
A(1,3) = 1;
A(1,4) = 0;

A(2,1) = 0;
A(2,2) = 0;
A(2,3) = 0;
A(2,4) = 1;

A(3,1) = 12.60;
A(3,2) = -12.60;
A(3,3) = 0;
A(3,4) = 0;

A(4,1) = -16.80;
A(4,2) = 46.20;
A(4,3) = 0;
A(4,4) = 0;

B = zeros(4,1);
B(1,1) = 0;
B(2,1) = 0;
B(3,1) = -30.0/7.0;
B(4,1) = 96.0 / 7.0;

sys = ss(A,B,[1,0,0,0],0);

Ts = 0.01;
Q = Ts*eye(4);
R = Ts;

% Finding the feedback law of the LQR controller
% u = -K x

[Kd,S,e] = lqrd(A,B,Q,R,Ts)