A sparse MPC solver for walking motion generation (old version).: Derivations for a linearly constrained problem

Objective function:

$f(\mbm{x}) = 0.5 \mbm{x}^T \mbm{H} \mbm{x} + \mbm{g}^T \mbm{x}$

Constraints:

$\\ \mbm{E}\mbm{x} = \mbm{e}\\ \mbm{D}\mbm{x} - \mbm{d} \le \mbm{0}$

Define the new objective function as the sum of the old one and logarithmic barrier function:

$\phi(\mbm{x}) = 0.5 \mbm{x}^T \mbm{H} \mbm{x} + \mbm{g}^T \mbm{x} -\frac{1}{t} \sum^m_{i=1} \ln {(-\mbm{D}_i \mbm{x} + \mbm{d}_i)}$

Approximation of $\phi$ near some feasible point $\mbm{x}$ :

$\phi(\mbm{x} + \mbm{\Delta x}) = \phi(\mbm{x}) + \nabla\phi({\mbm{x}})\mbm{\Delta x} + 0.5 \mbm{\Delta x} \nabla^2 \phi(\mbm{x}) \mbm{\Delta x}$

where

$\\ \nabla\phi(\mbm{x}) = \mbm{H}\mbm{x} + \mbm{g} + \frac{1}{t} \sum^m_{i=1} \left( \frac{1}{-\mbm{D}_i\mbm{x} + \mbm{d}_i} \mbm{D}_i^T \right)\\ \nabla^2\phi(\mbm{x}) = \mbm{H} + \frac{1}{t} \sum^m_{i=1} \left( \frac{1}{(-\mbm{D}_i\mbm{x} + \mbm{d}_i)^2} \mbm{D}_i^T \mbm{D}_i \right)$

KKT system:

$\left( \begin{array}{cc} \nabla^2\phi(\mbm{x}) & \mbm{E}^T\\ \mbm{E} & \mbm{0}\\ \end{array} \right) \left( \begin{array}{c} \mbm{\Delta x}\\ \mbm{\omega}\\ \end{array} \right) = \left( \begin{array}{c} -\nabla\phi(\mbm{x})\\ \mbm{0}\\ \end{array} \right)$

Solution of the KKT system:

$\\ \mbm{\Delta x} = (\nabla^2\phi(\mbm{x}))^{-1} (-\nabla\phi(\mbm{x}) - \mbm{E}^T \mbm{\omega})\\ \mbm{E} (\nabla^2\phi(\mbm{x}))^{-1} (-\nabla\phi(\mbm{x}) - \mbm{E}^T \mbm{\omega}) = 0\\ \mbm{E} (\nabla^2\phi(\mbm{x}))^{-1} \mbm{E}^T \mbm{\omega} = - \mbm{E} (\nabla^2\phi(\mbm{x}))^{-1} \nabla\phi(\mbm{x})$

We can find $\mbm{\omega}$ using Cholesky decomposition and then determine the descent direction. Since approximation of $\phi$ is used, we have to resolve the system until $\mbm{\Delta x}$ or feasible step size is small enough.