Linear Quadratic Problem
In this blog, we revisit the LQR problem and introduce another factorization method:
\[\begin{aligned} \min_{\bar{x}, \bar{u}} \quad &\sum_{k=0}^{N-1} \begin{bmatrix} q_k \\ r_k \end{bmatrix}^\top \begin{bmatrix} x_k \\ u_k \end{bmatrix} + \frac{1}{2} \begin{bmatrix} x_k \\ u_k \end{bmatrix}^\top \begin{bmatrix} Q_k & S_k^\top \\ S_k & R_k \end{bmatrix} \begin{bmatrix} x_k \\ u_k \end{bmatrix} \\ &\quad\quad + p_N^\top x_N + \frac{1}{2} x_N^T P_N x_N \\ \text{s.t.}\quad & x_{k+1} = A_k x_k + B_k u_k + c_k,\; k = 0, \dots, N-1, \end{aligned}\]where $\bar{x} := \{ x_1, \dots, x_N \}$ and $\bar{u} := \{ u_0, \dots, u_{N-1} \}$. We consider the LQR problem as a quadratic program with equality constraints. The corresponding Lagrangian is then expresed as
\[\begin{aligned} \mathcal{L} (\bar{x}, \bar{u}, \bar{\lambda}) &= \sum_{k=0}^{N-1} \Bigg( \begin{bmatrix} q_k \\ r_k \end{bmatrix}^\top \begin{bmatrix} x_k \\ u_k \end{bmatrix} + \frac{1}{2} \begin{bmatrix} x_k \\ u_k \end{bmatrix}^\top \begin{bmatrix} Q_k & S_k^\top \\ S_k & R_k \end{bmatrix} \begin{bmatrix} x_k \\ u_k \end{bmatrix} + \lambda_{k+1}^\top \left(A_k x_k + B_k u_k + c_k - x_{k+1} \right) \Bigg) \\ &\quad\quad\quad + p_N^\top x_N + \frac{1}{2} x_N^T P_N x_N, \end{aligned}\]where $\bar{\lambda} = (\lambda_1, \cdots, \lambda_N)$ is a vector of Lagrange multipliers. The KKT conditions are derived as
\[\begin{aligned} \frac{\partial \mathcal{L}}{\partial \bar{x}} = 0,\; \frac{\partial \mathcal{L}}{\partial \bar{u}} = 0,\; \frac{\partial \mathcal{L}}{\partial \bar{\lambda}} = 0. \end{aligned}\]For $k = 0$,
\[\begin{aligned} 0 &= r_0 + R_0 u_0 + S_0 x_0 + B_0^\top \lambda_{1}. \end{aligned}\]For $0 < k <= N-1$,
\[\begin{aligned} 0 &= q_k + Q_k x_k + S_k^\top u_k + A_k^\top \lambda_{k+1} - \lambda_k , \\ 0 &= r_k + R_k u_k + S_k x_k + B_k^\top \lambda_{k+1}, \\ 0 &= A_{k-1} x_{k-1} + B_{k-1} u_{k-1} + c_{k-1} - x_k. \end{aligned}\]For $k = N$,
\[\begin{aligned} 0 &= p_N + P_N x_N - \lambda_N , \\ 0 &= A_{N-1} x_{N-1} + B_{N-1} u_{N-1} + c_{N-1} - x_N. \end{aligned}\]The corresponding KKT system $(N = 3)$ is then
\[\begin{equation*} \left[ \begin{array}{cccccc|ccc} R_0 & 0 & 0 & 0 & 0 & 0 & B_0^\top & 0 & 0 \\ 0 & Q_1 & S_1^\top & 0 & 0 & 0 & -I & A_1^\top & 0 \\ 0 & S_1 & R_1 & 0 & 0 & 0 & 0 & B_1^\top & 0 \\ 0 & 0 & 0 & Q_2 & S_2^\top & 0 & 0 & -I & A_2^\top \\ 0 & 0 & 0 & S_2 & R_2 & 0 & 0 & 0 & B_2^\top \\ 0 & 0 & 0 & 0 & 0 & P_3 & 0 & 0 & -I \\ \hline B_0 & -I & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & A_1 & B_1 & -I & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & A_2 & B_2 & -I & 0 & 0 & 0 \\ \end{array} \right] \left[ \begin{array}{c} u_0 \\ x_1 \\ u_1 \\ x_2 \\ u_2 \\ x_3 \\ \hline \lambda_1 \\ \lambda_2 \\ \lambda_3 \\ \end{array} \right] = \left[ \begin{array}{c} -r_0 - S_0 x_0 \\ -q_1 \\ -r_1 \\ -q_2 \\ -r_2 \\ -p_3 \\ \hline -A_0 x_0 - c_0 \\ -c_1 \\ -c_2 \\ \end{array} \right] \end{equation*}\]For notational convenience, let $\bar{w} = (u_0, x_1, u_1, \dots, x_3)$.
\[\begin{equation} \begin{bmatrix} H & E^\top \\ E & 0 \end{bmatrix} \begin{bmatrix} \bar{w} \\ \bar{\lambda} \end{bmatrix} = \begin{bmatrix} r_w \\ r_{\lambda} \end{bmatrix} \end{equation}\]Assuming that $H$ is positive definite, we first eliminate $\bar{w}$ from the first row:
\[\begin{equation} \bar{w} = - H^{-1} E^\top \bar{\lambda} + H^{-1} r_{w} \end{equation}\]Substituting this expression into the second row of the KKT system:
\[\begin{aligned} E \bar{w} &= r_{\lambda} \\ E \left(- H^{-1} E^\top \bar{\lambda} + H^{-1} r_{w} \right) &= r_{\lambda} \\ -E H^{-1} E^\top \bar{\lambda} + E H^{-1} r_{w} &= r_{\lambda} \end{aligned}\]Rearranging to solve for $\bar{\lambda}$:
\[\begin{equation} E H^{-1} E^\top \bar{\lambda} = E H^{-1} r_{w} - r_{\lambda} \end{equation}\]The matrix $Y := E H^{-1} E^\top$ is called the Schur complement of $H$ in the original KKT matrix. This gives us the reduced system:
\[\begin{equation} Y \bar{\lambda} = E H^{-1} r_{w} - r_{\lambda} \end{equation}\]The Schur complement method for solving the LQP can be summarized in the following steps:
- Form the Schur complement: Compute $Y = E H^{-1} E^\top$
- Form the reduced right-hand side: Compute $\tilde{r} = E H^{-1} r_{w} - r_{\lambda}$
- Solve for Lagrange multipliers: Solve $Y \bar{\lambda} = \tilde{r}$
- Recover primal variables: Compute $\bar{w} = H^{-1} (r_{w} - E^\top \bar{\lambda})$
Sparsity Structure
The matrix $H$ has a block-diagonal structure, making $H^{-1}$ computationally efficient to obtain. Each block corresponds to the Hessian of the cost function at each time step. For our LQP, $H$ has the structure:
\[H = \begin{bmatrix} R_0 & 0 & 0 & 0 & 0 & 0 \\ 0 & Q_1 & S_1^\top & 0 & 0 & 0 \\ 0 & S_1 & R_1 & 0 & 0 & 0 \\ 0 & 0 & 0 & Q_2 & S_2^\top & 0 \\ 0 & 0 & 0 & S_2 & R_2 & 0 \\ 0 & 0 & 0 & 0 & 0 & P_3 \\ \end{bmatrix}\]Due to the block-diagonal structure, $H^{-1}$ can be computed as:
\[H^{-1} = \begin{bmatrix} \tilde{R}_0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \tilde{Q}_1 & \tilde{S}_1^\top & 0 & 0 & 0 \\ 0 & \tilde{S}_1 & \tilde{R}_1 & 0 & 0 & 0 \\ 0 & 0 & 0 & \tilde{Q}_2 & \tilde{S}_2^\top & 0 \\ 0 & 0 & 0 & \tilde{S}_2 & \tilde{R}_2 & 0 \\ 0 & 0 & 0 & 0 & 0 & \tilde{P}_3 \\ \end{bmatrix}\] \[\begin{bmatrix} \tilde{Q}_k & \tilde{S}_k^\top \\ \tilde{S}_k & \tilde{R}_k \end{bmatrix} = \begin{bmatrix} Q_k & S_k^\top \\ S_k & R_k \end{bmatrix}^{-1}\]Next, we discuss the structure of the Schur complement $Y$. From our KKT system, the constraint matrix $E$ has the structure:
\[E = \begin{bmatrix} B_0 & -I & 0 & 0 & 0 & 0 \\ 0 & A_1 & B_1 & -I & 0 & 0 \\ 0 & 0 & 0 & A_2 & B_2 & -I \end{bmatrix}\]The Schur complement $Y = E H^{-1} E^\top$ can be computed block by block. Let’s denote:
\[Y = \begin{bmatrix} Y_{11} & Y_{12} & Y_{13} \\ Y_{21} & Y_{22} & Y_{23} \\ Y_{31} & Y_{32} & Y_{33} \end{bmatrix}\]Each block $Y_{ij}$ is computed as:
\[\begin{aligned} Y_{11} &= B_0 \tilde{R}_0 B_0^\top + \tilde{Q}_0 \\ Y_{12} &= - \tilde{Q}_1 A_1^\top - \tilde{S}_1^\top B_1^\top \\ Y_{13} &= 0 \\ Y_{22} &= A_1 \tilde{Q}_1 A_1^\top + A_1 \tilde{S}_1^\top B_1^\top + B_1 \tilde{S}_1 A_1^\top + B_1 \tilde{R}_1 B_1^\top + \tilde{Q}_2 \\ Y_{23} &= - \tilde{Q}_2 A_2^\top - \tilde{S}_2^\top B_2^\top \\ Y_{33} &= A_2 \tilde{Q}_2 A_2^\top + A_2 \tilde{S}_2^\top B_2^\top + B_2 \tilde{S}_2 A_2^\top + B_2 \tilde{R}_2 B_2^\top + \tilde{P}_3 \end{aligned}\]The resulting Schur complement matrix $Y$ has a banded structure:
\[Y = \begin{bmatrix} Y_{11} & Y_{12} & 0 \\ Y_{12}^\top & Y_{22} & Y_{23} \\ 0 & Y_{23}^\top & Y_{33} \end{bmatrix}\]This banded structure arises from the sequential nature of the dynamics constraints and is crucial for efficient factorization. The Cholesky factorization of $Y$ is expressed as:
\[Y = L L^\top,\]where $L$ is a lower triangular matrix with the same banding pattern with the following submatrices:
\[\begin{aligned} L_{11} L_{11}^\top &= Y_{11}, \\ L_{ii}L_{i+1, i}^\top &= Y_{i, i+1},\quad i = 1, \dots, N - 1,\\ L_{ii}L_{ii}^\top &= Y_{ii} - L_{i, i-1} L_{i, i-1}^\top, \quad i = 2, \dots, N. \end{aligned}\]