updating to-dos

docs/solver/background.md

@@ -35,13 +35,30 @@ The indicator function says simply that there is infinite additional cost when $
 
 We modify the generic optimization problem to include the indicator function by adding it to the cost. We introduce a new state variable $z$, called the slack variable, to describe the constrained version of the original state variable $x$, which we will now call the primal variable.
 
-$$
+Our approach leverages the ADMM framework to distinctly separate the dynamics constraints from other convex constraints such as torque limits and obstacle avoidance. This separation is crucial as it allows us to:
+1. Handle dynamics through efficient LQR techniques in the primal update
+2. Manage other convex constraints through simple projection methods in the slack update
+
+Since both the state constraints ($\mathcal{X}$) and input constraints ($\mathcal{U}$) are convex, this decomposition works by projecting the primal variables ($x, u$) onto these constraint sets through the slack updates. This projection ensures constraint satisfaction while leveraging the separability of the constraint structure, significantly reducing computational complexity compared to solving the fully constrained problem directly.
+
+
+
+
+<!-- $$
 \begin{alignat}{2}
 \min_x & \quad f(x) + I_\mathcal{C}(z) \\
 \text{subject to} & \quad x = z.
 \end{alignat}
+$$ -->
+
+$$
+\begin{alignat}{2}
+\min_{x, u} & \quad f(x, u) + I_\mathcal{X}(z_x) + I_\mathcal{U}(z_u) \\
+\text{subject to} & \quad x = z_x, \quad u = z_u.
+\end{alignat}
 $$
 
+
 At minimum cost, the primal variable $x$ must be equal to the slack variable $z$, but during each solve they will not necessarily be equal. This is because the slack variable $z$ manifests in the algorithm as the version of the primal variable $x$ that has been projected onto the feasible set $\mathcal{C}$, and thus whenever the primal variable $x$ violates any constraint, the slack variable at that iteration will be projected back onto $\mathcal{C}$ and thus differ from $x$. To push the primal variable $x$ back to the feasible set $\mathcal{C}$, we introduce a third variable, $\lambda$, called the dual variable. This method is referred to as the [augmented Lagrangian](https://en.wikipedia.org/wiki/Augmented_Lagrangian_method){:target="_blank"} (originally named the method of multipliers), and introduces a scalar penalty parameter $\rho$ alongside the dual variable $\lambda$ (also known as a Lagrange multiplier). The penalty parameter $\rho$ is the augmentation to what would otherwise just be the Lagrangian of our constrained optimization problem above. $\lambda$ and $\rho$ work together to force $x$ closer to $z$ by increasing the cost of the augmented Lagrangian the more $x$ and $z$ differ.
 
 $$
@@ -62,7 +79,122 @@ where $x^+$, $z^+$, and $\lambda^+$ refer to the primal, slack, and dual variabl
 
 Now all we have to do is solve a few unconstrained optimization problems!
 
-## TODO: primal and slack update and discrete algebraic riccati equation
+<!-- ## TODO: primal and slack update and discrete algebraic riccati equation -->
+---
+
+## Primal and slack update
+
+The primal update in TinyMPC takes advantage of the special structure of Model Predictive Control (MPC) problems. The optimization problem can be written as:
+
+$$
+\min_{x_{1:N}, u_{1:N-1}} J = \frac{1}{2}x_N^{\intercal}Q_fx_N + q_f^{\intercal}x_N + \sum_{k=1}^{N-1} \frac{1}{2}x_k^{\intercal}Qx_k + q_k^{\intercal}x_k + \frac{1}{2}u_k^{\intercal}Ru_k
+$$
+
+$$
+\text{subject to: } x_{k+1} = Ax_k + Bu_k \quad \forall k \in [1,N)
+$$
+
+In addition to the dynamics constraints, the optimization problem also includes convex state and input constraints:
+
+$$
+x_k \in \mathcal{X}, u_k \in \mathcal{U} \quad \forall k \in [1,N)
+$$
+
+where $\mathcal{X}$ and $\mathcal{U}$ are convex sets representing the feasible state and input regions, respectively. These convex constraints ensure that the solution remains within feasible boundaries for both the state and the control inputs at every time step.
+
+When we apply ADMM to this problem, the primal update becomes an equality-constrained quadratic program with modified cost matrices:
+
+$$
+\begin{aligned}
+\tilde{Q}_f &= Q_f + \rho I, \quad \tilde{q}_f = q_f + \lambda_N - \rho z_N \\
+\tilde{Q} &= Q + \rho I, \quad \tilde{q}_k = q_k + \lambda_k - \rho z_k \\
+\tilde{R} &= R + \rho I, \quad \tilde{r}_k = r_k + \mu_k - \rho w_k
+\end{aligned}
+$$
+
+This modified LQR problem has a closed-form solution through the discrete Riccati equation. The feedback law takes the form:
+
+$$
+u_k^* = -K_kx_k - d_k
+$$
+
+where $K_k$ is the feedback gain and $d_k$ is the feedforward term. These are computed through the backward Riccati recursion:
+
+$$
+\begin{aligned}
+K_k &= (R + B^{\intercal}P_{k+1}B)^{-1}(B^{\intercal}P_{k+1}A) \\
+d_k &= (R + B^{\intercal}P_{k+1}B)^{-1}(B^{\intercal}p_{k+1} + r_k) \\
+P_k &= Q + K_k^{\intercal}RK_k + (A - BK_k)^{\intercal}P_{k+1}(A - BK_k) \\
+p_k &= q_k + (A - BK_k)^{\intercal}(p_{k+1} - P_{k+1}Bd_k) + K_k^{\intercal}(Rd_k - r_k)
+\end{aligned}
+$$
+
+The slack update is simpler, requiring only projection onto the constraint sets:
+
+$$
+\begin{aligned}
+z_k^+ &= \text{proj}_{\mathcal{X}}(x_k^+ + y_k) \\
+w_k^+ &= \text{proj}_{\mathcal{U}}(u_k^+ + g_k)
+\end{aligned}
+$$
+
+where $\mathcal{X}$ and $\mathcal{U}$ are the feasible sets for states and inputs respectively, and $y_k, g_k$ are scaled dual variables.
+
+A key optimization in TinyMPC is the pre-computation of certain matrices that remain constant throughout the iterations. Given a sufficiently long horizon, the Riccati recursion converges to the infinite-horizon solution, allowing us to cache:
+
+$$
+\begin{aligned}
+C_1 &= (R + B^{\intercal}P_{\text{inf}}B)^{-1} \\
+C_2 &= (A - BK_{\text{inf}})^{\intercal}
+\end{aligned}
+$$
+
+This significantly reduces the online computational burden while maintaining the algorithm's effectiveness.
+
+---
+
+## Discrete Algebraic Riccati Equation (DARE)
+
+For long time horizons, the Riccati recursion converges to a steady-state solution given by the discrete algebraic Riccati equation:
+
+$$
+P_{\text{inf}} = Q + A^{\intercal}P_{\text{inf}}A - A^{\intercal}P_{\text{inf}}B(R + B^{\intercal}P_{\text{inf}}B)^{-1}B^{\intercal}P_{\text{inf}}A
+$$
+
+This steady-state solution $P_{\text{inf}}$ yields a constant feedback gain:
+
+$$
+K_{\text{inf}} = (R + B^{\intercal}P_{\text{inf}}B)^{-1}B^{\intercal}P_{\text{inf}}A
+$$
+
+TinyMPC leverages this property by pre-computing these steady-state matrices offline, significantly reducing the online computational burden. The only online updates needed are for the time-varying linear terms in the cost function.
+
+---
+
+## Dual Updates and Convergence
+
+The dual update step in ADMM pushes the solution toward constraint satisfaction:
+
+$$
+\begin{aligned}
+y_k^+ &= y_k + x_k^+ - z_k^+ \\
+g_k^+ &= g_k + u_k^+ - w_k^+
+\end{aligned}
+$$
+
+where $y_k$ and $g_k$ are the scaled dual variables ($y_k = \lambda_k/\rho$ and $g_k = \mu_k/\rho$).
+
+The algorithm terminates when both primal and dual residuals are sufficiently small:
+
+$$
+\begin{aligned}
+\text{primal residual: } & \|x_k^+ - z_k^+\|_2 \leq \epsilon_{\text{pri}} \\
+\text{dual residual: } & \rho\|z_k^+ - z_k\|_2 \leq \epsilon_{\text{dual}}
+\end{aligned}
+$$
+
+where $\epsilon_{\text{pri}}$ and $\epsilon_{\text{dual}}$ are user-defined tolerance parameters.
+
 
 <!--
 this is an example of `code` in markdown