A sparse MPC solver for walking motion generation (old version).: Schur complement

In order to solve KKT system we have to form Schur complement:

$\\ \mbm{S} = \frac{1}{2}\mbm{E}\mbm{H}^{-1}\mbm{E}^T = \frac{1}{2}\left[\begin{array}{cc}\bar{\mbm{E}}_c \tilde{\mbm{E}}_u\end{array}\right] \left[\begin{array}{cc}\tilde{\mbm{H}}_c & \mbm{0} \\ \mbm{0} & \mbm{H}_u\end{array}\right] \left[\begin{array}{c}\bar{\mbm{E}}_c^T \\ \tilde{\mbm{E}}_u^T \end{array}\right] = \frac{1}{2}\bar{\mbm{E}}_c\tilde{\mbm{H}}_c^{-1}\bar{\mbm{E}}_c^T + \frac{1}{2}\tilde{\mbm{E}}_u\mbm{H}_u^{-1}\tilde{\mbm{E}}_u^T.$

For $N = 4$ we have.

$\\ \tilde{\mbm{H}}_c^{-1}\bar{\mbm{E}}_c^T = \left[ \begin{array}{ccccc} \tilde{\mbm{Q}}^{-1} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{Q}}^{-1} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{Q}}^{-1} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{Q}}^{-1} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{Q}}^{-1} \end{array} \right] \left[ \begin{array}{ccccc} -\bar{\mbm{R}}_1^T & \bar{\mbm{R}}_1^T\mbm{A}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & -\bar{\mbm{R}}_2^T & \bar{\mbm{R}}_2^T\mbm{A}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & -\bar{\mbm{R}}_3^T & \bar{\mbm{R}}_3^T\mbm{A}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & -\bar{\mbm{R}}_4^T & \bar{\mbm{R}}_4^T\mbm{A}^T \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & -\bar{\mbm{R}}_5^T \\ \end{array} \right] \\ = \left[ \begin{array}{ccccc} -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T\mbm{A}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T\mbm{A}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T\mbm{A}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T\mbm{A}^T \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_5^T \end{array} \right]$

$\bar{\mbm{E}}_c\tilde{\mbm{H}}_c^{-1}\bar{\mbm{E}}_c^T = \\ = \left[ \begin{array}{ccccc} -\bar{\mbm{R}}_1 & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{A}\bar{\mbm{R}}_1 & -\bar{\mbm{R}}_2 & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{A}\bar{\mbm{R}}_2 & -\bar{\mbm{R}}_3 & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{A}\bar{\mbm{R}}_3 & -\bar{\mbm{R}}_4 & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{A}\bar{\mbm{R}}_4 & -\bar{\mbm{R}}_5 \\ \end{array} \right] \left[ \begin{array}{ccccc} -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T\mbm{A}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T\mbm{A}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T\mbm{A}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T & \tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T\mbm{A}^T \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & -\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_5^T \end{array} \right]\\ = \left[ \begin{array}{ccccc} \bar{\mbm{R}}_1\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T & -\bar{\mbm{R}}_1\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T\mbm{A}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ -\mbm{A}\bar{\mbm{R}}_1\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T & \mbm{A}\bar{\mbm{R}}_1\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_1^T\mbm{A}^T + \bar{\mbm{R}}_2\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T & -\bar{\mbm{R}}_2\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T\mbm{A}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & -\mbm{A}\bar{\mbm{R}}_2\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T & \mbm{A}\bar{\mbm{R}}_2\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_2^T\mbm{A}^T + \bar{\mbm{R}}_3\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T & -\bar{\mbm{R}}_3\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T\mbm{A}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & -\mbm{A}\bar{\mbm{R}}_3\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T & \mbm{A}\bar{\mbm{R}}_3\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_3^T\mbm{A}^T + \bar{\mbm{R}}_4\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T & -\bar{\mbm{R}}_4\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T\mbm{A}^T \\ \mbm{0} & \mbm{0} & \mbm{0} & -\mbm{A}\bar{\mbm{R}}_4\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T & \mbm{A}\bar{\mbm{R}}_4\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_4^T\mbm{A}^T + \bar{\mbm{R}}_5\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_5^T \end{array} \right] \\ = \left[ \begin{array}{ccccc} \mbm{M}_{11} & -\mbm{M}_{11}\mbm{A}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ -\mbm{A}\mbm{M}_{11} & \mbm{A}\mbm{M}_{11}\mbm{A}^T + \mbm{M}_{22} & -\mbm{M}_{22}\mbm{A}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & -\mbm{A}\mbm{M}_{22} & \mbm{A}\mbm{M}_{22}\mbm{A}^T + \mbm{M}_{33} & -\mbm{M}_{33}\mbm{A}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & -\mbm{A}\mbm{M}_{33} & \mbm{A}\mbm{M}_{33}\mbm{A}^T + \mbm{M}_{44} & -\mbm{M}_{44}\mbm{A}^T \\ \mbm{0} & \mbm{0} & \mbm{0} & -\mbm{A}\mbm{M}_{44} & \mbm{A}\mbm{M}_{44}\mbm{A}^T + \mbm{M}_{55} \end{array} \right],$

where $\mbm{M}_{ii} = \bar{\mbm{R}}_i\tilde{\mbm{Q}}^{-1}\bar{\mbm{R}}_i^T = \tilde{\mbm{Q}}^{-1}$ (due to the special structure of $\tilde{\mbm{Q}}^{-1}$ and $\bar{\mbm{R}}_i$ ), this is not true if logarithmic barrier is added to the objective see 'Schur complement (IP method)'.

$\\ \tilde{\mbm{E}}_u\mbm{H}_u^{-1}\tilde{\mbm{E}}_u^T = \left[ \begin{array}{ccccc} \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}}\\ \end{array} \right] \left[ \begin{array}{ccccc} \mbm{P}^{-1} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{P}^{-1} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{P}^{-1} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{P}^{-1} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{P}^{-1} \end{array} \right] \left[ \begin{array}{ccccc} \tilde{\mbm{B}}^T & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{B}}^T & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{B}}^T & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}}^T & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{B}}^T\\ \end{array} \right] \\ =\left[ \begin{array}{ccccc} \tilde{\mbm{P}} & \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \tilde{\mbm{P}} & \mbm{0} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \tilde{\mbm{P}} & \mbm{0} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{P}} & \mbm{0} \\ \mbm{0} & \mbm{0} & \mbm{0} & \mbm{0} & \tilde{\mbm{P}}\\ \end{array} \right],$

where $\tilde{\mbm{P}} = \tilde{\mbm{B}}\mbm{P}^{-1}\tilde{\mbm{B}}^T$ .

$\\ 2\mbm{S}_{11} = \mbm{M}_{11} + \tilde{\mbm{P}}, \\ 2\mbm{S}_{kk} = \mbm{A}\mbm{M}_{k-1,k-1}\mbm{A}^T + \mbm{M}_{kk} + \tilde{\mbm{P}}, \\ 2\mbm{S}_{k,k+1} = \mbm{S}_{k+1,k}^T = -\mbm{M}_{kk}\mbm{A}^T.$

$\\ 2\mbm{S}_{11} = \tilde{\mbm{Q}}^{-1} + \tilde{\mbm{P}}, \\ 2\mbm{S}_{kk} = \mbm{A}\tilde{\mbm{Q}}^{-1}\mbm{A}^T + \tilde{\mbm{Q}}^{-1} + \tilde{\mbm{P}}, \\ 2\mbm{S}_{k,k+1} = \mbm{S}_{k+1,k}^T = -\tilde{\mbm{Q}}^{-1}\mbm{A}^T.$

Hence, the matrix $\mbm{S}$ is constant (if $\mbm{A}$ and $\mbm{B}$ do not change).