root/applications/doprava/texty/novotny_vyzk_LQ/Implementation/Minimalization.tex

% Kvadratické kritérium ve tvaru
% \begin{equation}\label{eq:J}
%  J = \sum_{t=0}^h x(t)^T Q x(t) + u(t)^T R u(t) \;,  
% \end{equation}
% kde $Q$ a $R$ jsou diagonální pozitivně definitní matice vah,
% rozepíšeme pomocí vztahu \ref{eq:prechod_mat_po_subs}
% a budeme minimalizovat pro jednotlivá $t$ podle $u(t)$.
% Proměnná $u(h)$ se v kritériu vyskytuje pouze ve členu
% \begin{equation}
% u(h)^T Ru(h)
% \end{equation}
% a tedy musí být $u(h) = 0$. $u(h - 1)$ se vyskytuje ve členech
% \begin{equation}
%  u(h - 1)^T Ru(h - 1) + (x(h - 1)^T A^T +u(h - 1)^T B^T ) Q ( A x(h - 1) + B u(h - 1) + I_0) \;,
% \end{equation}
% což lze pomocí složených vektorů a matic přepsat do tvaru
% \begin{equation} \label{eq:J_sloz}
%  (u(h-1)^T, x(h - 1)^T ) 
%   \left( \begin{array}{cc}
%           B^T \sqrt{Q}^T & \sqrt{R}^T \\
%         A^T \sqrt{Q}^T & 0
%         \end{array}
%   \right)  
%   \left( \begin{array}{cc}
%            \sqrt{Q} B & \sqrt{Q} A \\
%         \sqrt{R} & 0
%         \end{array}
%   \right)
%   \left( \begin{array}{c}
%          u(h-1) \\
%           x(h - 1)
%         \end{array}
%   \right) \;,
% \end{equation}
% kde symbolem $\sqrt{Q}$ myslíme matici, pro níž platí $\sqrt{Q}^T \sqrt{Q} = Q$.
% Matice $Q$ a $R$ jsou pozitivně definitní diagonální matice, takže $\sqrt{Q}$ bude také
% pozitivně definitní diagonální a její diagonální prvky budou rovny odmocnině
% příslušných prvků původní matice $Q$.
% Složenou matici
% \begin{equation}  
%   M_0 =  \left( \begin{array}{cc}
%            \sqrt{Q} B & \sqrt{Q} A \\
%         \sqrt{R} & 0
%         \end{array}
%   \right)
% \end{equation}
% můžeme pomocí QR dekompozice rozložit na součin 
% 
% \begin{equation}
%  M_0 = M_R M_Q ,
% \end{equation}
% kde $M_Q$ je horní trojůhelníková matice a $M_R$ je matice ortonormální.
% Pro ortonormální matici $M_R$ platí
% \begin{equation}
%  M_R^T M_R = I \;,
% \end{equation}
% neboť její zloupce jsou vzájemně ortogonální a normované na jednotku.
% Z toho plyne, že součin matic $M_0^T M_0$ vyskytující se v členu \ref{eq:J_sloz}
% můžeme pomocí QR rozlkladu převést na tvar
% \begin{equation}
%  M_0^T M_0 = ( M_R M_Q )^T  M_R M_Q = M_Q^T M_R^T M_R M_Q = M_Q^T M_Q \;,
% \end{equation}
% kde
% \begin{equation}
%  M_Q =  \left( \begin{array}{cc}
%           L_u & L \\
%         0 & L_q
%         \end{array}
%   \right) \;,
% \end{equation}
% je horní trojůhelníková matice.
% Člen \ref{eq:J_sloz} poté přejde na tvar
% \begin{equation}
%  (\Delta C(h-1)^T, x(h - 1)^T ) 
%   \left( \begin{array}{cc}
%           L_q^T & 0 \\
%         L^T & L_u^T
%         \end{array}
%   \right)  
%   \left( \begin{array}{cc}
%           L_u & L \\
%         0 & L_q
%         \end{array}
%   \right)
%   \left( \begin{array}{c}
%          u(h-1) \\
%           x(h - 1)
%         \end{array}
%   \right) \;,
% \end{equation}
% který můžeme dále upravit na
% \begin{equation}
%  ( L_u u(h-1) +  L_q x(h-1) )^T ( L_uu(h-1) +  L_q x(h-1) ) + x(h-1)^T L_q^T L_q x(h-1) 
% \end{equation}. Pokud tento člen chceme minimalizovat v proměnné $x(h-1)$, musí nutně platit
% \begin{equation}
% u(h-1) = - L_u^{-1} L x(h-1)
% \end{equation}. Zbývající nenulová část $x(h-1)^T L_q^T L_q x(h-1) $ lze přepsat pomocí rovnice \ref{eq:prechod} do tvaru 
% \begin{equation}
%  x(h-1)^T L_q^T L_q x(h-1) = ( A x(h-2) + B u(h-2) )^T L_q^T L_q ( A x(h-2) + B u(h-2) ) \;,
% \end{equation}
% kde se vyskytuje $u(t)$ pouze pro $t = h-2$. 
% Při minimalizaci podle $u(h-2)$ musíme zahrnout i tuto část, člen ve složeném maticovém zápisu
% tudíž nabyde tvaru
% \begin{equation}
%  (u(h-2)^T, x(h-2)^T ) 
%   \left( \begin{array}{ccc}
%           B^T \sqrt{Q}^T & \sqrt{R}^T & L_x A \\
%         A^T \sqrt{Q}^T & 0 & L_x B
%         \end{array}
%   \right)  
%   \left( \begin{array}{cc}
%            \sqrt{Q} B & \sqrt{Q} A \\
%         \sqrt{R} & 0 \\
%         L_x A & L_x B
%         \end{array}
%   \right)
%   \left( \begin{array}{c}
%          u(h-2) \\
%           x(h-2)
%         \end{array}
%   \right) \;.
% \end{equation}
% Matici
% \begin{equation}
%  M = \left( \begin{array}{cc}
%            \sqrt{Q} B & \sqrt{Q} A \\
%         \sqrt{R} & 0 \\
%         L_x A & L_x B
%         \end{array}
%   \right) = \left( \begin{array}{c}
%            M_0  \\
%         L_x A \; L_x B
%         \end{array}
%   \right)
% \end{equation}
% opět převedeme QR dekompozicí do horního trojůhelníkového tvaru a dostaneme vztah pro $u(h-2)$.
% Tento postup se analogicky aplikuje na každé $u(t)$. Matice $M_0$ je stále stjná, $L_x$ použijeme z předchozího kroku minimalizace.

\subsubsection{Implementace minimalizace}

Minimaizace kvadratického kritéria je v simulaci prováděna metoduo \texttt{mat L( const int horizont )} ve třídě
\texttt{ QuadraticMinimalizator }, které se jako parametry konstruktoru předávají matice \texttt{A}, \texttt{B}, \texttt{L} a \texttt{Q}.
Pro numerické výpočty je použite knihovna IT++, která umožňuje jednoduché operace s maticemi a vektory syntakticky podobně jako v jazyku
MATLAB. Jsou zde také implementovány jednoduché algoritmy, jako například pro naše účely potřebný QR rozklad.
Níže je vložen zdrojový kód metody pro minimalizaci kritéria a získání hodnot řídících parametrů. Proměnné jsou v kódu značeny
stejně jako v textu výše. Ty, které jsou v kódu použity navíc, jsou popsány v komentářích.

\begin{lstlisting}
 mat L( const int horizont ) {
        int xdim = A.cols(); // Dimenze vektoru x
        int udim = B.cols(); // Dimenze vektoru u
        mat sqQ = sqrt(Q);   // Odmocninove matice
        mat sqR = sqrt(R);
        mat M0, M;
        mat qrQ;
        mat L, Ls, Lx, Lu;
        // sestaveni matice M0
        M0 = concat_vertical(                           
                concat_horizontal( sqQ * B,     sqQ * A ),
                concat_horizontal( sqR,         zeros( udim, xdim ) )
            );
        M = M0;
        // hlavni cyklus
        for (int h = horizont; h >= 0; h --) {
            qr(M, L);  // QR rozklad matice M 
            // rozklad matice L na do slozek
            Lu = L(0,    udim-1,       0,    udim-1);     
            Lx = L(udim, udim+xdim-1,  udim, udim+xdim-1);
            Ls = L(0,    udim-1,       udim, udim+xdim-1);
            // kompozice matice M
            M = concat_vertical(
                    M0,
                    concat_horizontal(Lx * B, Lx * A )
                );
        }
        // navratova hodnota - vysledna matice L : u(t+1) = Lx(t)
        return - inv(Lu) * Ls;
    }
\end{lstlisting}