%% ELEC-E8116 Model-based control systems
%% Exercise 9

clearvars; close all; clc;

% G(s)=\frac{Y(s)}{U(s)}=\frac{s+0.5}{s^2+2s+4} and the criterion to be minimized J = \int_0^{\infty}(3y^2+0.5u^2)dt
% Note that Matlab’s lqr function uses the cost J = \int_0^{\infty}(x^TQx+u^TRu)dt
% Thus the given criterion becomes
%
%
% J = \int_0^{\infty}(3y^2+0.5u^2)dt = \int_0^{\infty}(3y^Ty+0.5u^2)dt = \int_0^{\infty}(3\underbrace{(Cx)^T}_{x^TC^T}Cx+0.5u^2)dt \\
%
% = \int_0^{\infty}(x^T(3C^TC)x+0.5u^2)dt
%

% Process model
s = tf('s');
G = (s+0.5)/(s^2+2*s+4);
Gss = ss(G);
[A,B,C,D] = ssdata(Gss);

% Cost function 
Q = 3*(C')*C;
R = 0.5;

%
% The process and optimal controller are
% \dot{x} = Ax+Bu \\
% y = Cx \\
% u = -Kx
% Which leads to
% \dot{x} = Ax-BKx = (A-BK)x \\
% y = Cx \\
% Note that in the materials K is indicated with the letter L.
% Calculate the damping ratio of the closed loop system.

% Optimal control law u = -Kx, regulator problem
[K,S,P] = lqr(A, B, Q, R);
damp(A-B*K)

% Damping ratio is 7.87e-01.
%
%
% Simulate the system by letting the reference signal be zero (regulator problem) and letting the initial states be non-zero.
% The simulation result using the initial conditionx_0 =\left\lbrack -1,\;1\right\rbrack is shown in the figure.  Note that it shows the output signal y.  If both states should be plotted, you should have defined the C-matrix as an identity matrix.  The system would not change, but both states would be defined as outputs.

% C = eye(2);

% Closed loop 
Gclss=ss(A-B*K,zeros(2,1),C,0); 

% Simulation 
x0=[-1,1]'; 
t=0:0.01:10; 
U=zeros(size(t)); 
lsim(Gclss,U,t,x0) 

%
% Then consider the tracking problem.  Use a static pre-compensator to set the static gain of the closed-loop system to the value 1.  Then simulate the system for a step change in the reference signal.
% As for the servo problem, the controller is u = -Kx + Tr where T is a constant and r the reference.  The closed loop is
% \dot{x} = Ax-BKx = (A-BK)x + TBr \\
% y = Cx \\
% The corresponding transfer function from reference to output is
% G_{CL} = TC(sI-A+BK)^{-1}B
% Note: in some materials pre-compensator is indicated for example with k, T or other letters.
% The static gain of G_{\textrm{CL}}should be 1
% G_{CL}(s=0) = 1 \\
% TC(-A+BK)^{-1}B = 1 \\
% T = \frac{1}{C(-A+BK)^{-1}B}

% Optimal control u = -Kx+Tr, servo problem 
T=1/(C*inv(-A+B*K)*B); 
G2clss=ss(A-B*K,T*B,C,0); 
figure(1); clf; 
step(G2clss) 

%
%