% TSC - LEZIONE 33
%% controllo ottimo LQ di un doppio integratore nel caso a orizzonte infinito
%% implementazione in Simulink
%
%
%% 1.  modello di sistema da controllare
% \dot{\mathbf{x}} \;=\left\lbrack \begin{array}{c}
% \dot{\mathrm{x}} \;\\
% \ddot{\mathrm{x}} \;
% \end{array}\right\rbrack =\;\textrm{Fx}+\mathbf{g}\;\mathrm{u}      con    \mathbf{F}=\left\lbrack \begin{array}{cc}
% 0 & 1\\
% 0 & 0
% \end{array}\right\rbrack  e \;\;\mathbf{g}=\left\lbrack \begin{array}{c}
% 0\\
% 1
% \end{array}\right\rbrack
%            {\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\mathbf{x}}_0 =\left\lbrack \begin{array}{c}
% 1\\
% 0
% \end{array}\right\rbrack

F = diag(1,1)
g = [0; 1]
x_0 = [1; 0]

%
%% 2. funzionale costo
% J_{\infty } =\int_0^{\infty } {\mathbf{x}}^{\top } \left(\mathrm{t}\right)\textrm{Qx}\left(\mathrm{t}\right)+{\mathrm{u}}^{\top } \textrm{ru}\left(\mathrm{t}\right)\;\textrm{dt}
% \Longrightarrowsoluzione
% u_{\infty }^{\star } \left(t\right)=-\frac{1}{r}{\mathbf{g}}^{\top } {\mathbf{M}}_{\infty } \mathit{\mathbf{x}}\left(t\right)
% dove {\mathbf{M}}_{\infty } è soluzione dell'equazione algebrica di Riccati (EAR)
% 0\;={\mathbf{F}}^{\top } \mathbf{M}+{\textrm{MF}}^{\top } +\mathbf{Q}-\frac{1}{\mathrm{r}}\mathbf{M}{\mathbf{g}}^{\top } \textrm{gM}
%
%% RUOLO del PARAMETRO Q
% definizione dei parametri Q e r

Q_1 = [1 0; 0 1]
Q_2 = [0.0001 0; 0 1]
Q_3 = [1000 0; 0 1]
Q_4 = [1 0; 0 0.0001]
Q_5 = [1 0; 0 1000]
r = 1

%
% calcolo della matrice di retroazione


k_inf_1 = lqr(F,g,Q_1,r)
k_inf_2 = lqr(F,g,Q_2,r)
k_inf_3 = lqr(F,g,Q_3,r)
k_inf_4 = lqr(F,g,Q_4,r)
k_inf_5 = lqr(F,g,Q_5,r)

%
% simulazione della dinamica controllata

k_inf = k_inf_1;
res_1 = sim('doppio_integratore.slx');
u_1 = res_1.u.data;
x_1 = res_1.x.data;
x_dot_1 = x_1(:,2);
x_1 = x_1(:,1);

k_inf = k_inf_2;
res_2 = sim('doppio_integratore.slx');
u_2 = res_2.u.data;
x_2 = res_2.x.data;
x_dot_2 = x_2(:,2);
x_2 = x_2(:,1);

k_inf = k_inf_3;
res_3 = sim('doppio_integratore.slx');
u_3 = res_3.u.data;
x_3 = res_3.x.data;
x_dot_3 = x_3(:,2);
x_3 = x_3(:,1);

k_inf = k_inf_4;
res_4 = sim('doppio_integratore.slx');
u_4 = res_4.u.data;
x_4 = res_4.x.data;
x_dot_4 = x_4(:,2);
x_4 = x_4(:,1);

k_inf = k_inf_5;
res_5 = sim('doppio_integratore.slx');
u_5 = res_5.u.data;
x_5 = res_5.x.data;
x_dot_5 = x_5(:,2);
x_5 = x_5(:,1);

t = res_5.x.time;

%
% analisi del ruolo di Q

figure;
title("control inputs")
xlabel("time $t \ [s]$",'interpreter','latex')
plot(t,u_1, "LineWidth",1)
hold on
plot(t,u_2, "LineWidth",1)
hold on
plot(t,u_3, "LineWidth",1)
hold on
plot(t,u_4, "LineWidth",1)
hold on
plot(t,u_5, "LineWidth",1)
legend("opt1","opt2","opt3","opt4","opt5",'Location',"southeast")
figure; 
title("$x$",'interpreter','latex')
xlabel("time $t \ [s]$",'interpreter','latex')
plot(t,x_1, "LineWidth",1)
hold on
plot(t,x_2, "LineWidth",1)
hold on
plot(t,x_3, "LineWidth",1)
hold on
plot(t,x_4, "LineWidth",1)
hold on
plot(t,x_5, "LineWidth",1)
legend("opt1","opt2","opt3","opt4","opt5",'Location',"southeast")
figure;
title("$\dot{x}$",'interpreter','latex')
xlabel("time $t \ [s]$",'interpreter','latex')
plot(t,x_dot_1, "LineWidth",1)
hold on
plot(t,x_dot_2, "LineWidth",1)
hold on
plot(t,x_dot_3, "LineWidth",1)
hold on
plot(t,x_dot_4, "LineWidth",1)
hold on
plot(t,x_dot_5, "LineWidth",1)
legend("opt1","opt2","opt3","opt4","opt5",'Location',"southeast")
