Local Optimal Control: LQR, DDP, and Riccati

Local Optimal Control: LQR, DDP, and Riccati

Formulates local optimal control via LQR, differential dynamic programming, and Riccati equations for trajectory optimization in nonlinear systems.

Zoom in, Solve, Check, Repeat

A motion planner needs both a destination and a price for getting there. It predicts how today’s action affects later states, then balances the predicted benefit against effort and other costs. For a linear system with a quadratic objective, this calculation is exact. For a nonlinear system it is a local approximation: roll the proposed action through the full dynamics and check whether the promised improvement actually occurs.

Three questions organize this chapter: what is optimal for a specified model and cost, whether an algorithm finds a stationary solution, and whether the resulting feedback stabilizes the physical system. An answer to one does not automatically answer the others.

Bellman and Pontryagin: Two Views of the Same Objective

For a fixed initial state, consider \[ \min_{u_0,\ldots,u_{N-1}}J=\ell_N(x_N)+\sum_{k=0}^{N-1}\ell_k(x_k,u_k), \qquad x_{k+1}=f_k(x_k,u_k). \tag{1}\] Here $x_k\in\mathbb R^n$, $u_k\in\mathbb R^m$, and $f_k$ is a discrete transition, potentially obtained by integrating a continuous model. Its derivatives must match that transition and its time step.

Bellman’s principle gives \[ V_k(x)=\min_u\{\ell_k(x,u)+V_{k+1}(f_k(x,u))\},\qquad V_N=\ell_N. \tag{2}\] The future value makes this different from repeatedly minimizing today’s stage cost. With linear dynamics, quadratic costs, and unconstrained controls, this recursion reduces to a Riccati recursion (Bellman 1957).

For Pontryagin’s conditions, choose the Lagrangian sign convention \[ \mathcal L=\ell_N(x_N)+\sum_{k=0}^{N-1} \{\ell_k(x_k,u_k)+\lambda_{k+1}^{T}[f_k(x_k,u_k)-x_{k+1}]\}. \tag{3}\] With $H_k=\ell_k+\lambda_{k+1}^{T}f_k$, stationarity gives \[ 0=\ell_{k,u}+f_{k,u}^{T}\lambda_{k+1}, \tag{4}\] \[ \lambda_k=\ell_{k,x}+f_{k,x}^{T}\lambda_{k+1},\quad \lambda_N=\ell_{N,x}. \tag{5}\] In continuous time, the normal minimization convention $H=L+\lambda^Tf$ uses $\dot x=H_\lambda$, $\dot\lambda=-H_x$, and a control minimizing $H$. The alternative maximum convention changes the sign of the running-cost term. The name “maximum principle” does not justify mixing conventions. For general discrete nonlinear dynamics the displayed stationarity conditions are necessary first-order conditions; they do not establish a global Hamiltonian minimum (Pontryagin et al. 1962).

Where the value function is differentiable, the costate trajectory is the gradient of the value function along an optimal trajectory: $\lambda_k=\nabla_xV_k(x_k)$. Thus $V=\tfrac12x^TSx$ gives $\lambda=Sx$, whereas $V=x^TSx$ gives $\lambda=2Sx$. A costate is a covector of marginal cost, not the scalar cost itself. Constraints, nondifferentiable values, and abnormal extremals require the corresponding more general conditions.

A Dimensionally Consistent Local Model

For the Taylor models below, assume the transition and stage/terminal costs are twice continuously differentiable near the feasible nominal trajectory. Use independent local coordinates for state perturbations. If a transition includes an impact, a fixed-mode flow Jacobian alone omits the reset and event-time sensitivity. This smooth derivation does not supply those event derivatives; a hybrid transition requires its own applicable treatment (Kong et al. 2024).

Let $(\bar x_k,\bar u_k)$ be dynamically feasible and write $\delta x=x-\bar x$, $\delta u=u-\bar u$. Derivatives below are evaluated on that trajectory. Use column gradients, $\ell_{ux}\in\mathbb R^{m\times n}$, and $\ell_{xu}=\ell_{ux}^T$: \[ \begin{aligned} \ell\simeq{}&\ell_0+\ell_x^T\delta x+\ell_u^T\delta u\\ &+\frac12\begin{bmatrix}\delta x\\\delta u\end{bmatrix}^{T} \begin{bmatrix}\ell_{xx}&\ell_{xu}\\\ell_{ux}&\ell_{uu}\end{bmatrix} \begin{bmatrix}\delta x\\\delta u\end{bmatrix}. \end{aligned} \tag{6}\] Retain the linear terms: they drive improvement when the nominal trajectory is not stationary. Dropping them generally removes the feedforward correction. The first-order dynamics are \[ \begin{aligned} \delta x_{k+1}&\simeq A_k\delta x_k+B_k\delta u_k,\\ A_k&=f_{k,x},\qquad B_k=f_{k,u}. \end{aligned} \tag{7}\] Even an exact Jacobian gives a tangent approximation to a finite perturbation. An infeasible nominal trajectory introduces a defect $f_k(\bar x_k,\bar u_k)-\bar x_{k+1}$ that a defect-aware method must carry.

Write the next local value as \[ \begin{aligned} V_{k+1}\simeq{}&V_0+s^T\delta x_{k+1}\\ &+\tfrac12\delta x_{k+1}^TS\delta x_{k+1}. \end{aligned} \] The action value $Q=\ell+V_{k+1}\circ f_k$ has derivatives \[ \begin{aligned} Q_x&=\ell_x+A^Ts,\\ Q_u&=\ell_u+B^Ts. \end{aligned} \tag{8}\] \[ Q_{xx}=\ell_{xx}+A^TSA+\sum_i s_i f^i_{xx}, \tag{9}\] \[ Q_{uu}=\ell_{uu}+B^TSB+\sum_i s_i f^i_{uu}, \tag{10}\] \[ Q_{ux}=\ell_{ux}+B^TSA+\sum_i s_i f^i_{ux}. \tag{11}\] The superscript $i$ selects a component of $f$. The sums contract the value gradient with dynamics second derivatives. Full DDP retains these terms; iLQR omits them. Both methods retain the complete cost Hessian, including $\ell_{ux}$, and the first-derivative coupling $B^TSA$. Velocity dependence does not itself require full DDP: iLQR still linearizes those dynamics.

The Discrete Riccati Recursion

Suppose $Q_{uu}\succ0$. Invertibility alone gives only a stationary control, possibly a maximum or saddle. Completing the square gives \[ \delta u^*=k+K\delta x, \tag{12}\] \[ k=-Q_{uu}^{-1}Q_u,\qquad K=-Q_{uu}^{-1}Q_{ux}. \tag{13}\] Negative signs are included in $k$ and $K$ throughout this chapter. Implement the inverses with factorization and linear solves. The unregularized update is \[ \begin{aligned} s_k&=Q_x-Q_{xu}Q_{uu}^{-1}Q_u,\\ S_k&=Q_{xx}-Q_{xu}Q_{uu}^{-1}Q_{ux}. \end{aligned} \tag{14}\] Initialize $s_N=\ell_{N,x}$ and $S_N=\ell_{N,xx}$. Both gradient and curvature must be propagated. A large eigenvalue of $Q_{uu}$ penalizes a control change; for fixed gradient it reduces the step in that direction.

For linear dynamics and stage cost \[ \begin{aligned} \ell_k={}&\tfrac12 x^TQ_kx+u^TN_kx\\ &+\tfrac12 u^TR_ku, \end{aligned} \] with $N_k\in\mathbb R^{m\times n}$, the exact recursion is \[ \begin{aligned} H_k&=R_k+B_k^TS_{k+1}B_k,\\ D_k&=N_k+B_k^TS_{k+1}A_k,\\ K_k&=-H_k^{-1}D_k,\\ S_k={}&Q_k+A_k^TS_{k+1}A_k\\ &-D_k^TH_k^{-1}D_k,\\ S_N&=Q_N. \end{aligned} \tag{15}\] A sufficient convexity assumption is that the joint stage-cost block $\left[\begin{smallmatrix}Q_k&N_k^T\\N_k&R_k\end{smallmatrix}\right]$ is positive semidefinite, $R_k\succ0$, and $Q_N\succeq0$. For $N_k=0$ this is the familiar $Q_k\succeq0$, $R_k\succ0$ setting. The feedback minimizes this finite-horizon linear-quadratic problem globally. Applying it to a nonlinear system has a local approximation scope.

An independent check, including the cross-cost terms, is \[ \begin{aligned} A_{{\rm cl},k}&=A_k+B_kK_k,\\ S_k={}&Q_k+K_k^TN_k+N_k^TK_k\\ &+K_k^TR_kK_k+A_{{\rm cl},k}^TS_{k+1}A_{{\rm cl},k}. \end{aligned} \tag{16}\] Let $A=B=Q=R=1$, $N=1/2$, and $S_{k+1}=1$. Then $H=2$, $D=3/2$, $K=-3/4$, and $S_k=7/8$. Both formulas give $7/8$; omitting the two cross-cost terms does not.

Controllability, Finite Cost, and Stabilization

Finite-horizon LQR does not require controllability. Under the positivity assumptions above, $H_k\succ0$, including when $B_k=0$. The optimizer then computes unavoidable state cost. Controllability asks whether arbitrary state transfers can be achieved. A hard terminal equality can be infeasible even though unconstrained finite-horizon LQR has a finite solution.

For a continuous time-varying linearization, endpoint reachability uses \[ W_c(t_0,T)=\int_{t_0}^T\Phi(T,s)B(s)B(s)^T\Phi(T,s)^T\,ds, \] where $\Phi$ is the state-transition matrix of $A(t)$. A frozen-time rank test on $[B,AB,\ldots]$ is not a general substitute. Small Gramian eigenvalues indicate expensive transfers under the specified state units and input metric; raw condition numbers change under arbitrary rescaling.

For infinite-horizon time-invariant LQR with $Q\succeq0$, $R\succ0$, and no cross term, stabilizability of $(A,B)$ and detectability of $(Q^{1/2},A)$ are standard sufficient assumptions for the stabilizing Riccati solution. Controllability is stronger than necessary. A finite-horizon optimum alone does not guarantee asymptotic stability.

Differential Dynamic Programming and iLQR

DDP repeatedly constructs the local action value and updates the nominal trajectory (Jacobson and Mayne 1970). A practical implementation follows this sequence.

Specify initial state, model, integration method, horizon, cost, and constraints. Roll out nominal controls to obtain a feasible state sequence.
Compute cost gradients and Hessians and dynamics derivatives. Full DDP adds the three dynamics-Hessian contractions; iLQR omits those contractions.
Sweep backward from the terminal gradient and Hessian. Ensure positive control curvature, increasing regularization and restarting when necessary.
From the same initial state, roll out $u_k^{\rm trial}=\bar u_k+\alpha k_k+K_k(x_k^{\rm trial}-\bar x_k)$ and $x_{k+1}^{\rm trial}=f_k(x_k^{\rm trial},u_k^{\rm trial})$.
Compare actual and predicted cost changes and accept sufficient decrease. If no trial is acceptable, increase regularization or report failure.
Check stationarity, feasibility, cost change, and step size. A failed line search or tiny regularized step is not a certificate of convergence.

Here $\alpha$ is a trial step fraction, $0<\alpha\le1$. It scales the feedforward correction; the feedback term uses the trial trajectory’s state deviation. Test the resulting rollout against the original objective.

When choosing a policy, regularization may replace $Q_{uu}$ by $Q_{uu}+\mu I$. Choose $\mu$ large enough to obtain positive definiteness. To evaluate that policy in the original quadratic model, use general substitution: \[ \begin{aligned} s_k&=Q_x+K^TQ_u+Q_{xu}k+K^TQ_{uu}k,\\ S_k&=Q_{xx}+Q_{xu}K+K^TQ_{ux}+K^TQ_{uu}K. \end{aligned} \] Mixing regularized gains with an unqualified Schur-complement update silently changes the model. State the regularization convention implemented.

Identity regularization uses a declared control scale. For inputs with different units or characteristic magnitudes, normalize the controls or specify a fixed positive-definite regularization metric $W$. Choose $\mu W$ with units compatible with $Q_{uu}$; the added quadratic penalty is $\tfrac{\mu}{2}\delta u^TW\delta u$, with $\mu\ge0$. Under a constant nonsingular change of scale $\delta u=T_u\delta v$, \[ \begin{aligned} H_{\rm reg}&=Q_{uu}+\mu W,\\ H_{{\rm reg},v}&=T_u^TQ_{uu}T_u+\mu T_u^TWT_u. \end{aligned} \tag{17}\] The metric transforms by congruence. Replacing $T_u^TWT_u$ by an identity matrix generally changes the physical trial policy. The original $\mu I$ rule is valid in its declared scale; it is not invariant to arbitrary input rescaling. Numerical regularization is not a measurement of mechanical joint damping or metabolic expenditure.

Both methods use local Taylor approximations. Solving a quadratic subproblem exactly does not make that approximation globally exact or establish a global optimum. Repeated relinearization updates the approximation where it is used. For dense matrices, both methods propagate an $n\times n$ value Hessian and factor an $m\times m$ control Hessian. Standard dense algebra costs on the order of $N(n^3+n^2m+nm^2+m^3)$, before derivative evaluation. Full DDP adds dynamics second derivatives or their contractions. iLQR does not reduce the whole backward pass to $O(Nm^3)$. Compare measured runtime at matched tolerances; there is no universal hundredfold speed advantage.

What Local Convergence Requires

Eliminating states in an unconstrained shooting problem yields a reduced objective $J(U)$ in stacked controls. Exact Newton solves $\nabla^2J(U)\Delta U=-\nabla J(U)$. With positive-definite Hessian at a stationary point, locally Lipschitz Hessian, sufficiently close initialization, and eventual full unregularized steps, its standard local result is \[ \|U^{(i+1)}-U^*\|\le C\|U^{(i)}-U^*\|^2. \tag{18}\] This measures error from the solution, not successive step size. A strict minimum need not have a positive-definite Hessian; twice continuous differentiability alone does not imply a Lipschitz Hessian.

Full second-order DDP has a closely related local quadratic-convergence theory under suitable smoothness, nonsingular positive control curvature, and step acceptance assumptions (Jacobson and Mayne 1970). Its nonlinear feedback rollout need not equal an open-loop Newton step away from the solution. Holding each costate fixed and independently minimizing its Hamiltonian misses the dependence of later states and costates on that control.

iLQR uses an approximate reduced Hessian. Superlinear convergence is not a general consequence: linear convergence is possible, and faster rates need additional conditions on omitted terms. An exact Jacobian alone establishes neither quadratic nor global convergence. Globalization helps find acceptable steps without guaranteeing the desired local minimum.

A Reproducible Pendulum Example

For a point mass $m$ on a massless rod of length $l$, viscous damping $b\ge0$, torque $\tau$, and angle measured from downward vertical, \[ ml^2\ddot\theta=-mgl\sin\theta-b\dot\theta+\tau. \tag{19}\] Hanging is $\theta=0$ and upright is $\theta=\pi$. With $m=1\,\mathrm{kg}$, $l=1\,\mathrm m$, $g=10\,\mathrm{m/s^2}$, $b=0.1\,\mathrm{N\,m\,s}$, \[ \dot\theta=\omega,\qquad\dot\omega=-10\sin\theta-0.1\omega+\tau. \tag{20}\] The energy $E=\tfrac12ml^2\omega^2+mgl(1-\cos\theta)$ obeys $\dot E=\tau\omega-b\omega^2$, independently checking the damping sign.

A swing-up angle penalty $1+\cos\theta$ is minimized at upright. An illustrative running cost is $L=1+\cos\theta+0.1\omega^2+0.01\tau^2$. Near upright, an unwrapped local angle chart allows the balancing cost $100(\theta-\pi)^2+10\omega^2+0.1\tau^2$. These weighted objectives are not energy or metabolic expenditure. A step $h$ gives a running-cost approximation $hL$; changing $h$ without scaling it changes the trade-off with terminal cost.

An independently reproducible calculation is infinite-horizon LQR balance for $e=\theta-\pi$ with \[ \begin{aligned} A&=\begin{bmatrix}0&1\\10&-0.1\end{bmatrix},\qquad B=\begin{bmatrix}0\\1\end{bmatrix},\\ Q&=\operatorname{diag}(100,10),\qquad R=0.1. \end{aligned} \] For cost $\int_0^\infty(x^TQx+R\tau^2)dt$, feedback is $\tau=-R^{-1}B^TPx$. Write the symmetric $P$ using entries $p_{11},p_{12},p_{22}$. Its stabilizing solution satisfies \[ \begin{aligned} p_{12}&=1+\sqrt{11},\\ p_{22}&=\frac{-0.2+\sqrt{0.04+40(2p_{12}+10)}}{20},\\ p_{11}&=0.1p_{12}-10p_{22}+10p_{12}p_{22}. \end{aligned} \] The position and velocity feedback gains are approximately $43.166$ and $13.551$. The closed-loop polynomial is $s^2+(0.1+13.551)s+(43.166-10)$, with negative real-part roots. This establishes linearized local balance, not swing-up or robustness to arbitrary torque limits.

To report a swing-up experiment, publish initial state, integration scheme and step, horizon, bounds, objective, initial controls, solver version, derivative checks, stopping tolerances, and trajectories. Check energy residuals and step convergence. The previous iteration and gain tables did not specify such a reproducible run; they are not evidence for the corrected model.

Constraints and the Meaning of the Cost

Clamping a trial torque respects box bounds for that trial but does not solve the bound-constrained local problem. Use a box-constrained backward solve or another constrained optimizer. Augmented Lagrangian methods update multipliers as well as penalties; a pure penalty method should not be given that name.

A soft penalty $\rho\max(0,g(x))^2/2$ does not guarantee $g(x)\le0$ at finite $\rho$. A logarithmic barrier $-\mu\log[-g(x)]$, $\mu>0$, tends to positive infinity when approaching the boundary from inside. It is undefined outside, so the solver must reject infeasible trials. Neither treatment automatically enforces constraints between integration nodes.

Weights encode the question. Normalize by meaningful scales and distinguish impact accuracy, peak load, squared torque, work, and metabolic cost. Squared torque and work have different dimensions and favor different actions. Examine sensitivity to weights. Increasing an effort weight is not a universal method for increasing damping: check poles, constraints, and nonlinear response.

Continuous-Time Riccati and a Counterexample

The Hamilton–Jacobi–Bellman equation is \[ -V_t=\min_u\{L+\nabla V^Tf\},\qquad V(T,x)=\ell_T(x). \tag{21}\] For $\dot x=Ax+Bu$, $L=\tfrac12(x^TQx+u^TRu)$, and $V=\tfrac12x^TSx$, \[ \begin{aligned} -\dot S={}&Q+A^TS+SA\\ &-SBR^{-1}B^TS,\\ S(T)&=Q_T,\\ u^*&=-R^{-1}B^TSx. \end{aligned} \tag{22}\] The time-invariant algebraic equation sets $\dot S=0$ and requires the stabilizing solution under the assumptions stated earlier. For $\dot x=x$, $B=0$, $Q=1$, $Q_T=0$, the finite-horizon solution is $S(t)=(e^{2(T-t)}-1)/2$. It is finite despite uncontrollability and instability. The infinite-horizon cost diverges for nonzero initial state. Finite cost, reachability, and stabilization are different properties.

Model Predictive Control and Terminal Guarantees

MPC repeatedly solves a finite-horizon problem from the measured state, applies its first control, and replans. DDP and iLQR are possible solvers, alongside other nonlinear and quadratic-programming methods. Warm-start by shifting old controls one relative horizon index, appending terminal control, and rolling out states from the new measurement. Disturbances and changing active constraints can make this guess poor.

A standard nominal stability argument assumes admissible terminal feedback $\kappa_f$, an invariant terminal set $\mathcal X_f$, and \[ \begin{gathered} V_f(f(x,\kappa_f(x)))-V_f(x)\\ \le-\ell(x,\kappa_f(x)),\qquad x\in\mathcal X_f. \end{gathered} \] With initial feasibility, the terminal constraint, positive-definite stage cost relative to the target, and appropriate regularity, a shifted candidate establishes recursive feasibility and decrease of the optimal value. Exact optimization, or a suboptimal solver preserving that feasibility and decrease, supports the stability conclusion. An arbitrary finite-horizon local solve does not supply those guarantees.

An LQR quadratic is a candidate terminal cost; nonlinear decrease and input admissibility must be checked on the terminal set. Lyapunov decrease toward one equilibrium differs from contraction between trajectories. A differential metric $M(x,t)$ needs uniform positive-definiteness bounds and an inequality involving $\dot M+A_{\rm cl}^TM+MA_{\rm cl}$ throughout the region. A cost Hessian is not automatically such a certificate.

Connecting the Calculation to the Golf Swing

Geometry changes the dynamics Jacobians; accumulated motion sets the state from which the next action operates; joint impedance changes perturbation response; and impact accuracy changes terminal error weights. Riccati propagation connects these effects across time. A small action can matter because it changes a later, highly weighted impact variable. Maximizing instantaneous acceleration is a different objective.

Compare stiff and compliant strategies under the same actuator limits, starting state, impact target, perturbation model, and cost definition. Include the mechanics and energetic cost of changing stiffness. Otherwise an apparent advantage may reflect extra actuator authority or a different objective.

An optimal torque history is a candidate explanation to test. It does not identify muscle co-contraction, prove that a golfer uses an LQR controller, or establish the nervous system’s objective. Compare predicted kinematics, forces, perturbation responses, and uncertainty with measurements. This preserves the useful connection between geometry and control without turning an optimizer into an unsupported physiological claim.

Exercises

Verify the $7/8$ Riccati example with both formulas and show the error from dropping $N$ while retaining the original cost.
Substitute the pendulum matrix $P$ into the algebraic Riccati equation; check residual, positive definiteness, and closed-loop roots.
Check a discrete pendulum Jacobian and Hessian contractions against finite differences at nonzero states.
Compare iLQR and DDP with the same cross-coupled cost, reporting stationarity residual, feasibility, objective, and runtime from several guesses.
Explain why a hard endpoint constraint changes feasibility in the uncontrollable finite-horizon counterexample.
For $\dot x=-x+u$ and cost $\int(x^2+u^2)dt$, verify $P=-1+\sqrt2$ solves $1-2P-P^2=0$. Substitute $V=Px^2$, $u^*=-Px$ into $0=x^2+(u^*)^2+2Px(-x+u^*)$.

Cite this page

References

Bellman, R. 1957. Dynamic Programming. Princeton University Press.

Jacobson, D. H., and D. Q. Mayne. 1970. Differential Dynamic Programming. American Elsevier Publishing Company. https://books.google.com/books?id=9AZRAAAAMAAJ.

Kong, Nathan J., J. Joe Payne, James Zhu, and Aaron M. Johnson. 2024. “Saltation Matrices: The Essential Tool for Linearizing Hybrid Dynamical Systems.” Proceedings of the IEEE 112 (6): 585–608. https://doi.org/10.1109/JPROC.2024.3440211.

Pontryagin, L. S., V. G. Boltyanskii, R. V. Gamkrelidze, and E. F. Mishchenko. 1962. The Mathematical Theory of Optimal Processes. Interscience.

Citation

BibTeX citation: Download .bib

@online{untitled,
  author = {},
  title = {Local {Optimal} {Control:} {LQR,} {DDP,} and {Riccati}},
  url = {https://affinedrift.com/articles/The_Geometry_of_Motion/quarto/ch05_optimal_control.html},
  langid = {en}
}

For attribution, please cite this work as:

“Local Optimal Control: LQR, DDP, and Riccati.” n.d. https://affinedrift.com/articles/The_Geometry_of_Motion/quarto/ch05_optimal_control.html.

--- title: "Local Optimal Control: LQR, DDP, and Riccati" description: "Formulates local optimal control via LQR, differential dynamic programming, and Riccati equations for trajectory optimization in nonlinear systems." --- []{#ch-optimal-control} ::: {.callout-note} ## Zoom in, Solve, Check, Repeat A motion planner needs both a destination and a price for getting there. It predicts how today's action affects later states, then balances the predicted benefit against effort and other costs. For a linear system with a quadratic objective, this calculation is exact. For a nonlinear system it is a local approximation: roll the proposed action through the full dynamics and check whether the promised improvement actually occurs. ::: Three questions organize this chapter: what is optimal for a specified model and cost, whether an algorithm finds a stationary solution, and whether the resulting feedback stabilizes the physical system. An answer to one does not automatically answer the others. ## Bellman and Pontryagin: Two Views of the Same Objective For a fixed initial state, consider $$ \min_{u_0,\ldots,u_{N-1}}J=\ell_N(x_N)+\sum_{k=0}^{N-1}\ell_k(x_k,u_k), \qquad x_{k+1}=f_k(x_k,u_k). $$ {#eq-ch5:traj-opt} Here $x_k\in\mathbb R^n$, $u_k\in\mathbb R^m$, and $f_k$ is a discrete transition, potentially obtained by integrating a continuous model. Its derivatives must match that transition and its time step. Bellman's principle gives $$ V_k(x)=\min_u\{\ell_k(x,u)+V_{k+1}(f_k(x,u))\},\qquad V_N=\ell_N. $$ {#eq-ch5:bellman} The future value makes this different from repeatedly minimizing today's stage cost. With linear dynamics, quadratic costs, and unconstrained controls, this recursion reduces to a Riccati recursion [@bellman1957]. For Pontryagin's conditions, choose the Lagrangian sign convention $$ \mathcal L=\ell_N(x_N)+\sum_{k=0}^{N-1} \{\ell_k(x_k,u_k)+\lambda_{k+1}^{T}[f_k(x_k,u_k)-x_{k+1}]\}. $$ {#eq-ch5:lagrangian} With $H_k=\ell_k+\lambda_{k+1}^{T}f_k$, stationarity gives $$ 0=\ell_{k,u}+f_{k,u}^{T}\lambda_{k+1}, $$ {#eq-ch5:kkt-u} $$ \lambda_k=\ell_{k,x}+f_{k,x}^{T}\lambda_{k+1},\quad \lambda_N=\ell_{N,x}. $$ {#eq-ch5:costate-update} In continuous time, the normal minimization convention $H=L+\lambda^Tf$ uses $\dot x=H_\lambda$, $\dot\lambda=-H_x$, and a control minimizing $H$. The alternative maximum convention changes the sign of the running-cost term. The name "maximum principle" does not justify mixing conventions. For general discrete nonlinear dynamics the displayed stationarity conditions are necessary first-order conditions; they do not establish a global Hamiltonian minimum [@pontryagin1962]. Where the value function is differentiable, the costate trajectory is the gradient of the value function along an optimal trajectory: $\lambda_k=\nabla_xV_k(x_k)$. Thus $V=\tfrac12x^TSx$ gives $\lambda=Sx$, whereas $V=x^TSx$ gives $\lambda=2Sx$. A costate is a covector of marginal cost, not the scalar cost itself. Constraints, nondifferentiable values, and abnormal extremals require the corresponding more general conditions. ## A Dimensionally Consistent Local Model For the Taylor models below, assume the transition and stage/terminal costs are twice continuously differentiable near the feasible nominal trajectory. Use independent local coordinates for state perturbations. If a transition includes an impact, a fixed-mode flow Jacobian alone omits the reset and event-time sensitivity. This smooth derivation does not supply those event derivatives; a hybrid transition requires its own applicable treatment [@Kong2024SaltationFoundations]. Let $(\bar x_k,\bar u_k)$ be dynamically feasible and write $\delta x=x-\bar x$, $\delta u=u-\bar u$. Derivatives below are evaluated on that trajectory. Use column gradients, $\ell_{ux}\in\mathbb R^{m\times n}$, and $\ell_{xu}=\ell_{ux}^T$: $$ \begin{aligned} \ell\simeq{}&\ell_0+\ell_x^T\delta x+\ell_u^T\delta u\\ &+\frac12\begin{bmatrix}\delta x\\\delta u\end{bmatrix}^{T} \begin{bmatrix}\ell_{xx}&\ell_{xu}\\\ell_{ux}&\ell_{uu}\end{bmatrix} \begin{bmatrix}\delta x\\\delta u\end{bmatrix}. \end{aligned} $$ {#eq-ch5:cost-expansion} Retain the linear terms: they drive improvement when the nominal trajectory is not stationary. Dropping them generally removes the feedforward correction. The first-order dynamics are $$ \begin{aligned} \delta x_{k+1}&\simeq A_k\delta x_k+B_k\delta u_k,\\ A_k&=f_{k,x},\qquad B_k=f_{k,u}. \end{aligned} $$ {#eq-ch5:perturbation} Even an exact Jacobian gives a tangent approximation to a finite perturbation. An infeasible nominal trajectory introduces a defect $f_k(\bar x_k,\bar u_k)-\bar x_{k+1}$ that a defect-aware method must carry. Write the next local value as $$ \begin{aligned} V_{k+1}\simeq{}&V_0+s^T\delta x_{k+1}\\ &+\tfrac12\delta x_{k+1}^TS\delta x_{k+1}. \end{aligned} $$ The action value $Q=\ell+V_{k+1}\circ f_k$ has derivatives $$ \begin{aligned} Q_x&=\ell_x+A^Ts,\\ Q_u&=\ell_u+B^Ts. \end{aligned} $$ {#eq-ch5:Qx} $$ Q_{xx}=\ell_{xx}+A^TSA+\sum_i s_i f^i_{xx}, $$ {#eq-ch5:Qxx} $$ Q_{uu}=\ell_{uu}+B^TSB+\sum_i s_i f^i_{uu}, $$ {#eq-ch5:Quu} $$ Q_{ux}=\ell_{ux}+B^TSA+\sum_i s_i f^i_{ux}. $$ {#eq-ch5:Qxu} The superscript $i$ selects a component of $f$. The sums contract the value gradient with dynamics second derivatives. Full DDP retains these terms; iLQR omits them. Both methods retain the complete cost Hessian, including $\ell_{ux}$, and the first-derivative coupling $B^TSA$. Velocity dependence does not itself require full DDP: iLQR still linearizes those dynamics. ## The Discrete Riccati Recursion Suppose $Q_{uu}\succ0$. Invertibility alone gives only a stationary control, possibly a maximum or saddle. Completing the square gives $$ \delta u^*=k+K\delta x, $$ {#eq-ch5:feedback-affine} $$ k=-Q_{uu}^{-1}Q_u,\qquad K=-Q_{uu}^{-1}Q_{ux}. $$ {#eq-ch5:gain-def} Negative signs are included in $k$ and $K$ throughout this chapter. Implement the inverses with factorization and linear solves. The unregularized update is $$ \begin{aligned} s_k&=Q_x-Q_{xu}Q_{uu}^{-1}Q_u,\\ S_k&=Q_{xx}-Q_{xu}Q_{uu}^{-1}Q_{ux}. \end{aligned} $$ {#eq-ch5:S-def} Initialize $s_N=\ell_{N,x}$ and $S_N=\ell_{N,xx}$. Both gradient and curvature must be propagated. A large eigenvalue of $Q_{uu}$ penalizes a control change; for fixed gradient it reduces the step in that direction. For linear dynamics and stage cost $$ \begin{aligned} \ell_k={}&\tfrac12 x^TQ_kx+u^TN_kx\\ &+\tfrac12 u^TR_ku, \end{aligned} $$ with $N_k\in\mathbb R^{m\times n}$, the exact recursion is $$ \begin{aligned} H_k&=R_k+B_k^TS_{k+1}B_k,\\ D_k&=N_k+B_k^TS_{k+1}A_k,\\ K_k&=-H_k^{-1}D_k,\\ S_k={}&Q_k+A_k^TS_{k+1}A_k\\ &-D_k^TH_k^{-1}D_k,\\ S_N&=Q_N. \end{aligned} $$ {#eq-ch5:riccati-full} A sufficient convexity assumption is that the joint stage-cost block $\left[\begin{smallmatrix}Q_k&N_k^T\\N_k&R_k\end{smallmatrix}\right]$ is positive semidefinite, $R_k\succ0$, and $Q_N\succeq0$. For $N_k=0$ this is the familiar $Q_k\succeq0$, $R_k\succ0$ setting. The feedback minimizes this finite-horizon linear-quadratic problem globally. Applying it to a nonlinear system has a local approximation scope. An independent check, including the cross-cost terms, is $$ \begin{aligned} A_{{\rm cl},k}&=A_k+B_kK_k,\\ S_k={}&Q_k+K_k^TN_k+N_k^TK_k\\ &+K_k^TR_kK_k+A_{{\rm cl},k}^TS_{k+1}A_{{\rm cl},k}. \end{aligned} $$ {#eq-ch5:riccati-alt} Let $A=B=Q=R=1$, $N=1/2$, and $S_{k+1}=1$. Then $H=2$, $D=3/2$, $K=-3/4$, and $S_k=7/8$. Both formulas give $7/8$; omitting the two cross-cost terms does not. ## Controllability, Finite Cost, and Stabilization Finite-horizon LQR does not require controllability. Under the positivity assumptions above, $H_k\succ0$, including when $B_k=0$. The optimizer then computes unavoidable state cost. Controllability asks whether arbitrary state transfers can be achieved. A hard terminal equality can be infeasible even though unconstrained finite-horizon LQR has a finite solution. For a continuous time-varying linearization, endpoint reachability uses $$ W_c(t_0,T)=\int_{t_0}^T\Phi(T,s)B(s)B(s)^T\Phi(T,s)^T\,ds, $$ where $\Phi$ is the state-transition matrix of $A(t)$. A frozen-time rank test on $[B,AB,\ldots]$ is not a general substitute. Small Gramian eigenvalues indicate expensive transfers under the specified state units and input metric; raw condition numbers change under arbitrary rescaling. For infinite-horizon time-invariant LQR with $Q\succeq0$, $R\succ0$, and no cross term, stabilizability of $(A,B)$ and detectability of $(Q^{1/2},A)$ are standard sufficient assumptions for the stabilizing Riccati solution. Controllability is stronger than necessary. A finite-horizon optimum alone does not guarantee asymptotic stability. ## Differential Dynamic Programming and iLQR []{#sec-ch5-ilqr-vs-ddp} DDP repeatedly constructs the local action value and updates the nominal trajectory [@jacobson1970dpp]. A practical implementation follows this sequence. 1. Specify initial state, model, integration method, horizon, cost, and constraints. Roll out nominal controls to obtain a feasible state sequence. 1. Compute cost gradients and Hessians and dynamics derivatives. Full DDP adds the three dynamics-Hessian contractions; iLQR omits those contractions. 1. Sweep backward from the terminal gradient and Hessian. Ensure positive control curvature, increasing regularization and restarting when necessary. 1. From the same initial state, roll out $u_k^{\rm trial}=\bar u_k+\alpha k_k+K_k(x_k^{\rm trial}-\bar x_k)$ and $x_{k+1}^{\rm trial}=f_k(x_k^{\rm trial},u_k^{\rm trial})$. 1. Compare actual and predicted cost changes and accept sufficient decrease. If no trial is acceptable, increase regularization or report failure. 1. Check stationarity, feasibility, cost change, and step size. A failed line search or tiny regularized step is not a certificate of convergence. Here $\alpha$ is a trial step fraction, $0<\alpha\le1$. It scales the feedforward correction; the feedback term uses the trial trajectory's state deviation. Test the resulting rollout against the original objective. When choosing a policy, regularization may replace $Q_{uu}$ by $Q_{uu}+\mu I$. Choose $\mu$ large enough to obtain positive definiteness. To evaluate that policy in the original quadratic model, use general substitution: $$ \begin{aligned} s_k&=Q_x+K^TQ_u+Q_{xu}k+K^TQ_{uu}k,\\ S_k&=Q_{xx}+Q_{xu}K+K^TQ_{ux}+K^TQ_{uu}K. \end{aligned} $$ Mixing regularized gains with an unqualified Schur-complement update silently changes the model. State the regularization convention implemented. Identity regularization uses a declared control scale. For inputs with different units or characteristic magnitudes, normalize the controls or specify a fixed positive-definite regularization metric $W$. Choose $\mu W$ with units compatible with $Q_{uu}$; the added quadratic penalty is $\tfrac{\mu}{2}\delta u^TW\delta u$, with $\mu\ge0$. Under a constant nonsingular change of scale $\delta u=T_u\delta v$, $$ \begin{aligned} H_{\rm reg}&=Q_{uu}+\mu W,\\ H_{{\rm reg},v}&=T_u^TQ_{uu}T_u+\mu T_u^TWT_u. \end{aligned} $$ {#eq-ch5:regularization-scale} The metric transforms by congruence. Replacing $T_u^TWT_u$ by an identity matrix generally changes the physical trial policy. The original $\mu I$ rule is valid in its declared scale; it is not invariant to arbitrary input rescaling. Numerical regularization is not a measurement of mechanical joint damping or metabolic expenditure. Both methods use local Taylor approximations. Solving a quadratic subproblem exactly does not make that approximation globally exact or establish a global optimum. Repeated relinearization updates the approximation where it is used. For dense matrices, both methods propagate an $n\times n$ value Hessian and factor an $m\times m$ control Hessian. Standard dense algebra costs on the order of $N(n^3+n^2m+nm^2+m^3)$, before derivative evaluation. Full DDP adds dynamics second derivatives or their contractions. iLQR does not reduce the whole backward pass to $O(Nm^3)$. Compare measured runtime at matched tolerances; there is no universal hundredfold speed advantage. ## What Local Convergence Requires []{#sec-ch5-ddp-as-newton} Eliminating states in an unconstrained shooting problem yields a reduced objective $J(U)$ in stacked controls. Exact Newton solves $\nabla^2J(U)\Delta U=-\nabla J(U)$. With positive-definite Hessian at a stationary point, locally Lipschitz Hessian, sufficiently close initialization, and eventual full unregularized steps, its standard local result is $$ \|U^{(i+1)}-U^*\|\le C\|U^{(i)}-U^*\|^2. $$ {#eq-ch5:quad-convergence} This measures error from the solution, not successive step size. A strict minimum need not have a positive-definite Hessian; twice continuous differentiability alone does not imply a Lipschitz Hessian. Full second-order DDP has a closely related local quadratic-convergence theory under suitable smoothness, nonsingular positive control curvature, and step acceptance assumptions [@jacobson1970dpp]. Its nonlinear feedback rollout need not equal an open-loop Newton step away from the solution. Holding each costate fixed and independently minimizing its Hamiltonian misses the dependence of later states and costates on that control. iLQR uses an approximate reduced Hessian. Superlinear convergence is not a general consequence: linear convergence is possible, and faster rates need additional conditions on omitted terms. An exact Jacobian alone establishes neither quadratic nor global convergence. Globalization helps find acceptable steps without guaranteeing the desired local minimum. ## A Reproducible Pendulum Example For a point mass $m$ on a massless rod of length $l$, viscous damping $b\ge0$, torque $\tau$, and angle measured from downward vertical, $$ ml^2\ddot\theta=-mgl\sin\theta-b\dot\theta+\tau. $$ {#eq-ch5:pendulum-dynamics} Hanging is $\theta=0$ and upright is $\theta=\pi$. With $m=1\,\mathrm{kg}$, $l=1\,\mathrm m$, $g=10\,\mathrm{m/s^2}$, $b=0.1\,\mathrm{N\,m\,s}$, $$ \dot\theta=\omega,\qquad\dot\omega=-10\sin\theta-0.1\omega+\tau. $$ {#eq-ch5:pendulum-first-order} The energy $E=\tfrac12ml^2\omega^2+mgl(1-\cos\theta)$ obeys $\dot E=\tau\omega-b\omega^2$, independently checking the damping sign. A swing-up angle penalty $1+\cos\theta$ is minimized at upright. An illustrative running cost is $L=1+\cos\theta+0.1\omega^2+0.01\tau^2$. Near upright, an unwrapped local angle chart allows the balancing cost $100(\theta-\pi)^2+10\omega^2+0.1\tau^2$. These weighted objectives are not energy or metabolic expenditure. A step $h$ gives a running-cost approximation $hL$; changing $h$ without scaling it changes the trade-off with terminal cost. An independently reproducible calculation is infinite-horizon LQR balance for $e=\theta-\pi$ with $$ \begin{aligned} A&=\begin{bmatrix}0&1\\10&-0.1\end{bmatrix},\qquad B=\begin{bmatrix}0\\1\end{bmatrix},\\ Q&=\operatorname{diag}(100,10),\qquad R=0.1. \end{aligned} $$ For cost $\int_0^\infty(x^TQx+R\tau^2)dt$, feedback is $\tau=-R^{-1}B^TPx$. Write the symmetric $P$ using entries $p_{11},p_{12},p_{22}$. Its stabilizing solution satisfies $$ \begin{aligned} p_{12}&=1+\sqrt{11},\\ p_{22}&=\frac{-0.2+\sqrt{0.04+40(2p_{12}+10)}}{20},\\ p_{11}&=0.1p_{12}-10p_{22}+10p_{12}p_{22}. \end{aligned} $$ The position and velocity feedback gains are approximately $43.166$ and $13.551$. The closed-loop polynomial is $s^2+(0.1+13.551)s+(43.166-10)$, with negative real-part roots. This establishes linearized local balance, not swing-up or robustness to arbitrary torque limits. To report a swing-up experiment, publish initial state, integration scheme and step, horizon, bounds, objective, initial controls, solver version, derivative checks, stopping tolerances, and trajectories. Check energy residuals and step convergence. The previous iteration and gain tables did not specify such a reproducible run; they are not evidence for the corrected model. ## Constraints and the Meaning of the Cost Clamping a trial torque respects box bounds for that trial but does not solve the bound-constrained local problem. Use a box-constrained backward solve or another constrained optimizer. Augmented Lagrangian methods update multipliers as well as penalties; a pure penalty method should not be given that name. A soft penalty $\rho\max(0,g(x))^2/2$ does not guarantee $g(x)\le0$ at finite $\rho$. A logarithmic barrier $-\mu\log[-g(x)]$, $\mu>0$, tends to positive infinity when approaching the boundary from inside. It is undefined outside, so the solver must reject infeasible trials. Neither treatment automatically enforces constraints between integration nodes. Weights encode the question. Normalize by meaningful scales and distinguish impact accuracy, peak load, squared torque, work, and metabolic cost. Squared torque and work have different dimensions and favor different actions. Examine sensitivity to weights. Increasing an effort weight is not a universal method for increasing damping: check poles, constraints, and nonlinear response. ## Continuous-Time Riccati and a Counterexample The Hamilton--Jacobi--Bellman equation is $$ -V_t=\min_u\{L+\nabla V^Tf\},\qquad V(T,x)=\ell_T(x). $$ {#eq-ch5:hjb} For $\dot x=Ax+Bu$, $L=\tfrac12(x^TQx+u^TRu)$, and $V=\tfrac12x^TSx$, $$ \begin{aligned} -\dot S={}&Q+A^TS+SA\\ &-SBR^{-1}B^TS,\\ S(T)&=Q_T,\\ u^*&=-R^{-1}B^TSx. \end{aligned} $$ {#eq-ch5:DRE} The time-invariant algebraic equation sets $\dot S=0$ and requires the stabilizing solution under the assumptions stated earlier. For $\dot x=x$, $B=0$, $Q=1$, $Q_T=0$, the finite-horizon solution is $S(t)=(e^{2(T-t)}-1)/2$. It is finite despite uncontrollability and instability. The infinite-horizon cost diverges for nonzero initial state. Finite cost, reachability, and stabilization are different properties. ## Model Predictive Control and Terminal Guarantees MPC repeatedly solves a finite-horizon problem from the measured state, applies its first control, and replans. DDP and iLQR are possible solvers, alongside other nonlinear and quadratic-programming methods. Warm-start by shifting old controls one relative horizon index, appending terminal control, and rolling out states from the new measurement. Disturbances and changing active constraints can make this guess poor. A standard nominal stability argument assumes admissible terminal feedback $\kappa_f$, an invariant terminal set $\mathcal X_f$, and $$ \begin{gathered} V_f(f(x,\kappa_f(x)))-V_f(x)\\ \le-\ell(x,\kappa_f(x)),\qquad x\in\mathcal X_f. \end{gathered} $$ With initial feasibility, the terminal constraint, positive-definite stage cost relative to the target, and appropriate regularity, a shifted candidate establishes recursive feasibility and decrease of the optimal value. Exact optimization, or a suboptimal solver preserving that feasibility and decrease, supports the stability conclusion. An arbitrary finite-horizon local solve does not supply those guarantees. An LQR quadratic is a candidate terminal cost; nonlinear decrease and input admissibility must be checked on the terminal set. Lyapunov decrease toward one equilibrium differs from contraction between trajectories. A differential metric $M(x,t)$ needs uniform positive-definiteness bounds and an inequality involving $\dot M+A_{\rm cl}^TM+MA_{\rm cl}$ throughout the region. A cost Hessian is not automatically such a certificate. ## Connecting the Calculation to the Golf Swing Geometry changes the dynamics Jacobians; accumulated motion sets the state from which the next action operates; joint impedance changes perturbation response; and impact accuracy changes terminal error weights. Riccati propagation connects these effects across time. A small action can matter because it changes a later, highly weighted impact variable. Maximizing instantaneous acceleration is a different objective. Compare stiff and compliant strategies under the same actuator limits, starting state, impact target, perturbation model, and cost definition. Include the mechanics and energetic cost of changing stiffness. Otherwise an apparent advantage may reflect extra actuator authority or a different objective. An optimal torque history is a candidate explanation to test. It does not identify muscle co-contraction, prove that a golfer uses an LQR controller, or establish the nervous system's objective. Compare predicted kinematics, forces, perturbation responses, and uncertainty with measurements. This preserves the useful connection between geometry and control without turning an optimizer into an unsupported physiological claim. ## Exercises 1. Verify the $7/8$ Riccati example with both formulas and show the error from dropping $N$ while retaining the original cost. 1. Substitute the pendulum matrix $P$ into the algebraic Riccati equation; check residual, positive definiteness, and closed-loop roots. 1. Check a discrete pendulum Jacobian and Hessian contractions against finite differences at nonzero states. 1. Compare iLQR and DDP with the same cross-coupled cost, reporting stationarity residual, feasibility, objective, and runtime from several guesses. 1. Explain why a hard endpoint constraint changes feasibility in the uncontrollable finite-horizon counterexample. 1. For $\dot x=-x+u$ and cost $\int(x^2+u^2)dt$, verify $P=-1+\sqrt2$ solves $1-2P-P^2=0$. Substitute $V=Px^2$, $u^*=-Px$ into $0=x^2+(u^*)^2+2Px(-x+u^*)$.