% TSC - LEZIONE 33
%% controllo ottimo LQ di un doppio integratore nel caso a orizzonte finito e infinito
%% implementazione in MATLAB
%
%
%% 1.  modello di sistema da controllare
% \dot{\mathbf{x}} \;=\left\lbrack \begin{array}{c}
% \dot{p} \;\\
% \ddot{p} \;
% \end{array}\right\rbrack =\;\mathit{\mathbf{F}}\left\lbrack \begin{array}{c}
% p\\
% \dot{p}
% \end{array}\right\rbrack +\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

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

% osservazione: il sistema è raggiungibile

R = [g F*g]
R = ctrb(F,g)
rkR = rank(R)

%
%% CONTROLLO OTTIMO A ORIZZONTE FINITO
%% 2. funzionale costo
% \mathrm{J}={\mathbf{x}}^{\top } \left(\mathrm{T}\right)\textrm{Sx}\left(\mathrm{T}\right)+\int_0^T {\mathbf{x}}^{\top } \left(\mathrm{t}\right)\textrm{Qx}\left(\mathrm{t}\right)+{\mathrm{u}}^{\top } \textrm{ru}\left(\mathrm{t}\right)\;\textrm{dt}         con         T=5, \mathbf{Q}=\mathbf{S}=\left\lbrack \begin{array}{cc}
% 1 & 0\\
% 0 & 1
% \end{array}\right\rbrack

global T Q
T = 5
Q = eye(2)
S = eye(2)

%
% \Longrightarrowsoluzione
% u_T^{\star } \left(t\right)=-\frac{1}{r}{\mathbf{g}}^{\top } \mathbf{M}\left(t\right)\mathbf{x}\left(t\right)
% dove \mathbf{M}\left(t\right)è soluzione dell'equazione differenziale di Riccati (EDR)
% \begin{array}{l}
% -\dot{\mathbf{M}} \left(\mathrm{t}\right)={\mathbf{F}}^{\top } \mathbf{M}\left(\mathrm{t}\right)+{\mathbf{M}\left(\mathrm{t}\right)\mathbf{F}}^{\top } +\mathbf{Q}-\frac{1}{\mathrm{r}}\mathbf{M}\left(\mathrm{t}\right){\mathbf{g}}^{\top } \textrm{gM}\left(\mathrm{t}\right)\\
% \mathbf{M}\left(\mathrm{T}\right)=\mathbf{S}
% \end{array}
%
%% CONTROLLO OTTIMO A ORIZZONTE INFINITO
%% 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}         con         \mathbf{Q}=\left\lbrack \begin{array}{cc}
% 1 & 0\\
% 0 & 1
% \end{array}\right\rbrack

global Q
Q = eye(2)

% \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 r
%

global r
r=10^0.93
r = 1;

%% CASO A ORIZZONTE FINITO

t0 = 0;

% risoluzione numerica della EDR
global solMvec
solMvec = ode45(@REfun, [t0 T], reshape(S, 4, 1));

% calcolo della dinamica retroazionata
[t, x] = ode45(@sysCLfun, [t0 T], x_0);

% determinazione della legge di controllo ottima
M_T = deval(solMvec,T-t);
k_T = zeros(length(t),2);
u_T = zeros(length(t),1);
for t_idx = 1:length(t)
    M_T_t = reshape(M_T(:,t_idx),2,2);
    k_T(t_idx,:) = g.'*M_T_t./r;
    k_T_t = k_T(t_idx,:);
    x_t = x(t_idx,:);
    u_T(t_idx) = -k_T_t*x_t';
end

%%  CASO A ORIZZONTE INFINITO

% determinazione della legge di controllo ottima
[k_inf, m_inf] = lqr(F, g, Q, r);
sysCLinf = ss(F-g*k_inf, g, eye(2), []);
[x_inf,t_inf] = initial(sysCLinf, x_0, T);
u_inf = -k_inf*x_inf.';

%
%% a. confronto soluzione dell'EDR {\mathrm{M}}_T \left(\mathrm{t}\right) e dell'EAR {\mathrm{M}}_{\infty }

tit = ["$\mathbf{M}_{1,1}$","$\mathbf{M}_{1,2}$","$\mathbf{M}_{2,1}$","$\mathbf{M}_{2,2}$"];
figure()
for t_idx=1:4
    subplot(2,2,t_idx)
    hold on
    title(tit(t_idx),'interpreter','latex')
    xlabel("time $t \ [s]$",'interpreter','latex')
    plot(t, M_T(t_idx,:), "LineWidth",1)
    plot(t([1,end]),m_inf(t_idx)*[1,1],"LineWidth",1)
    legend("fin. horizon","inf. horizon",'Location',"southwest")
end

%
%% b. confonto matrice di retroazione \mathbf{K}\left(t\right) e {\mathbf{K}}_{\infty }

tit=["$k_1$","$k_2$"];
k_T = k_T';
figure()
for t_idx=1:2
    subplot(2,1,t_idx)
    hold on
    title(tit(t_idx),'interpreter','latex')
    xlabel("time $t \ [s]$",'interpreter','latex')
    plot(t,k_T(t_idx,:), "LineWidth",1)
    plot(t([1,end]),k_inf(t_idx)*[1,1],"LineWidth",1)
    legend("fin. horizon","inf. horizon",'Location',"west")
end

%
%% c. confonto ingressi di controllo u\left(t\right) e u_{\infty } \left(t\right)

figure()
hold on
title("control inputs")
xlabel("time $t \ [s]$",'interpreter','latex')
plot(t, u_T, "LineWidth",1)
plot(t_inf,u_inf,"LineWidth",1)
legend("fin. horizon","inf. horizon",'Location',"southeast")

%
%% d. confonto evoluzioni in catena chiusa

tit=["$x$","$\dot{x}$"];
figure()
for t_idx=1:2
    subplot(2,1,t_idx)
    hold on
    title(tit(t_idx),'interpreter','latex')
    xlabel("time $t \ [s]$",'interpreter','latex')
    plot(t, x(:,t_idx), "LineWidth",1)
    plot(t_inf,x_inf(:,t_idx),"LineWidth",1)
    legend("fin. horizon","inf. horizon",'Location',"best")
end   

%
%% e. mappa poli-zeri del sistema retroazionato

figure
pzplot(sysCLinf)
a = findobj(gca,'type','line');
set(a,'MarkerSize',15,'Linewidth',1.5);
sgrid()