$\vu_0(\cdot)$ - Forward integrate system dynamics: $\{\ \bar{\vx}(z),\bar{\vu}(z)\}_{z=0}^{I},$ - Compute the LQ approximation of the problem along the nominal trajectory, and . - Solve the final value differential equations, (\[eq:riccati\_Sm\]-\[eq:riccati\_s\]). - Line search for the optimal $\alpha$ with policy - Update control law: $\vu^*(z,\vx) = \bar{\vu}(z) + \alpha^* \vl(z) + \vL(z) \delta\vx $

**Given** - Mode switch sequence and initial switching times, $\vt_0$ - The optimal control problem in equations and **Initialization** - Empty the solution bag - Initialize SLQ policy, $\vu_0(\cdot)$ - Initialize the switching times, $\vt_0$ - Compute the equivalent cost function in Equation - Compute the equivalent system dynamics in Equation - Initialized the SLQ policy with the controller in the solution bag that has the most similar switching vector - Initialized the SLQ policy, $\vu_0(z,\vx)$. - Run the SLQ algorithm - Get the optimal control $\vu^*(z,\vx;\vt^*)$ - Memorize the pair $\left(\vt^*,\vu^*(z,\vx;\vt^*)\right)$ in the solution bag - Calculate $\partial_{t_j}\vS(0)$, $\partial_{t_j}\vs(0)$, and $\partial_{t_j}s(0)$ using (\[eq:riccati\_nablaSm\]-\[eq:riccati\_nablas\]). - Calculate the cost function gradient $\nabla_{\vt}J = [\partial_{t_j}s(0)]_j$ - Use a gradient-descent method to update $\vt^*$\
**Return** the optimal $\vt^*$, and $\vu^*(z,\vx;\vt^*)$.

Equations (\[eq:riccati\_nablaSm\]-\[eq:riccati\_nablas\]) can be derived by directly differentiating the Riccati equations in (\[eq:riccati\_Sm\]-\[eq:riccati\_s\]), where based on the continuity condition, the 