Tracking, iterative LQR, and differential dynamic programming

A cart can follow the wrong trajectory perfectly. A tracking controller tries to return to a reference; a trajectory optimizer tries to improve the reference. Both may use an LQR backward pass, but they optimize different costs. Confusing those purposes can produce a numerically impeccable controller that actively resists every useful change to the plan.

The running state is \(x_k=(p_k,v_k)^\mathsf T\), with position, velocity, time, and acceleration expressed in the normalized coordinates introduced in chapter 7. A bar denotes a nominal state or input. The actual differences are \(\delta x_k=x_k-\bar x_k\) and \(\delta u_k=u_k-\bar u_k\). The horizon is \(N\) discrete transitions. We first follow a feasible reference and then use a nonlinear cart to construct a better reference.

Based on AA203 lecture 8: generalized LQR, linear and nonlinear tracking, iLQR, numerical safeguards, and DDP. Full affine recursions, a reference-defect counterexample, the nonlinear cart implementation, and the one-step curvature experiment are original teaching extensions. Local optimization and finite-horizon tracking do not imply global convergence or asymptotic stability. The official slides add the cross-term and augmented-state forms of generalized LQR, the explicit tracking objective, the identification of the iterative LQR subproblem with that generalized step, the outer iteration, the loop of sequential convex programming for comparison, the safeguards and initialization remarks together with the thesis they cite, and the Q-function expansion behind DDP; where their notation differs, the text translates it.

Affine LQR explains why a control law needs a feedforward term 8:21

Pure LQR uses a homogeneous quadratic cost and linear dynamics, so its optimum is linear feedback through the origin. Tracking and local approximation introduce linear cost terms, cross terms, and possibly affine dynamics. To see their effect without hiding it in notation, consider one backward step with \(x\in\mathbb R^n\), \(u\in\mathbb R^m\), and successor \(y=Ax+Bu+d\). The vector \(d\in\mathbb R^n\) is a known offset.

Write the stage cost as \(c+q^\mathsf Tx+r^\mathsf Tu+\frac12x^\mathsf TQx+x^\mathsf TMu+\frac12u^\mathsf TRu\), where \(c\) is scalar, \(q\in\mathbb R^n\), \(r\in\mathbb R^m\), \(Q\) and \(R\) are symmetric, and \(M\in\mathbb R^{n\times m}\). Assume the next value approximation is \(V_+(y)=a_++s_+^\mathsf Ty+\frac12y^\mathsf TP_+y\). A plus subscript means the next stage, not a matrix pseudoinverse.

Substitute the affine dynamics into the value and collect powers of \(x\) and \(u\). Define

Here \(S\) is \(m\times m\), \(L\) is \(m\times n\), and \(g\) is an \(m\)-vector. The Bellman objective is \(c_0+q_0^\mathsf Tx+\frac12x^\mathsf TWx+u^\mathsf T(g+Lx)+\frac12u^\mathsf TSu\). If \(S\succ0\), differentiating with respect to \(u\) gives the unique minimizer

The first term is feedforward: it can act even when the state coordinate is zero. The second is feedback. Substituting the minimizer subtracts \(\frac12(g+Lx)^\mathsf TS^{-1}(g+Lx)\), yielding

These formulas are the generalized Riccati step. They show exactly how the constant, linear, and quadratic pieces propagate. A scalar check exposes their purpose: with \(y=x+u+1\), zero state stage cost, input cost \(u^2/2\), and terminal cost \(y^2/2\), the optimizer is \(u^\star=-(x+1)/2\). At \(x=0\), applying zero input is suboptimal because the known drift remains. Feedforward cancels part of that drift while balancing input effort.

The condition \(S\succ0\) is essential. Arbitrary cross terms or Taylor Hessians need not produce a bounded minimization. For a globally convex quadratic problem, positive semidefiniteness of the joint stage-cost matrix \(\begin{bmatrix}Q&M\\M^\mathsf T&R\end{bmatrix}\), a positive input penalty, and a convex terminal cost give a familiar sufficient setting. A local nonlinear approximation may need regularization instead. Adding general quadratic terms preserves algebraic form; it does not automatically preserve well-posedness.

The cross term alone shows why these generalizations leave the structure of chapter 7 intact. Suppose the stage cost has no constant or linear part, \(c=0\), \(q=0\), \(r=0\), the dynamics have no offset, \(d=0\), and the terminal value is purely quadratic. Then \(s_+=0\) and \(a_+=0\) at the last step, so \(g=0\), \(q_0=0\) and \(c_0=0\), and the update returns \(s=0\) and \(a=0\). Backward induction keeps every stage in this form. The optimal control is the pure linear feedback \(u^\star=-S^{-1}(M^\mathsf T+B^\mathsf TP_+A)x\), and the value remains the pure quadratic \(\frac12x^\mathsf TPx\). The cross term changes the gain and the Riccati matrix, not the form of either. Such a term is natural when the penalty on control effort should depend on the state, and it appears automatically when a cost is expanded about a nominal state and input, as in the iterative methods below.

When \(R\succ0\), the cross term can also be removed by a change of input variable. Completing the square in \(u\) gives \(\frac12x^\mathsf TQx+x^\mathsf TMu+\frac12u^\mathsf TRu=\frac12x^\mathsf T(Q-MR^{-1}M^\mathsf T)x+\frac12w^\mathsf TRw\), with \(w=u+R^{-1}M^\mathsf Tx\). Substituting \(u=w-R^{-1}M^\mathsf Tx\) into the dynamics gives \(y=(A-BR^{-1}M^\mathsf T)x+Bw\). In the variables \((x,w)\) the step is a standard LQR step with state weight \(\hat Q=Q-MR^{-1}M^\mathsf T\) and state matrix \(\hat A=A-BR^{-1}M^\mathsf T\), and its gain for \(w\) translates back into a gain for \(u\). A scalar check with \(A=B=Q=R=P_+=1\) and \(M=1/2\) gives \(S=2\) and \(L=3/2\), so \(u^\star=-3x/4\) and \(P=W-L^2/S=2-9/8=7/8\). The transformed step has \(\hat Q=3/4\) and \(\hat A=1/2\), so its optimal input is \(w=-x/4\), hence \(u=w-x/2=-3x/4\), and its value matrix is \(3/4+1/4-1/8=7/8\). The transformation also explains the convexity condition stated above: with \(R\succ0\), the joint stage-cost matrix is positive semidefinite exactly when its Schur complement \(\hat Q\) is. A cross term can therefore make a problem nonconvex even though \(Q\succeq0\); with \(Q=R=1\) and \(M=2\), the complement is \(\hat Q=-3\).

The affine terms can be absorbed in a similar way by appending a constant coordinate to the state. Let \(\tilde x=(x^\mathsf T,1)^\mathsf T\in\mathbb R^{n+1}\). The affine transition becomes the linear one

because the last row reproduces the constant one. The stage cost becomes \(\frac12\tilde x^\mathsf T\tilde Q\tilde x+\tilde x^\mathsf T\tilde Mu+\frac12u^\mathsf TRu\), with \(\tilde Q=\begin{bmatrix}Q&q\\q^\mathsf T&2c\end{bmatrix}\) and \(\tilde M=\begin{bmatrix}M\\r^\mathsf T\end{bmatrix}\), and the next value becomes \(\frac12\tilde y^\mathsf T\tilde P_+\tilde y\), with \(\tilde P_+=\begin{bmatrix}P_+&s_+\\s_+^\mathsf T&2a_+\end{bmatrix}\). The corner entries are doubled because the quadratic form multiplies them by one half. The result is a cross-term LQR step in \(\tilde x\). Because the last row of \(\tilde B\) is zero, its control curvature is \(R+\tilde B^\mathsf T\tilde P_+\tilde B=S\), unchanged, and its coupling matrix is \(\tilde M^\mathsf T+\tilde B^\mathsf T\tilde P_+\tilde A=\begin{bmatrix}L&g\end{bmatrix}\). The augmented gain therefore applies \(-S^{-1}L\) to \(x\) and \(-S^{-1}g\) to the constant coordinate, which are the feedback and feedforward derived above. The updated matrix \(\tilde P\) carries \(P\), \(s\) and \(2a\) in the same positions as \(\tilde P_+\) carries \(P_+\), \(s_+\) and \(2a_+\).

The scalar drift example checks the construction. There \(\tilde A=\begin{bmatrix}1&1\\0&1\end{bmatrix}\), \(\tilde B=(1,0)^\mathsf T\), \(\tilde Q=0\), \(\tilde M=0\), \(R=1\) and \(\tilde P_+=\operatorname{diag}(1,0)\). Then \(S=2\), the augmented coupling is \((1,1)\), and the gain row is \((-1/2,-1/2)\), the control \(u^\star=-(x+1)/2\) found before. The updated matrix is \(\tilde P=\frac12\begin{bmatrix}1&1\\1&1\end{bmatrix}\), so \(P=1/2\), \(s=1/2\) and \(a=1/4\), the values of Exercise 1. The augmentation has two limits. First, \(\tilde Q\) need not be positive semidefinite, because its last row and column hold the unrestricted coefficients \(q\) and \(c\); well-posedness is still decided by \(S\succ0\). Second, the constant coordinate is a mode with eigenvalue one that no input can influence, so the pair \((\tilde A,\tilde B)\) is never stabilizable. Finite-horizon recursions do not need stabilizability, but infinite-horizon results that assume it do not transfer to the augmented problem.

The official course slides present these generalizations with different letters, and four of them collide with symbols used here. The slides write the terminal weight as \(Q_N\) and the cross weight as \(H_k\); this chapter writes \(M\) for the cross weight and reserves \(H\) for a terminal weight. The slides write the optimal policy as \(L_kx_k+\ell_k\), so their \(L_k\) is the signed gain \(-S^{-1}L\), written \(F_k\) in the previous lecture's slides and \(-K_k\) in chapter 7, and their \(\ell_k\) is the feedforward \(-S^{-1}g\). Here, by contrast, \(L\) is the coupling matrix \(M^\mathsf T+B^\mathsf TP_+A\), and \(\ell\) is the stage cost of the nonlinear problem. The slides' quadratic, linear and constant value coefficients are \(P_k\), a bold vector \(p_k\) and a scalar \(p_k\); they correspond to \(P\), \(s\) and \(a\) (here \(p_k\) is the cart position). Their cost coefficients \(q_k\), \(r_k\), \(c_k\) and offset \(d_k\) match the symbols used here. The AA203 course notes treat the same material in Section 3.1, including LQR with bilinear cost and affine dynamics and tracking, and iterative LQR and DDP in Section 3.2.

A feasible reference turns tracking into regulation of the error 19:08

Assume actual and nominal trajectories obey the same linear model: \(x_{k+1}=A_kx_k+B_ku_k\) and \(\bar x_{k+1}=A_k\bar x_k+B_k\bar u_k\). Subtracting the equations gives \(\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k\). No approximation has occurred. A quadratic tracking objective penalizing \(\delta x_k\) and \(\delta u_k\) is therefore exactly an LQR problem in the deviation variables.

Written out with a symmetric terminal weight \(H\), such an objective is

which equals \(\frac12\delta x_N^\mathsf TH\delta x_N+\frac12\sum_{k=0}^{N-1}(\delta x_k^\mathsf TQ\delta x_k+\delta u_k^\mathsf TR\delta u_k)\) in deviation variables. The weights are design choices that trade tracking accuracy against corrective effort; the reference does not determine them. The input term penalizes the departure \(u_k-\bar u_k\) from the reference input, not the input itself. If the objective penalized \(\frac12u_k^\mathsf TRu_k\) instead, expanding \(u_k=\bar u_k+\delta u_k\) would add the linear term \(\bar u_k^\mathsf TR\,\delta u_k\), and the deviation problem would acquire the feedforward term of the affine recursion. For the scalar integrator \(x_{k+1}=x_k+u_k\) following the feasible ramp \(\bar x_k=k\), \(\bar u_k=1\), with \(Q=H=R=1\), consider one remaining step from zero error. The tracking objective gives \(\delta u=0\) and zero final error. Penalizing \(u\) itself minimizes \(\frac12\delta u^2+\frac12(1+\delta u)^2\) and gives \(\delta u=-1/2\), so the controller falls half a unit behind the reference to save effort.

Using chapter 7's unsigned gain convention, \(\delta u_k^\star=-K_k\delta x_k\). The actuator receives the actual control

not the deviation alone. At zero tracking error, the controller applies the reference input. For a cart that is accelerating along its reference, this feedforward term is what maintains the planned acceleration. Sending only \(-K_k\delta x_k\) would command zero acceleration at precisely the point where the cart is following the plan correctly.

The reference must be dynamically feasible for the subtraction to remove the constant term. If it is not, define its defect \(d_k=A_k\bar x_k+B_k\bar u_k-\bar x_{k+1}\). Then the exact deviation dynamics are \(\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k+d_k\). The affine recursion above can account for this term; ignoring it asserts a different model.

An original counterexample uses the scalar integrator \(x_{k+1}=x_k+u_k\), a desired reference step from zero to one, and nominal control zero. The reference defect is \(-1\). With initial error zero, an error-only homogeneous controller chooses \(\delta u=0\), leaving actual state zero and next tracking error minus one. Under the true tracking cost \(\frac12[(u-1)^2+u^2]\), the optimal input is \(1/2\). The nonzero feedforward correction follows from the affine defect and the cost tradeoff.

Finite-horizon LQR minimizes the chosen finite tracking cost. It does not, without further assumptions, force the error to converge to zero as time tends to infinity. The earlier example with zero terminal cost even has a zero final feedback gain. The useful conclusion is an optimal finite response under the model, not an unstated stability theorem.

A nonlinear tracker is locally accurate along a moving reference 32:48

For \(x_{k+1}=f_k(x_k,u_k)\), assume a feasible nominal pair satisfies \(\bar x_{k+1}=f_k(\bar x_k,\bar u_k)\). Expand around that pair. With Jacobians \(A_k=\partial f_k/\partial x\) and \(B_k=\partial f_k/\partial u\), evaluated at the nominal state and input,

The remainder \(\mathcal R_k\) contains second and higher order terms. If the relevant second derivatives are bounded on a neighborhood, its norm is bounded by a constant times the squared norm of the combined deviation. A time-invariant nonlinear plant can yield time-varying \(A_k,B_k\), because the nominal point changes along the trajectory.

Use the original discrete nonlinear cart model

The positive step \(h\) and nonnegative drag coefficient \(\gamma\) are normalized. This is an Euler discretization chosen as the model for the experiment; it is not claimed to be the exact sampled continuous dynamics. Its Jacobians are \(A_k=\begin{bmatrix}1&h\\0&1-3h\gamma\bar v_k^2\end{bmatrix}\), \(B_k=(0,h)^\mathsf T\). The velocity remainder is \(-h\gamma[3\bar v_k(\delta v_k)^2+(\delta v_k)^3]\), and the position remainder is zero.

A local LQR tracker ignores that remainder while computing gains, then applies the control to the nonlinear plant. The remainder explains the locality restriction: a large deviation can invalidate the model used to design the correction. The approach also needs both nominal states and nominal controls. A geometric path alone does not specify a dynamically feasible time parameterization or the inputs required to produce it.

The source places tracking within a layered control design: a planner constructs the nominal motion and a feedback controller corrects local deviations. A changing obstacle or objective may require replanning rather than stronger tracking of an obsolete reference. That design pattern is useful without implying that a particular hierarchy is mandatory for every robot.

Trajectory optimization deliberately moves away from the nominal path 42:06

Now the nominal trajectory is merely feasible, not desired or optimal. We want a new trajectory with lower cost \(J=\phi(x_N)+\sum_{k=0}^{N-1}\ell_k(x_k,u_k)\). The stage cost \(\ell_k\) is supplied by the task, and \(\phi\) is its terminal cost. In tracking, the quadratic weights were design choices defining closeness to a reference. In iterative LQR, the coefficients are derivatives of the actual optimization objective around the current nominal trajectory.

At one stage, suppress \(k\), define all gradients as column vectors, and evaluate derivatives at \((\bar x,\bar u)\). The second-order expansion is

The matrix \(\ell_{ux}\) is \(m\times n\); its transpose is \(\ell_{xu}\) for a twice continuously differentiable cost. The linear terms are crucial. At \(\delta x=\delta u=0\), they can provide a nonzero direction of improvement. Replacing this expansion by a positive quadratic penalty on deviations alone would typically prefer no change and would solve a tracking problem instead.

Together with the linearized dynamics, this expansion is an instance of the generalized step of the first section, written in deviation variables. At stage \(k\) its constant, linear, quadratic and cross coefficients are \(c=\ell_0\), \(q=\ell_x\), \(r=\ell_u\) (the linear coefficient \(r\) of the first section, not the cart's input weight below), \(Q=\ell_{xx}\), \(R=\ell_{uu}\) and \(M=\ell_{xu}\); its transition matrices are the Jacobians \(A_k\) and \(B_k\); and the terminal value starts from \(a_N=\phi(\bar x_N)\), \(s_N=\nabla\phi(\bar x_N)\) and \(P_N=\nabla^2\phi(\bar x_N)\). The offset vanishes, \(d_k=f_k(\bar x_k,\bar u_k)-\bar x_{k+1}=0\), because the nominal trajectory is a rollout of the model. With these identifications the quantities \(g\), \(L\) and \(S\) of the first section become the blocks \(\mathcal Q_u\), \(\mathcal Q_{ux}\) and \(\mathcal Q_{uu}\) of the next section, and the linear value coefficient \(s\) becomes the value gradient written \(v\) there.

In absolute coordinates the same approximation reads \(x_{k+1}\approx A_kx_k+B_ku_k+(\bar x_{k+1}-A_k\bar x_k-B_k\bar u_k)\), which is affine. This is the sense in which the local subproblem is an LQR problem with cross-quadratic cost and affine dynamics. If an initial guess is not a rollout, for example a straight-line interpolation between two states, then \(d_k\neq0\) and the affine recursion accounts for the defect, as in the tracking counterexample above. The official slides pose the problem without a terminal cost and write the stage cost as \(c\), its nominal value as \(c_k\), and its derivatives as \(c_{x,k}\), \(c_{u,k}\), \(c_{xx,k}\), \(c_{uu,k}\) and \(c_{xu,k}\), labeled \(q_k\), \(r_k\), \(Q_k\), \(R_k\) and \(H_k\). Setting \(\phi=0\) recovers their formulation.

For the cart choose \(\ell(x,u)=\frac12x^\mathsf TQx+\frac12ru^2\), with \(Q=\operatorname{diag}(1,0.1)\), scalar \(r=0.05\), and terminal cost \(\phi(x)=\frac12x^\mathsf THx\), \(H=\operatorname{diag}(20,2)\). At a nominal state \((1,0)\), \(\ell_x=(1,0)^\mathsf T\). A negative position deviation reduces the state cost to first order. This is the mathematical pressure that deforms the initial stationary trajectory toward the origin.

The local Bellman step produces both a correction and a feedback gain 57:04

Let \(v_+\in\mathbb R^n\) and \(P_+\in\mathbb R^{n\times n}\) be the gradient and Hessian of the next local value approximation at the nominal successor. They are coefficients of an approximation, not the globally known exact value function. With linearized deviation dynamics \(A\delta x+B\delta u\), define a local state-action cost \(\mathcal Q\), consisting of current stage cost plus approximated next value. The calligraphic symbol distinguishes it from the state penalty matrix \(Q\).

For iLQR the coefficients are

Assuming \(\mathcal Q_{uu}\succ0\), stationarity in the control deviation gives \(\mathcal Q_u+\mathcal Q_{ux}\delta x+\mathcal Q_{uu}\delta u=0\). Define the feedforward correction \(d=-\mathcal Q_{uu}^{-1}\mathcal Q_u\) and the signed feedback gain \(F=-\mathcal Q_{uu}^{-1}\mathcal Q_{ux}\). Then \(\delta u=d+F\delta x\). Unlike a pure tracker, the optimizer can change the nominal control even when \(\delta x=0\).

Substituting this minimizing correction yields the updated local value coefficients

The local constant value decreases by \(\mathcal Q_u^\mathsf T\mathcal Q_{uu}^{-1}\mathcal Q_u/2\). That is a predicted improvement in the local quadratic model, not yet a measured improvement in the original objective. Start the backward pass from \(v_N=\nabla\phi(\bar x_N)\), \(P_N=\nabla^2\phi(\bar x_N)\), then recurse to stage zero, saving \(d_k,F_k\).

A forward pass generates an actual candidate by setting \(x_0^{\rm new}\) to the fixed initial state and applying

The nonlinear rollout is essential. Adding the linearized state correction to \(\bar x_k\) generally gives a state sequence that does not satisfy the original dynamics. The forward feedback term compensates for how the new rollout departs from the nominal trajectory; \(\alpha\) scales the planned feedforward change. Evaluate the original cost of the candidate and reduce \(\alpha\) when the change is unacceptable.

An accepted candidate replaces the nominal trajectory, and the whole construction repeats about it. One iteration of iterative LQR therefore has two passes. The backward pass, from \(k=N\) down to zero, linearizes the dynamics and expands the cost about the current nominal trajectory, solves the local Bellman step for \(d_k\) and \(F_k\), and propagates the local cost-to-go coefficients. The forward pass, from \(k=0\) to \(N\), applies the new policy to the full nonlinear model \(f_k\), not to its linearization. Iterations continue until a termination test of the kind discussed in the next section holds. Because every accepted iterate is a rollout of \(f_k\) from the fixed initial state, every iterate is dynamically feasible, and the method can be stopped early with a feasible, if suboptimal, trajectory. The new states are the rollout states, not the linear prediction \(\bar x_k+\delta x_k^\star\); the two differ by terms of second order in the step.

The loop uses the closed-loop machinery of dynamic programming to produce an open-loop result. Its output is an improved nominal pair, so iterative LQR is a direct method for trajectory optimization, an alternative to the sequential convex programming of chapter 6. Its final trajectory can then be handed to the tracker of the first sections, whose own weights decide how strongly disturbances are corrected about it.

Regularization and line search address different failure modes 58:04

If the local control curvature is indefinite, the stationary point can be a saddle or maximum rather than a minimum. One simple regularization replaces \(\mathcal Q_{uu}\) by \(\mathcal Q_{uu}+\mu I\), with \(\mu>0\) large enough for positive definiteness. It changes the local model by adding a quadratic control-deviation penalty. A line search instead controls how far to move along a proposed policy update by evaluating actual rollouts. Positive curvature does not ensure a full step is accurate enough, so both mechanisms can be needed.

Both mechanisms serve the first requirement of a practical implementation: the new states and controls must stay close enough to the linearization point for the local model to remain informative. The regularized correction \(d_\mu=-(\mathcal Q_{uu}+\mu I)^{-1}\mathcal Q_u\) minimizes the local model plus the extra deviation penalty \(\frac{\mu}{2}\|\delta u\|^2\). As \(\mu\to0\) it approaches the full Newton-like correction; for large \(\mu\) it approaches \(-\mathcal Q_u/\mu\), a short step along the negative local gradient whose length decreases like \(1/\mu\). For symmetric \(\mathcal Q_{uu}\) with smallest eigenvalue \(\lambda_{\min}\), adding \(\mu I\) shifts every eigenvalue by \(\mu\), so \(\mathcal Q_{uu}+\mu I\succ0\) exactly when \(\mu\gt-\lambda_{\min}\). Implementations usually test positive definiteness directly, for example by attempting a Cholesky factorization, and raise \(\mu\) until the test passes. In the scalar curvature counterexample below, where the exact curvature is \(0.1\) and the gradient is \(-1\), the choice \(\mu=1.5\) gives the step \(1/1.6=0.625\): the same point, with the same cost \(0.03567505\), as the full DDP step of that counterexample, \(10\), shortened by the line-search fraction \(\alpha=1/16\). In this one-step problem, with one control, no state deviation and positive curvature, the two mechanisms produce the same family of steps. In general they differ. A line search multiplies every feedforward correction \(d_k\) by the same \(\alpha\) and keeps the gains \(F_k\), so once the feedback acts on the new states the realized control change is not a scaled copy of the full step. Increasing \(\mu\) instead changes the backward pass itself: at each stage whose curvature \(\mathcal Q_{uu,k}\) is positive definite it shortens the correction computed from that stage's blocks and, with several controls, turns it toward \(-\mathcal Q_{u,k}\); it also changes the gains \(F_k\) and the value coefficients passed to earlier stages, so the correction at an earlier stage can even grow. When \(\mathcal Q_{uu}\) is indefinite, increasing \(\mu\) can reverse the direction of a step, which no choice of \(0\lt\alpha\le1\) can do.

The simplified Schur-complement value updates above assume gains minimize the same unregularized quadratic being propagated. If gains are computed from a modified curvature but the value update evaluates a different quadratic, substitute the gains explicitly: \(v=\mathcal Q_x+F^\mathsf T\mathcal Q_u+\mathcal Q_{xu}d+F^\mathsf T\mathcal Q_{uu}d\), and \(P=\mathcal Q_{xx}+F^\mathsf T\mathcal Q_{uu}F+F^\mathsf T\mathcal Q_{ux}+\mathcal Q_{xu}F\). Keeping this convention consistent avoids a subtle implementation error.

The small cart implementation below has convex quadratic costs and uses the iLQR dynamics approximation, so its local control curvature remains positive in the tested setting. It checks this condition and raises an error if it fails; it does not silently claim to be a general regularized solver. It uses decreasing-cost line search and an independently computed first-order stationarity residual. Tiny cost changes alone are a weak stopping rule because they can reflect rounding, excessive damping, or poor scaling.

The source compares iLQR with sequential convex programming. Both repeatedly approximate a nonlinear trajectory problem. Basic iLQR exploits an unconstrained quadratic subproblem through Riccati-style elimination. SCP can use more general convex costs and explicit convex constraints, at the cost of invoking a suitable constrained solver. Penalties in an unconstrained iLQR objective do not turn state or input bounds into hard guarantees.

The comparison is sharper when the two loops are set side by side. Write a continuous-time problem as minimizing \(\int_0^{t_f}\ell_{\rm c}(x(t),u(t),t)\,dt\) subject to \(\dot x=f_{\rm c}(x,u,t)\), \(x(0)=x_0\), the terminal condition \(x(t_f)=x_f\), and \(u(t)\in\mathcal U\subseteq\mathbb R^m\) for \(t\in[0,t_f]\). Here \(\ell_{\rm c}\) is a running cost, \(f_{\rm c}\) the continuous-time dynamics, \(t_f\) the final time, \(x_f\) a required final state and \(\mathcal U\) the admissible input set. Number the iterations of sequential convex programming with an index \(j\). Iteration \(j\) linearizes \(f_{\rm c}\) about the current solution \((x^{(j)}(\cdot),u^{(j)}(\cdot))\), giving a model \(f^{(j+1)}\), and poses the continuous problem with \(f^{(j+1)}\) in place of \(f_{\rm c}\). It then discretizes that problem on times \(t_i\) with steps \(h_i\), which gives the dynamics \(x_{i+1}=x_i+h_if^{(j+1)}(x_i,u_i,t_i)\), the objective \(\sum_{i=0}^{N-1}h_i\ell_{\rm c}(x_i,u_i,t_i)\), and the constraints \(u_i\in\mathcal U\) and \(x_N=x_f\). Finally it solves this program for the next iterate \((x^{(j+1)},u^{(j+1)})\). The program is convex when \(\ell_{\rm c}\) is convex in \((x,u)\) and \(\mathcal U\) is convex, because the discretized model dynamics are affine. The official slides index these iterations with \(k\), which everywhere else in this chapter denotes the stage.

Aligned this way, the loops differ in four places. The subproblem is a general convex program in SCP; in iLQR it is an unconstrained quadratic problem, solved by the Riccati-style backward pass at a cost that grows linearly with \(N\). SCP keeps the input set \(\mathcal U\) and the terminal equality \(x_N=x_f\) as constraints, whereas the iLQR step has no place for either and can represent them only through penalties. SCP's next iterate solves the convexified problem, so it satisfies the linearized dynamics; it satisfies the discretized nonlinear dynamics at a fixed point of the iteration, where the linearization point and the solution coincide. iLQR's next iterate is a rollout of the nonlinear model, so it is dynamically feasible at every iteration but may violate constraints that the local problem never represented. Finally, both methods need a mechanism that keeps the new solution near the linearization: SCP uses the trust regions of chapter 6, which constrain the deviation explicitly, and iLQR uses the regularization and line search described above.

Tassa, Erez, and Todorov's 2012 primary paper treats dynamics derivatives, regularization, and line search as central implementation choices in an online trajectory optimizer. Its empirical demonstrations concern its particular systems and implementation; the normalized cart here provides a separate, reproducible calculation rather than a reproduction of those experiments.

Section 2.2.3 of Tassa's doctoral thesis, Theory and Implementation of Biomimetic Motor Controllers (Hebrew University of Jerusalem, 2011), collects several refinements of these safeguards. The standard Levenberg–Marquardt regularization, which the thesis attributes to Jacobson and Mayne's 1970 monograph Differential Dynamic Programming, adds \(\mu I\) to \(\mathcal Q_{uu}\) and so penalizes control deviations. The same control change moves the state by different amounts at different stages, depending on \(B_k\). The thesis therefore adds \(\mu I\) to the next value Hessian inside the control blocks, using \(\ell_{uu}+B^\mathsf T(P_++\mu I)B\) and \(\ell_{ux}+B^\mathsf T(P_++\mu I)A\), plus the DDP terms when they are present. This penalizes deviations of the successor state from the previous trajectory. Under the control penalty the gain \(F=-(\mathcal Q_{uu}+\mu I)^{-1}\mathcal Q_{ux}\) tends to zero as \(\mu\) grows; under the state penalty the gains do not vanish and instead pull the new trajectory toward the old one. The thesis also uses the gain-consistent value update given above, which it attributes to Todorov and Li's 2005 American Control Conference paper on a generalized iterative LQG method. It raises \(\mu\) and restarts the backward pass whenever the regularized control curvature is not positive definite, and lowers \(\mu\) after a successful backward pass.

The thesis's line search compares the actual cost change with the change that the local model predicts. For a step fraction \(\alpha\), the predicted change is

where \(\mathcal Q_{u,k}\) and \(\mathcal Q_{uu,k}\) are the stage-\(k\) blocks of the backward pass. Each stage term is the change of that stage's local model when the correction \(\alpha d_k\) is applied at zero state deviation. With the unregularized correction \(d_k=-\mathcal Q_{uu,k}^{-1}\mathcal Q_{u,k}\), each stage contributes \(-(\alpha-\alpha^2/2)\mathcal Q_{u,k}^\mathsf T\mathcal Q_{uu,k}^{-1}\mathcal Q_{u,k}\). With \(\mathcal Q_{uu,k}\succ0\) this is never positive for \(0\lt\alpha\le1\), and at \(\alpha=1\) it equals \(-\frac12\mathcal Q_{u,k}^\mathsf T\mathcal Q_{uu,k}^{-1}\mathcal Q_{u,k}\), the negative of the decrease predicted by the local Bellman step. Let \(\rho\) be the ratio of the actual change \(J(\text{candidate})-J(\text{nominal})\) to \(\Delta(\alpha)\). The candidate is accepted only if \(\beta_1\lt\rho\lt\beta_2\) for chosen constants \(0\lt\beta_1\lt\beta_2\); otherwise \(\alpha\) is reduced. The lower bound demands a real decrease of at least a fraction \(\beta_1\) of the prediction. The upper bound rejects a decrease much larger than predicted, which the thesis interprets as a jump from one poor region of the state space to another that can introduce undesired features and end in a local minimum. A ratio near one indicates that the local model described the step well.

None of these safeguards chooses among local minima, because each acts on the local model. iLQR descends from its initial nominal trajectory toward a nearby stationary point, and a different initialization can reach a different stationary point with a different cost while satisfying the same first-order stopping test. Exercise 7 shows this exactly for the scalar problem of the curvature counterexample, where two starting inputs end at costs of about \(0.0259\) and \(0.3605\). A good initialization is therefore often decisive. Warm starts, for example from the solution of a neighboring problem or from a predictor trained on many solved problems, change where the local search begins; they do not change its local character.

DDP retains curvature of the dynamics inside the Bellman expansion 1:03:35

Iterative LQR linearizes dynamics, approximates costs quadratically, and solves that local problem. Differential dynamic programming instead retains the second-order Taylor expansion of the composed state-action value \(\ell(x,u)+V_+(f(x,u))\). The distinction follows directly from the second derivative chain rule.

The object being expanded is the state-action value, also called the Q-function. With the exact optimal cost-to-go \(V_{k+1}\) of chapter 7, it is the cost of applying a perturbed input now and acting optimally afterward:

The Bellman equation reads \(V_k(\bar x_k+\delta x)=\min_{\delta u}\mathcal Q_k^\star(\delta x,\delta u)\), because \(\bar u_k\) is fixed and minimizing over \(u=\bar u_k+\delta u\) is the same as minimizing over \(\delta u\). The second-order Taylor model of \(\mathcal Q_k^\star\) about \((0,0)\) is

Here the blocks are the gradient and Hessian blocks of \(\mathcal Q_k^\star\) at \((0,0)\); their values are given below. The model's first-order terms are \(\mathcal Q_x^\mathsf T\delta x+\mathcal Q_u^\mathsf T\delta u\), and its second-order terms are \(\frac12\delta x^\mathsf T\mathcal Q_{xx}\delta x+\delta x^\mathsf T\mathcal Q_{xu}\delta u+\frac12\delta u^\mathsf T\mathcal Q_{uu}\delta u\). Minimizing the model over \(\delta u\) gives the stationarity condition, the correction \(d\) and the gain \(F\) of the local Bellman step, and DDP then runs the same backward and forward passes as iLQR. One qualification makes the expansion computable. For a nonlinear problem the exact \(V_{k+1}\) is unknown, so DDP differentiates \(\ell_k+V_+\circ f_k\), where \(V_+\) is the local quadratic model of the next value produced by the previous backward step, with gradient \(v_+\) and Hessian \(P_+\) at the nominal successor. The constant term is then \(\ell_k(\bar x_k,\bar u_k)+V_+(\bar x_{k+1})\). The same state-action value returns in the learning-based chapters, where it is estimated from data instead of computed from a model.

For a scalar composition \(V(f(z))\), with scalar \(z\) and vector-valued \(f\), differentiating once gives \(V_f^\mathsf Tf'(z)\). Differentiating again produces \(f'(z)^\mathsf TV_{ff}f'(z)+\sum_i(V_f)_i f_i''(z)\). The first term differentiates the outer function twice; the second differentiates the inner dynamics twice. iLQR retains the first type and omits the second.

For vector state and control, let \(f^i\) be the \(i\)-th scalar component of the transition. Full DDP replaces the Hessian blocks by

The component Hessians have the same dimensions as the blocks they enter. The gradient expressions stay unchanged. Calling the added operation a tensor contraction means precisely this weighted sum over successor coordinates; no mysterious new object is required. The control correction and value-update algebra then have the same form as before, subject to the curvature and local-accuracy qualifications.

The official slides use a third set of letters for these objects. Their Q-function \(Q_k\) and its blocks \(Q_{x,k}\), \(Q_{u,k}\), \(Q_{xx,k}\), \(Q_{uu,k}\) and \(Q_{ux,k}\) are \(\mathcal Q\) and its blocks here; their cost derivatives \(c_{x,k}\), \(c_{xx,k}\) and so on are \(\ell_x\), \(\ell_{xx}\) and so on; and their Jacobians \(f_{x,k}\) and \(f_{u,k}\) are \(A_k\) and \(B_k\). Their bold vector \(v_{k+1}\) and matrix \(V_{k+1}\) are the next value gradient \(v_+\) and Hessian \(P_+\) (not the cost-to-go function \(V_{k+1}\) used above), and their feedforward and gain \(l_k\) and \(L_k\) are \(d_k\) and \(F_k\). The product \(v_{k+1}\cdot f_{xx,k}\) denotes the contraction \(\sum_i(v_+)_if^i_{xx}\) above. In the first line of their list, \(Q_k=c_k+v_{k+1}\), the symbol \(v_{k+1}\) is the scalar next value at the nominal successor, not the gradient used in the following lines.

For the nonlinear cart, only the velocity component has nonzero second derivatives: \(\partial^2f^2/\partial v^2=-6h\gamma\bar v\). The added \(\mathcal Q_{xx}\) term therefore has one nonzero entry, \(-6h\gamma\bar v(v_+)_2\), in its velocity-velocity position. Because the dynamics are affine in input, \(f_{uu}=f_{ux}=0\). DDP can still differ from iLQR: the changed value Hessian propagates backward and affects earlier control curvatures and gains.

With linear dynamics, all second-derivative corrections vanish. They also vanish at a particular step when the next value gradient is zero, even if the dynamics are nonlinear there. These limits are useful checks for an implementation. DDP's additional curvature can improve a local model; it does not remove the need for initialization, damping, or acceptance checks.

Original counterexample: more accurate curvature can propose a disastrous step

Consider one scalar transition \(x_1=u+\frac12u^2\) from fixed \(x_0=0\), with cost \(J(u)=0.05u^2+\frac12(x_1-1)^2\). At nominal input zero, the successor is zero, terminal value gradient is minus one, terminal Hessian is one, and both first and second input derivatives of the dynamics equal one. Thus \(J'(0)=-1\).

iLQR's approximate curvature is \(0.1+1=1.1\). DDP includes the additional value-gradient-weighted dynamics curvature, obtaining \(1.1-1=0.1\), which equals the actual \(J''(0)\). The respective full local steps are about \(0.90909\) and \(10\). The DDP curvature is more accurate at the expansion point, but its proposed step is far outside a useful neighborhood.

The original cost is \(J(0)=0.5\). At \(u=10\), it is \(1745.5\). At the iLQR step it is approximately \(0.09326549\). Reducing the DDP step by a factor of sixteen gives \(u=0.625\) and cost \(0.03567505\). These values show why comparing algorithms solely by local derivative order is inadequate. The direction, step size, nonlinear rollout, and acceptance rule operate together.

This example does not establish that iLQR is generally better or worse than DDP. It demonstrates a narrower fact: even an exact positive Hessian at the current point does not make an unrestricted Newton-like step safe for a nonquadratic objective.

A tested nonlinear cart optimizer

The original function nonlinear_cart_ilqr in numerical_lab.py uses \(h=0.1\), \(\gamma=0.2\), \(N=30\), initial state \((1,0)\), and the cost matrices defined above. It starts from zero controls, runs a backward pass with analytic Jacobians, and evaluates each candidate with the full supplied nonlinear discrete model. It returns states, controls, accepted costs, and the infinity norm of the exact discrete objective gradient. Invalid parameters, nonpositive control curvature, failed line search, and exhausted iterations raise exceptions.

The exact gradient used for stopping is independently accumulated through an adjoint recursion. For current rollout \(x_k,u_k\), start \(a_N=Hx_N\), then compute \(\nabla_{u_k}J=ru_k+B^\mathsf Ta_{k+1}\) and \(a_k=Qx_k+A_k^\mathsf Ta_{k+1}\). Here \(a_k\) is an adjoint vector, unrelated to the affine value constant earlier. This gradient uses the current nonlinear trajectory, not the local predicted improvement. A small gradient is a first-order stationarity check, not a global optimality certificate.

import numpy as np
from numerical_lab import nonlinear_cart_ilqr, finite_lqr, rollout

states, controls, costs, gradient = nonlinear_cart_ilqr()
assert all(b < a for a, b in zip(costs, costs[1:]))
assert gradient < 1e-7
assert abs(costs[0] - 25.0) < 1e-12
assert abs(costs[-1] - 4.214707226190362) < 1e-9

# Removing nonlinear drag must recover the independently implemented LQR.
linear_states, linear_controls, _, _ = nonlinear_cart_ilqr(drag=0)
A, B = np.array([[1., .1], [0., 1.]]), np.array([[0.], [.1]])
gains, _ = finite_lqr(A, B, np.diag([1., .1]), np.array([[.05]]),
                      np.diag([20., 2.]), 30)
expected_states, expected_controls = rollout(A, B, gains, np.array([1., 0.]))
assert np.allclose(linear_states, expected_states, atol=1e-10)
assert np.allclose(linear_controls, expected_controls[:, 0], atol=1e-10)

# Check the dynamics-curvature term by differencing the original objective.
def objective(u):
    return .05*u*u + .5*(u+.5*u*u-1)**2
epsilon = 1e-4
curvature = (objective(epsilon)-2*objective(0)+objective(-epsilon))/epsilon**2
assert abs(curvature-.1) < 1e-7
assert objective(10) > objective(0)
assert objective(10/16) < objective(0)
print(round(costs[-1], 9), f"{gradient:.3e}")

The accepted nonlinear costs are \(25,\ 4.21838855,\ 4.21470859,\ 4.21470723\), followed by smaller refinements to \(4.21470722619\). The final gradient infinity norm is approximately \(4.67\times10^{-9}\), below the requested \(10^{-7}\) tolerance. The final state is approximately \((-0.00544537,0.01107909)\). In the zero-drag limit, the maximum control difference from the independent LQR recursion is below \(2\times10^{-15}\) in the recorded run.

The first optimized control is approximately \(-3.79224\). This large magnitude is permissible only because the experiment has no actuator bound. The code does not quietly clip it. Adding a physical limit would require a changed optimization problem and a corresponding method. A separate dense BFGS comparison reached the same cost and controls within about \(2.5\times10^{-8}\), but reported precision loss; it is corroborating numerical agreement rather than an independently successful convergence certificate.

Exercises with worked solutions

From local deterministic improvement to uncertainty

The implementation assumes differentiable dynamics, full state access, a feasible rollout, and unconstrained inputs. The reference and local models are exact only for the supplied discrete equations. Contact events, saturations, discontinuous costs, and changing obstacles require additional treatment. Specialized constrained DDP variants exist; the limitation concerns the unconstrained Riccati update derived here, not every algorithm bearing the DDP name. Interior Point Differential Dynamic Programming is a primary example of explicitly adding inequality-constraint machinery.

The next chapter changes the Bellman recursion itself by allowing random transitions. A local deterministic optimizer evaluates one predicted successor, while stochastic dynamic programming weighs possible successors by their probabilities. That change will require keeping expected performance separate from worst-case safety.

Primary sources

• AA203 lecture 8 recording and official lecture slides.
• Harrison and Buurmeijer, Optimal and Learning-Based Control: Course Notes, Sections 3.1 and 3.2, version of April 12, 2026.
• Boyd, discrete LQR and tracking extensions, Stanford EE363.
• Tassa, Erez, and Todorov, Synthesis and Stabilization of Complex Behaviors through Online Trajectory Optimization, IROS 2012.
• Tassa, Theory and Implementation of Biomimetic Motor Controllers, doctoral thesis, Hebrew University of Jerusalem, 2011, Section 2.2.3.
• Pavlov, Shames, and Manzie, Interior Point Differential Dynamic Programming, 2020 preprint.