clear all
% close all
% clc

M=10;    %mass of the cart          [kg]
m=2;     %mass of the pendulum      [kg]
l=1;     %lenght                    [m]
ra=10;   %armature resistance       [ohm]
rr=.1;   %radius of the wheels      [m]
ke=2;    %constant                  [N*m/A]
kc=2;    %constant                  [V*s/rad]
g=9.81;
T = 0.1; %discretization period     [s]

%% Define the matrices of the discretized system

A = [0  1  0      0;
     0 -4 -1.9620 0;
     0  0  0      1;
     0  4 11.7720 0];

B = [0;
     0.2;
     0;
    -0.2];

[Ad,Bd] = c2d(A,B,T);
  

%% (a) - Check if it is possible to achieve offset-free tracking 
%        when the position of the cart is measured

C2 = [1 0 0 0];

Sigma2 = [Ad-eye(size(Ad)) Bd; C2 0];
det(Sigma2)                           % ~= 0 

% Since det(Sigma) ~= 0, it is possible to achieve offset-free tracking

% Create the matrices of the augmented dynamics with eta = [x' v]' where
... v(k) is the integration variable with dynamics v(k+1) = v(k) + (y0(k)-
... y(k))

Abar = [Ad zeros(size(Ad,1),1);
       -C2          1         ];
    
Bbar = [Bd;
        0];
    
    
% Check the reachability of the augmented dynamics

rank(ctrb(Abar,Bbar))     % = 5. (Abar,Bbar) is reachable
    
% Design the LQR controller

% Specify the maximum absolute values of states and input
xmax = [sqrt(5); sqrt(0.05); sqrt(5); sqrt(0.05); sqrt(0.05)];
umax = sqrt(500);

% Specify the initial values of the free parameters
q_tilde = 0.5;
r_tilde = 0.5;

% Compute Q and R matrices using normalization approach
Q = diag(q_tilde./(xmax.^2));
R = r_tilde/umax^2;

% Compute the comtroller gain K 
K = -dlqr(Abar,Bbar,Q,R);
eig(Abar+Bbar*K)

%% (b) - Improvement of the controller - adapt Q matrix
% Download LQ_integrator_pendulum_anim_sol.slx and run it. You
% can see from this simulation that the steady-state performance of the 
% current controller is not satisfactory. Therefore, design a new 
% controller to improve the steady-state performance.

% In order to improve the tracking performance, change Q(5,5) element,
% which weights the integrator state 
Q(5,5) = 50;

K = -dlqr(Abar,Bbar,Q,R);
eig(Abar+Bbar*K)

% Run the same simulation file with this new controller K. We now see that
% the new controller has a better performance compared to the old one. 

%% (c) - Improvement of the controller - scale the eigenvalues
% Now, we want to impose a prescribed level of stability, i.e., we will
% make sure to enforce the closed-loop poles of the system to be inside a
% ball of radius 1/alpha for alpha>1, instead of the unit ball. This then
% amounts to scaling the matrices Abar and Bbar when designing the LQR
% controller, as seen in the class. 

alpha = 1.1;
Ahat = Abar*alpha;
Bhat = Bbar*alpha;

Q = diag(q_tilde./(xmax.^2));
R = r_tilde/umax^2;

K = -dlqr(Ahat,Bhat,Q,R);
eig(Abar+Bbar*K)

% Run the same simulation file with this new controller K. We now see that
% the new controller has a better performance compared to the old one in 
% part (a).