\[ \min_{u} \quad J = \sum_{k=0}^{N-1} \left( x_k^T Q x_k + u_k^T R u_k \right) + x_N^T P x_N \] \[ \text{Subject to: } \begin{aligned} & x_{k+1} = A x_k + B u_k, \\ & u_{min} \leq u_k \leq u_{max}, \\ & x_{min} \leq x_k \leq x_{max} \end{aligned} \] \[ \text{Soft Constraints: } \quad \epsilon \geq 0, \quad J = J + \rho \|\epsilon\|^2 \]