iLQR Tutorial

Brian Jackson, Taylor Howell

Robotic Exploration Lab, Stanford University

July 2, 2019

1 LQR Derivation
1.1 Discrete Case (Regulator)

xk+1 = Ak xt + Bk uk
N −1
1 T 1 X T
J(x0 , U ) = xN Qf xN + xk Qk xk + uTk Rk uk
2 2
k=1

Using the Principle of Optimality and Pontryagin’s Minimum Principle, we can compute the optimal cost-to-go
Vk (xk ) = 12 xk Sk xk by working backwards from the boundary condition/terminal cost.
1 T 1
VN (xN ) = x Qf xN = xTN SN xN
2 N 2
1 T 1
VN −1 (xN −1 ) = min xN −1 QN −1 xN −1 + uTN −1 RN −1 uN −1 + VN (xN )
uN −1 2 2
1 T 1 T
= min xN −1 QN −1 xN −1 + uN −1 RN −1 uN −1 + VN (AN −1 xN −1 + BN −1 uN −1 )
uN −1 2 2
1 T 1 T 1
T

= min xN −1 QN −1 xN −1 + uN −1 RN −1 uN −1 + AN −1 xN −1 + BN −1 uN −1 SN AN −1 xN −1 + BN −1 uN −1
uN −1 2 2 2
1 T 1 T
= min xN −1 QN −1 xN −1 + uN −1 RN −1 uN −1
uN −1 2 2
1 T T
+ xN −1 AN −1 SN AN −1 xN −1 + uTN −1 BN
T T T T T
−1 SN BN −1 uN −1 + xN −1 AN −1 SN BN −1 uN −1 + uN −1 BN −1 SN AN −1 x
2
(1)
Using Pontryagin’s Minimum Principle we can solve for the optimal control for a single time step uk , instead of the
entire control sequence U .
∂V T T
= RN −1 uN −1 + BN −1 SN BN −1 uN −1 + BN −1 SN AN −1 xN −1 = 0
∂u
(2)
u∗N −1 = −(RN −1 + BN
T
−1 SN BN −1 )
−1 T
BN −1 SN AN −1 xN −1
≡ KN −1 xN −1
The optimal control can be substituted back into the above equation to compute the optimal cost-to-go
1 T 1
VN −1 (xN −1 ) = x QN −1 xN −1 xTN −1 KNT
−1 RN −1 KN −1 xN −1
2 N −1 2
1
T

+ AN −1 xN −1 + BN −1 KN −1 xN −1 PN AN −1 xN −1 + BN −1 KN −1 xN −1
2
1 T  T −1

= xN −1 QN −1 + KN R K
−1 N −1 N −1 + (A N −1 + B K
N −1 N −1 ) SN (A N −1 + B N −1 N −1 xN −1
K )
2
1
≡ xN −1 SN −1 xN −1
2
(3)

1
2 iLQR Derivation
We first start the derivation by setting up the problem by defining the dynamics, cost function, and cost-to-go
variable.

2.1 Discrete Dynamics

The dynamics are typically provided as differential equations. In order to apply iLQR, the dynamics must be
discretized with an appropriate quadrature rule (more details to follow in Section ??). Here we assume general,
non-linear, discretized dynamics:
xk+1 = f (xk , uk ) (4)
which we approximate with a first-order Taylor-series expansion about nominal trajectories X = {x0 , ...xN }, U =
{u0 , ..., uN −1 }:

∂f ∂f
xk+1 + δxk+1 = f (xk + δxk , uk + δuk ) ≈ f (xk , uk ) + (x − xk ) + (u − uk )
∂x xk ,uk ∂u xk ,uk
δxk+1 = A(xk , uk )δxk + B(xk , uk )δuk (5)
(6)
∂f ∂f
where A ≡ ∂x and B ≡ ∂u .

2.2 Cost Function

Most cost functions used iLQR are linear-quadratic cost functions. However, if the cost function is not linear-
quadratic, a second-order Taylor Series Expansion can be used to linearize the dynamics into a form common
for optimal control problems (note that constant terms are intentionally dropped as they have no impact on the
minimization):
N
X −1
J(x0 , U ) = `f (xN ) + `(xk , uk )
k=1
N −1
1 T T
X 1 T 1 1 1
≈ xN QN xN + qN xN + xk Qk xk + uTk Rk uk + xTk Hk uk + uTk HkT xk + qkT xk + rkT uk (7)
2 2 2 2 2
k=1

For the we define a few variables of convenience (→ indicates equivalence for quadratic cost function):

∂`
`x ≡ → Qk xk + qk
∂x xk ,uk
∂`
`u ≡ → Rk uk + rk
∂u xk ,uk
∂2`
`xx ≡ → Qk
∂x2 xk ,uk
(8)
∂2`
`uu ≡ → Rk
∂u2 xk ,uk
∂2`
`xu ≡ → Hk
∂x∂u xk ,uk
∂2`
`ux ≡ → HkT
∂u∂x xk ,uk

With a given cost function, we can apply Bellman’s Principle of Optimality to define the optimal cost-to-go Vk (x)
by the recurrence relation:
VN = `f (xN )
Vk = min{`(xk , uk ) + Vk+1 (f (xk , uk ))} (9)
u
Vk = min Qk (xk , uk )
u

2
We approximate the cost-to-go function as locally quadtratic near the nominal trajectory:

∂V 1 ∂2V
Vk + δVk = Vk (xk + δxk ) ≈ V (xk ) + (x − xk ) + (x − xk )T (x − xk )
∂x xk 2 ∂x2 xk
(10)
1
δVk (xk ) = sTk xk + xTk Sk xk
2
Similarly:

Qk + δQk = Q(xk + δx, uk + δu)

∂Q ∂Q
≈ Q(xk , uk ) + (x − xk ) + (u − uk )
∂x xk ,uk ∂u xk ,uk
1 ∂2Q 1 ∂2Q
+ (x − xk )T 2
(x − xk ) + (u − uk )T (u − uk )
2 ∂x xk ,uk 2 ∂u2 xk ,uk
1 ∂2Q 1 ∂2Q
+ (u − uk )T (x − xk ) + (x − xk )T (u − uk )
2 ∂u∂x xk ,uk 2 ∂x∂u xk ,uk

 T     T  
1 δxk Qxx Qxu δxk Qx δxk
δQk (xk , uk ) = +
2 δuk Qux Qxx δuk Qu δuk

We define the following variables using matrix calculus :

Qx = `x + sk+1 Ak
Qu = `u + sk+1 Bk
Qxx = `xx + ATk Sk+1 Ak (11)
Quu = `uu + BkT Sk+1 Bk
Qux = `ux + BkT Sk+1 Ak

and using the fact the Qux = QTxu .

which gives us all the values needed to calculate the next step. We can show this by combining Equations 11 and 12:

2.3 Cost-to-go at terminal state

By following a dynamic-programming approach, we can solve the tail problem for VN (x) for a problem with N time
steps. In order to solve the detail we define δV as the deviation from the optimal value: with sN and SN defined as
follows, given the cost function in Equation 7:
∂V
sN ≡
∂x xN
∂ 1
(x − xf )T Qf (x − xf )


=
∂x 2 xN
∂ 1 T T 1 T

= x Qf x − xf Qf x + xf Qf xf
∂x 2 2 xN
= Qf xN − Qf xf (Qf = QTf )
= Qf (xN − xf )
∂2V
SN ≡
∂x2 xN
∂

= Qf (x − xf )
∂x xN
= Qf

3
2.4 Solving the Dynamic Programming Problem
After solving the tail sub-problem, we can then apply the principle of optimality and define the process for solving
for the kth time step given the values at the k + 1th time step.

Vk = min{`(xk , uk ) + Vk+1 (f (xk , uk ))}

uk

= min{Qk (xk , uk )}
uk

δV = min{δQ(x, u)}
δu
1 1 1 1
= min{Qx δx + Qu δu + δxT Qxx δx + δuT Quu δu + δxT Qxu δu + δuT Qux δx}
δu 2 2 2 2

∂δQ 1 1
= Qu + Qux δx + QTxu δx + Quu δu = 0
∂δu 2 2

→ δu∗ = −Q−1
uu (Qux δxk + Qu)
= Kδx + d

So

dk = −Q−1
uu Qu
Kk = −Q−1
uu Qux

After calculating the optimal control as a function of the next time step we can plug it back into Equation ??.

 T     T  
1 δxk Qxx Qxu δxk Qx δxk
δQk (xk , uk ) = +
2 Kδx + d Q ux Qxx Kδx + d Qu Kδx + d

By equating the result with Equation ?? we get

1 T
∆V = d Quu d + dT Qu
2
s = Qx + K T Quu d + K T Qu + QTux d (12)

S = Qxx + K T Quu K + K T Qux + QTux K

2.5 Forward Pass

Since the dynamics and the cost function are only approximated at each time step, it is necessary to iteratively
solve the previous problem to successively get closer to the local minimum. After each backward pass solving for
the optimal correction in control values, δu∗k , these values are used to calculate a new state trajectory (X) from the
nominal trajectories X̄, Ū , often referred to as a “rollout”. The α term is used for a line search. This is done using
the following algorithm:

δx = xk − x̄k

uk = ūk + δu∗k
= ūk + Kk δxk + αdk

xk+1 = f (xk , uk )

where α is the step size, typically used to perform a simple line search (see Section ??).

4
2.6 Square-Root Backward Pass (V3)
√ √ √
2.6.1 A+ B→ A+B
√ 
A
Q, R = QR( √ )
B
√
R= A+B

2.6.2 Method
We approximate the state-action cost-to-go Q(x, u) using a second-order Taylor series expansion:
 T     T  
1 δx Qxx Qxu δx Qx δx
δQ(x, u) = +
2 δu Q ux Qxx δu Qu δu

The terminal cost-to-go Hessian and stage costs are factored:

Tp p
SN = SN SN
p Tp
Lii = Lii Lii

This factorization can be performed using Cholesky (or LDL) decomposition

The backward pass is modified with the diagonal blocks of the state-action cost-to-go Q(x, u) factored as follows:

√
p Tp T 0T 0
√ 0
T Lxx
Qxx = Lxx Lxx + A S S A = Lxx SA
S0A
√ 
p Tp T Luu
Luu + B T S 0T S 0 B = 0
√
Quu = Luu Luu SB
S0B

√ T√
We can factor Qxx → Qxx Qxx using the above technique:
√ 
p Lxx
−, Qxx = QR( )
S0A

Similarly for Quu :

√ 
p Luu
−, Quu = QR( )
S0B

The gain K = −Q−1

uu Qux can now be written in its square root form:
p −1 p −T
K=− Quu Quu Qux

The gain d = Q−1

uu Qu can be written in its square root form as well:
p −1 p −T
d = − Quu Quu Qu

The gradient of the cost-to-go s = Qx + K T Quu d + K T Qu + QTux d can be expressed as:

p T p
s = Qx + (K T Quu )( Quu d) + K T Qu + QTux d

The square root of the Hessian of the cost-to-go S = Qxx + K T Quu K + K T Qux + Qu xT K is derived:
 T   
I Qxx Qxu I
S=
K Qux Quu K

5
   T   T
αT β
  
Qxx Qxu α 0nxm α β α α
= T = T
Qux Quu β γT 0mxn γ β α βT β − γT γ

p
α= Qxx
p −T
β = Qxx Qux
q
p Tp
γ= Quu Quu − β T β

√
  
β I α
S=
0mxn γ K
 
α + βK
=
γK
√ √
Note: that S ∈ Rnxn , S ∈ R(n+m)xn and SN [1:n,1:n] = chol(SN )

2.7 Regularization
Due to limited numerical precision, it is common for Quu to become not positive definite. To address this problem,
regularization is added in a way that is equivalent to using trust region methods. Additionally, there are two options
for regularization. The first penalizes deviations from a control trajectory and the second penalizes deviations for
the state trajectory.
Option 1:
Q̃uu = `uu + B T S 0 B + ρI

d = −Q̃−1
uu Qu

K = −Q̃−1
uu Qux

1 T
∆V = d Quu d + dT Qu
2
s = Qx + K T Quu d + K T Qu + QTux d
S = Qxx + K T Quu K + K T Qux + QTux K

Option 2:
Q̃uu = `uu + B T (S 0 + ρI)B
Q̃ux = `ux + B T (S 0 + ρI)A

d = −Q̃−1
uu Qu

K = −Q̃−1
uu Q̃ux

1 T
∆V = d Quu d + dT Qu
2
s = Qx + K T Quu d + K T Qu + QTux d
S = Qxx + K T Quu K + K T Qux + QTux K

Note the subtle but very important distinctive use of Quu and Q̃uu (and Q̃ux ). The former is used to estimate
the cost-to-go and is propagated backward, while the regularized versions are used to compute the optimal gains.
Generally, the second option is more robust and is preferred.

6
3 Constraints
3.1 Augmented Lagrange Method
In order to solve systems with equality and inequality constraints we define a new cost function:
N
X −1
LA (x0 , U ; µ, λ) = `f (xN ) + {`(xk , uk )}
k=1
N −1
1 X 1
+ cN (xN )T IµN cN (xN ) + λTN cN (xN ) + { ck (xk , uk )T Iµk ck (xk , uk ) + λTk ck (xk , uk )}
2 2
k=1

We define a single equality constraint as:

cik (x, u) = 0, i ∈ E
cik ∈ C 1 , ∈ IR

and a single in equality constraint as:

cik (x, u) ≤ 0, i ∈ I
cik ∈ C 1 , ∈ IR

using subscripts to denote time index and superscripts to denote a particular constraint. The vector ck ∈ IRp is the
ordered (equality, then inequality), vertical concatenation of all p constraints at that time step.
The matrix Iµk ∈ IRpxp is used to turn on and off feasible inequality constraints and is defined as follows:
(
µik if any: i ∈ E, cik ∈ I > 0, λik > 0
Iµk [i, i] =
0 otherwise

The update for λ is defined as follows:

λik ← λik + µik cik (xk , uk ), i ∈ E

λik ← max(0, λik + µik cik (xk , uk )), i ∈ I

3.2 Modification to Backward Pass

To account for constraints in the optimization, we modify the backward pass as follows:

Q̂x = Qx + cTx Iµ c + cTx λ

Q̂u = Qu + cTu Iµ c + cTu λ
Q̂xx = Qxx + cTx Iµ cx
Q̂uu = Quu + cTx Iµ cx
Q̂ux = Qux + cTu Iµ cx

∂c ∂c
where cx = ∂x |x,u , cu = ∂u |x,u Additionally the boundary conditions SN , sN are augmented:

ŜN = SN + cTxN IµN cxN

ŝN = sN + cTxN IµN cN + cTxN λN

The backward pass can be performed as before using these augmented versions.

7
4 Infeasible Initial Trajectory
Given a desired state trajectory Xd : {xd1 , ...xdn } we must find a set of artificial/slack controls Ui : {ui1 , ...uin−1 } to
achieve this state trajectory despite the system dynamics. At each time step we solve for uik :

xk+1 = f (xk , uk ) + uik

s.t. xk+1 = xdk+1
→ uik = xdk+1 − xk+1

Before simulating the dynamics at the next step remember to apply the artificial control we just calculated: xk+1 + =
uik

5 Minimum Time
6 Taylor Series Approximation
Throughout the following derivations we use Taylor polynomials to approximate nonlinear functions. We are typi-
cally interested in first or second order approximations of nonlinear functions of two variables, the state and control.
The following derivations may prove helpful.

A second order Taylor Series approximation of f (x), a nonlinear scalar valued function that is dependent one
variable (x ∈ R) and linearized about x̄ (which is not necessarily zero):

f (x) → (13)
2
∂f 1∂ f
f (x̄ + δx) ≈ f (x̄) + (x − x̄) + (x − x̄)2 (14)
∂x x̄ 2 ∂x2 x̄
∂f 1 ∂2f
≈ f (x̄) + δx + δx2 (15)
∂x x̄ 2 ∂x2 x̄
A second order Taylor Series approximation of f (x), a nonlinear vector valued function that is dependent one variable
(x ∈ Rn ) and linearized about x̄ (which is not necessarily zero):

f (x) → (16)
2
∂f 1 ∂ f
f (x̄ + δx) ≈ f (x̄) + (x − x̄) + (x − x̄)T 2 (x − x̄) (17)
∂x x̄ 2 ∂x x̄
∂f 1 T ∂2f
≈ f (x̄) + δx + δx δx (18)
∂x x̄ 2 ∂x2 x̄
The most complicated approximation we perform is a second order Taylor Series approximation of a nonlinear
function f that is dependent on three variables. The procedure is the same as before:

f (x, y, z) →
f (x̄ + δx, ȳ + δy, z̄ + δz)
∂f ∂f ∂f
≈ f (x̄, ȳ, z̄) + (x − x̄) + (y − ȳ) + (z − z̄)
∂x x̄,ȳ,z̄ ∂y x̄,ȳ,z̄ ∂z x̄,ȳ,z̄
1 ∂2f 1 ∂2f 1 ∂2f
(x − x̄)T 2 (x − x̄) + (y − ȳ)T 2 (y − ȳ) + (z − z̄)T 2 (z − z̄)
2 ∂x x̄,ȳ,z̄ 2 ∂y x̄,ȳ,z̄ 2 ∂z x̄,ȳ,z̄
1 ∂2f 1 ∂2f
(x − x̄)T (y − ȳ) + (x − x̄)T (z − z̄)
2 ∂x∂y x̄,ȳ,z̄ 2 ∂x∂z x̄,ȳ,z̄
1 ∂2f 1 ∂2f
+ (y − ȳ)T (x − x̄) + (y − ȳ)T (z − z̄)
2 ∂y∂x x̄,ȳ,z̄ 2 ∂y∂z x̄,ȳ,z̄
1 ∂2f 1 ∂2f
+ (z − z̄)T (x − x̄) + (z − z̄)T (y − ȳ)
2 ∂z∂x x̄,ȳ,z̄ 2 ∂z∂y x̄,ȳ,z̄

8
7 Appendix
7.1 Square-Root Backward Pass(DONT USE)
It is common for S to become ill-conditioned, which causes numerical instability when performing the updates in (19).
This can be quantified using a conditioning number, which is a ratio between the largest and smallest eigenvalues of
a square matrix [Kaminkski’71] To alleviate numerical issues, we can derive a backward pass using only the square
root of S, defined as S = LLT , where L is the is value to be calculated and propagated backward in time. This is
done by using the a factor of S when calculating K, d, s, S (and Quu ) during the backward pass. To reduce visual
noise, we drop subscripts and define Sk ≡ S̄ and Sk+1 ≡ S. We also define A−T ≡ (A−1 )T .

K = Q−1
uu Qux
 −1
= `uu + B T SB B T SA
 −1
= `−1
uu B
T
S −1 + B`−1
uu B
T
A (Matrix Inversion Lemma2 )
 −1
= `−1
uu B
T
L−T L−1 + B`−1 uu B
T
A ((A−1 )T = (AT )−1 , S = LLT )
 −1
= `−1
uu B
T
L−T L−1 + L−T LT B`−1 T
uu B LL
−1
A (AA−1 = I)
h i−1
= `−1
uu B
T
L−T (I + LT B`−1 T
uu B L)L
−1
A (Factor out L−T (left), L−1 (right))
 −1
K = `−1 T T −1 T
uu B L I + L B`uu B L LT A ((ABC)−1 = C −1 B −1 A−1 )

In order to calculate d we will first calculate Quu in terms of the factored S:

Quu = B T SB + `uu
= B T LLT B + LR LTR (S = LLT , LR = chol(R))
= MMT
where M ≡ B T L
 
LR
= QRRT QT (Q, R = QR(M ))
LQuu LTQuu = QDQ T
(D is a diagonal matrix)
↓
LQuu = QD1/2

With Quu factorized with respect to L we can compute d in terms of L:

d = Q−1 T
uu Qu
 −1
= LQuu LTQuu Qu

Next, we can calculate S̄:

S̄ = `xx + AT SAk − ATk SB(`uu + B T SB)−1 B T SA

= AT (S −1 + B`1uu B T )1 A + `xx (Matrix Inversion Lemma1 )
A(S̄ − `xx )−1 AT = S −1 + B`−1
uu B
T
(Re-arrange and invert)
=L −T −1
L + L L BL−T
−T T −1 T
R LR B LL
−1
(S = LLT , LR = chol(`uu ))
= L−T (I + LT BL−T −1 T
R LR B L)L
−1

= L−T (I + GG )L T −1

where G ≡ LT BL−T
R ,G ∈ R
nxm

S̄ = AT L(I + GGT )−1 LT A + `xx (Re-arrange and invert)

9
 = AT L(I + W ΛW T )−1 LT A + `xx (W ΛW T = LDL(GGT ))
= AT L(W W T + W ΛW T )−1 LT A + `xx (W W T = I, W is orthogonal)
= AT L(W (I + Λ)W T )−1 LT A + `xx (Factor out W, W T )
= AT LW −T (I + Λ)−1 W −1 LT A + `xx
= AT LW (I + Λ)−1 W T LT A + `xx (W T = W −1 , W is orthogonal)
= AT LW (I + Λ)−1/2 (I + Λ)−1/2 W T LT A + LQ LTQ (LQ = chol(`xx ))
T
= MM
where M ≡ AT LW (I + Λ)−1/2
 
LQ
= QRRT QT (Q, R = QR(M ))
T T T
L̄L̄ = QV DV Q (V, D = LDL(RRT ))
↓
L̄ = QV D1/2
Finally, we can calculate s̄:
s̄ = Qx − K T Qu + K T QTuu d − QTux d
= Qx − (B T SA)T (`uu + B T SB)−T Qu + (B T SA)T (`uu + B T SB)−1 Qu − (B T SA)T (`uu + B T SB)−1 Qu
= Qx − (B T SA)T (`uu + B T SB)−T Qu ((ABC)T = C T B T AT )
= Qx − AT S T B(`uu + B T SB)−T Qu
 −1
= Qx − AT S −T + B`−Tuu B
T
B`−T
uu Qu (Matrix Inversion Lemma2 )
 −1
= Qx − AT L−T L−1 + B`−T uu B
T
B`−T
uu Qu ((A−1 )T = (AT )−1 , S = LLT )
 −1
= Qx − AT L−T L−1 + L−T LT B`−T T
uu B LL
−1
B`−T
uu Qu (AA−1 = I)
h i−1
= Qx − AT L−T (I + LT B`−T T
uu B L)L
−1
B`−T
uu Qu (Factor out L−T (left), L−1 (right))
s̄ = Qx − AT L(I + LT B`−T T
uu B L)
−1 T
L B`−T
uu Qu ((ABC)−1 = C −1 B −1 A−1 )

NOTE: chol(A) is the Cholesky decomposition, A = BB T , LDL(A) is an orthogonalization of a symmetric matrix,

A = LDLT , where L is an orthogonal matrix and D is a diagonal matrix. This factorization is most easily accom-
plished using the eigenvector decomposition, where L is the matrix of eigenvectors of A and D is a diagonal matrix
comprising the eigenvalues of A. QR(A) is the QR decomposition of A. Matrix Inversion Lemmas:
 −1 
A + U CV = A−1 − A−1 U V A−1 U + C −1 )V A−1
 −1  −1
A + U CV U C = A−1 U C −1 + V A−1 U

The algorithm for the square-root backward pass:

G = LT BL−T
R
W, Λ = LDL(GGT )
M = AT LW (I + Λ)−1/2
 
LQ
Q, R = QR(M )
V, D = LDL(RRT )
L̄ = QV D1/2

7.2 L derivation for FOH

−
Lxx = 4M1T Lm
xx M1 + Lxx

10
 −
Lxu = 2M1T Lm m
xu + 4M1 Lxx M2 + Lxu

Lxy = M1T Lm
xx
Lxv = 2M1 Lm
xu
−
Luu = Lm T m m m
uu + 2(M2 Lxu + Lux M2 ) + 4M2 Lxx M2 + Luu
1
Luy = Lm + M2T Lm
2 ux xx

Luv = Lm T m
uu + 2M2 Lxu
1
Lyy = Lm + L+
4 xx xx

1
Lyv = Lm + L+
2 xy xu

Lvv = Lm +
uu + Luu

Lx = Lmx M1 + Lx
+

1 −
Lu = Lm + Lmx M2 + Lu
2 u
1
Ly = Lm + L+
4 x x

1
Lv = Lm + L+
2 u u

7.3 Verbose iLQR derivation

Vk + δVk = min `k (xk + δxk , uk + δuk ) + Vk+1 (f (xk + δxk , uk + δxk ))

uk ,δuk

≈ min `(xk , uk ) + `x (x − xk ) + `u (u − uk ) (Taylor series expansion)

uk ,δuk
1 1
+ (x − xk )T `xx (x − xk ) + (u − uk )T `uu (u − uk )
2 2
1 1
+ (u − uk )T `ux (x − xk ) + (x − xk )T `xu (u − uk )
2 2
1
+ V (xk+1 , uk+1 ) + sk+1 δxk+1 + δxk+1 Sk+1 δxk+1
2
= min `(xk , uk ) + V (xk+1 , uk+1 ) (Separate minimizations)
uk
1 1
+ min `x δxk + `u δuk + δxTk `xx δxk + δuTk `uu δuk
δuk 2 2
1 1
+ δuTk `ux δxk + δxTk `xu δuk
2 2
1
+ sk+1 δxk+1 + δxk+1 Sk+1 δxk+1
2
1 1
δVk = min +`x δxk + `u δuk + δxTk `xx δxk + δuTk `uu δuk (Cancel terms)
δuk 2 2
1 1
+ δuTk `ux δxk + δxTk `xu δuk
2 2
1
+ sk+1 δxk+1 + δxk+1 Sk+1 δxk+1
2
1 1
= min `x δxk + `u δuk + δxTk `xx δxk + δuTk `uu δuk (Plug in dynamics)
δuk 2 2
1 1
+ δuTk `ux δxk + δxTk `xu δuk
2 2
1
+ sk+1 (Aδxk + Bδuk ) + (δuTk B T + δxTk AT )Sk+1 (Aδxk + Bδuk )
2

11
    
= min `x + sk+1 A δxk + `u + sk+1 B δuk (Combine Terms)
δuk
1   1  
+ δxTk `xx + AT Sk+1 A δxk + δuTk `uu + B T Sk+1 B δuk
2 2
1 T  1  
+ δuk `ux + B Sk+1 A δxk + δxTk `xu + AT Sk+1 B δuk
T
2 2

7.4 (
unnecssary plug in for s,S)

sk = qk + sk+1 Ak − (qk + sk+1 Bk )(Rk + BkT Sk+1 Bk )−1 (BkT Sk+1 Ak )

(19)
Sk = Qk + ATk Sk+1 Ak − ATk Sk+1 Bk (Rk + BkT Sk+1 Bk )−1 BkT Sk+1 Ak

which is easy to verify that all values are either functions of the current state and control (subscripts k), or are
from the next time step. By working backwards, the Sk , sk , and u∗k values can be calculated for each time step.
subsectionincorrect foh derivation

7.4.1 Simpson Integration of Stage Cost

The first step is to express L(xk , uk , xm , um , xk+1 , uk+1 ) as a linear expression of (xk , uk , xk+1 , uk+1 ) by finding
expressions for (xm , um ) in terms of the other variables. Hermite-Simpson integration performs the following ap-
proximation:
Z tk+1
x(t) = f (x, u)dt (20)
tk
2
≈ at + bt + c (21)
(22)

Without loss of generality, we assume knowledge of the points (xk , uk , xk+1 , uk+1 ) and assume tk = 0, tk+1 = dt. By
specifying 3 known points we can solve for the coefficients (a, b, c). We pick the points xk , xk+1 , ẋk = f (xk , uk ):
    
xk 0 0 1 a
 ẋk  =  0 1 0  b 
xk+1 dt2 dt 1 c
  (23)
a
= T b
c
Solving this linear equation gives
1
a= (−xk − ẋk dt + xk+1 )
dt2
b = ẋk (24)
c = xk
We can now calculate xm by evaluating the spline (Eq. 22) at dt/2:
 
1 2 1  a
xm = 4 dt b
2 dt 1
c
 
1 2 1  −1 xk
= 4 dt  ẋk 
2 dt 1 T (25)
xk+1
 
1  xk
= 3 dt 1  ẋk 
4
xk+1

12
which can be verified by either plugging in the expressions for a, b, c in Eq 24 or using a symbolic solver (e.g. SymPy).

In order to express [xk ẋk xk+1 ]T as a linear expression in the desired variables, we need to perform a 1st Order
Taylor-Series expansion of the dynamics:
∂f ∂f
ẋ + δ ẋ ≈ f (x, u) + δx + δu
∂x ∂u
= f (x, u) + A(x, u)δx + B(x, u)δu (26)

δ ẋ = A(x, u)δx + B(x, u)δu

It is important that these Jacobian matrices are distinct from the ones for linearized discrete dynamics in (43). These
Jacobians are calculated on the continuous dynamics equation.
Using this result we can now express the following:
 
    δxk
δxk In 0 0 0 
 δ ẋk  =  A B 0 0  δuk 

δxk+1  (27)
δxk+1 0 0 In 0
δuk+1

where In is an n × n identity matrix and Im is an m × m identity matrix.

We can now define the following:
 
  δxk
I
 n 0 0 0
1  δuk 
xm = 3 dt 1  A B 0 0  
δxk+1 
4
0 0 In 0
δuk+1
 
δxk
1   δuk 
= 3In + dtA dtB In 0  
δxk+1  (28)
4
δuk+1
 
δxk
 δuk 
=M δxk+1 


δuk+1

With an expression for xm , all we need is an expression for um . This expression is obtain using a simple interpolation:
um = 21 (uk + uk+1 ). Using both of these relationships we can can convert between point sets:
   
δxk In 0 0 0  
 δuk   0 Im 0 0
    δxk
 δxm   [M ]   δuk 
 δum  =  0
   1

1 

0 2  δxk+1

   2
δxk+1   0 0 In 0  δuk+1
δuk+1 0 0 0 Im (29)
 
δxk
 δuk 
=E δxk+1 


δuk+1

13
7.4.2 Approximating Stage Cost
We can find a quadratic expression for L(xk , uk , xk+1 , uk+1 ) by taking a 2nd Order Taylor Series Expansion of the
integral term in (41):
T T
L− L−
  
δxk xx xu δxk
 δuk  L− L−   δuk 
   ux uu  
dt  δxm   4Lm
xx 4Lm
xu
  δxm 
δLk ≈      (30)
6  δum 
 
 4Lm
ux 4Lm
uu
  δum 
  
δxk+1   L+
xx L+
xu
 δxk+1 
δuk+1 L+
ux L+
uu δuk+1
 T
δxk
 δuk 
 
dt  − +  δxm 
L−
 
+ Lx u 4Lm
x 4Lm
u L+
x Lu  (31)
6  δum 

δxk+1 
δuk+1
L− L−
 
T xx xu
−
L−
  
δxk Lux
 uu

 δxk
dt 
 δuk  E T 
  4Lm
xx 4Lm
xu
  δuk 
= E   (32)
6 δxk+1
  
 4Lm
ux 4Lm
uu
 δxk+1 

δuk+1  L+xx L+
xu
 δuk+1
L+ux L+
uu
 
δxk
dt  − δu k 
L−
 
4Lm 4Lm L+ L+

+ Lx u x u x u E
 (33)
6 δxk+1 
δuk+1

∂2L ∂2L
where Lx := ∂L
∂x , Lxx := ∂x2 , Lxu := ∂xu , etc. and L− , Lm , L+ are the partials evaluated at (xk , uk ), (xm , um ), (xk+1 , uk+1 ),
respectively.

Using a symbolic solver or chugging through the matrix multiplication by hand we arrive at the final expression for
L(xk , uk , xk+1 , uk+1 ):

7.5 Calculating P̄
Once we have δ L̄, we also need δP¯ from Equation ?? in order to calculate the elements of Q in Equation 51. We
simply plug in the linearized discrete dynamics (Eq. 43) into our cost-go-to for the next time step:
 T     
1 δy Syy Syv δy   δy
δP (δy, δv) = + Sy Sv (34)
2 δv Svy Svv δv δv
 T   
1 Aδx + Bδu + Cδv Syy Syv Aδx + Bδu + Cδv
δ P̄ (δx, δu, δv) = (35)
2 δv Svy Svv δv
 
  Aδx + Bδu + Cδv
+ Sy Sv
δv
 T     
δx Gxx Gxu Gxv δx δx
1  
= δu Gux Guu Guv  δu G + Gx Gy Gv δu (36)
2
δv Gvx Gvu Gvv δv δv

Gxx = AT Syy A
Guu = B T Syy B
Gvv = C T Syy C + C T Syv + Svy C + Svv (37)
T
Gxu = A Syy B

14
 Gxv = AT Syy C + AT Syv
Guv = B T Syy C + B T Syv

Gx = Sy A
Gy = Sy B (38)
Gv = Sy C + Sv

7.6 Calculating the Gains

With L̄ and P̄ we can now find the partials of Q in Eq. 51.

δQ(x, u, v) = δ L̄(δx, δu, δv) + δ P̄ (δx, δu, δv)

 T    T      
δx δx δx δx δx δx
1  1
= δu H δu + δu G δu + h δu + g δu
2 2
δv δv δv δv δv δv
 T    
δx δx δx
1
= δu (H + G) δu + (h + g) δu (39)
2
δv δv δv
 T     
δx Qxx Qxu Qxv δx δx
1  
= δu Qux Quu Quv  δu + Qx Qu Qv δu
2
δv Qvx Qvu Qvv δv δv

Qxx = Hxx + Gxx

Qxy = Hxy + Gxy
etc . . .

However, we need S in its square root form U . To accomplish this, we perform recursive rank-1 downdates on
−1
Wxx using the rows of Wuu Qux . An example of this process in Julia:
U = LinAlg . Cholesky (Wxx, ’ U’ )
tmp = Wuu’ \ Qux
f o r i = 1 : s i z e ( luu , 1 )
U = LinAlg . lowrankdowndate (U, tmp [ i , : ] )
end
U = U [ : U]

1 1
δV (xk ) = Qx δxk + Qu δxu + δxTk Qxx δxk + δuTk Quu δuk
2 2
1 T 1 T T
+ δuk Qux δxk + δxk Qux δuk
2 2
1 1
δVk = Qx δxk + Qu (−Quu (QTu + Qux δxk )) + δxTk Qxx δxk + (−Q−1
−1 T T −1 T
uu (Qu + Qux δxk )) Quu (−Quu (Qu + Qux δxk ))
2 2
1 1 T T
+ (−Q−1 T T −1 T
uu (Qu + Qux δxk )) Qux δxk + δxk Qux (−Quu (Qu + Qux δxk ))
2 2
= Qx δxk − Qu Q−1 T −1
uu Qu − Qu Quu Qux δxk
1 1
+ δxTk Qxx δxk + (δxTk QTux + Qu )Q−T T
uu (Qu + Qux δxk )
2 2
1 1
+ (−Qu Q−T T T −T T T −1 T T T −1
uu Qux δxk − δxk Qux Quu Qux δxk ) + (−δxk Qux Quu Qu − δxk Qux Quu Qux δxk )
2 2
= Qx δxk − Qu Q−1 T −1
uu Qu − Qu Quu Qux δxk
1 1
+ δxTk Qxx δxk + (δxTk QTux Q−T T −T T T T −T −T
uu Qu + Qu Quu Qu + δxk Qux Quu Qux δxk + Qu Quu Qux δxk )
2 2

15
 1 1
+ (−Qu Q−T T T −T T T −1 T T T −1
uu Qux δxk − δxk Qux Quu Qux δxk ) + (−δxk Qux Quu Qu − δxk Qux Quu Qux δxk )
2 2
−1 T 1
= −Qu Quu Qu + (Qu Q−T T −1 −1 −1
uu Qu ) + (Qx − Qu Quu Qux + Qu Quu Qux − 1Qu Quu Qux )δxk
2
1
+ δxTk (Qxx + QTux Q−T T −T
uu Qux − 2Qux Quu Qux )δxk
2
1 1 T
= − (Qu Q−T T −1 T −T
uu Qu ) + (Qx − Qu Quu Qux )δxk + δxk (Qxx − Qux Quu Qux )δxk
2 2

7.7 Active Set - Second-order Multiplier Update

[See: Constrained Optimization and Lagrange Multiplier Methods (Bertsekas 1996, Chapter 2.3)]

At each iteration of the backward pass we solve the following minimization:

1
min LA (xk , uk , λk ) = L(xk , uk ) + Vk+1 (xk+1 ) + ck (xk , uk )T Iµk ck (xk , uk ) + λTk ck (xk , uk )
xk ,uk 2

Where L is the stage cost and may incorporate infeasible or minimum time costs.

For notational convenience we define zk = [xk , uk ]T and Q(zk ) = L(zk ) + Vk+1 (zk+1 ). Active constraints are
denoted with a bar, for example: c̄. All equality and any violating inequality constraints are considered active.

Solving for the KKT conditions:

∂LA ∂Q ∂c̄T ∂c̄T ¯

= + λk + Iµ c̄k = 0
∂z ∂z ∂z ∂z k
∂LA
= c̄ = 0
∂λ

Taking the first order expansion of the KKT conditions:

∂LA ∂LA ∂ ∂LA ∂ ∂LA
≈ |zk ,λk + ( )|zk ,λk δz + ( )|zk ,λk δλ
∂z ∂z ∂z ∂z ∂λ ∂z
∂LA ∂LA ∂ ∂LA ∂ ∂LA
≈ |zk ,λk + ( )|zk ,λk δz + ( )|zk ,λk δλ
∂λ ∂λ ∂z ∂λ ∂λ ∂λ

The partial derivatives:

∂LA ∂Q ∂c̄ T ∂c̄ T ¯

= |zk ,λk + ( λk )|zk ,λk + ( Iµk c̄)|zk ,λk
∂z ∂z ∂z ∂z
∂LA
= c̄
∂λ
∂ ∂LA ∂ ∂LA T ∂c̄
( )= ( ) = |z ,λ
∂z ∂λ ∂λ ∂z ∂z k k
∂ ∂LA ∂2Q ∂c̄ T ¯ ∂c̄
( )= | z ,λ + ( Iuk )|zk ,λk
∂z ∂z ∂z 2 k k ∂z ∂z
∂ ∂LA
( ) = 0p̄xp̄
∂λ ∂λ

Forming the ... system:

" #
∂L2A ∂c̄ T
  ∂L 
δz − ∂zA
∂z 2 ∂z =
∂c̄
∂z 0p̄xp̄ δλ −c̄

16
 ∂L2A ∂c̄ T ∂LA
2
δz + δλ = −
∂z ∂z ∂z
∂c̄ T
δz = −c̄
∂z
−1
∂L2A ∂c̄
2
∂c̄ ∂LA
If ∂z 2 is invertible and ∂z is full rank then the system can be solved. Premultiply the first equation by ∂z ∂z 2 :
−1 −1 −1
∂c̄ ∂L2A ∂L2A ∂c̄ ∂L2A ∂c̄ T ∂c̄ ∂L2A ∂LA
δz + δλ = −
∂z ∂z 2 ∂z 2 ∂z ∂z 2 ∂z ∂z ∂z 2 ∂z
−1 T −1
∂c̄ ∂L2A ∂c̄ ∂c̄ ∂L2A ∂LA
−c̄ + δλ = −
∂z ∂z 2 ∂z ∂z ∂z 2 ∂z

−1 −1
∂c̄ ∂L2A ∂c̄ T −1 ∂c̄ ∂L2A ∂LA
δλ = ( ) (c̄ − )
∂z ∂z 2 ∂z ∂z ∂z 2 ∂z

This result can be used to update z:

−1
∂L2A ∂LA
δz = −
∂z 2 ∂z

Finally, the active-set second-order multiplier update is:

−1 −1
∂c̄ ∂L2A ∂c̄ T −1 ∂c̄ ∂L2A ∂LA
λk+1 = λk + ( ) (c̄ − )|zk ,uk
∂z ∂z 2 ∂z ∂z ∂z 2 ∂z

8 First Order Hold iLQR

This Differential Dynamic Programming formulation of the discrete optimal control problem includes Simpson
quadrature stage costs, first-order hold controls, infeasible controls, minimum time, and general nonlinear equal-
ity and inequality constraints to solve the following problem:

Z tf
min `f (x(tf )) + `(x, u)dt
u,tf 0
s.t. ẋ(t) = f (x(t), u(t)) + ui (t)
xmin ≤ x(t) ≤ xmax , 0 ≤ t ≤ tf
umin ≤ u(t) ≤ umax
h(x(t), u(t)) = 0
g(x(t), u(t)) ≤ 0
ui (t) = 0
tfmin ≤ tf ≤ tfmax
x(tf ) = xf
x(0) = x0

Converting to discrete time

N −1
X 1 4 1
min `N (xN ) + h2 ( `(xk , uk ) + `(xm , um ) + `(xk+1 , uk+1 )) + h2 Rm
ū1:N 6 6 6
k=1
s.t. xk+1 = fd (xk , uk , uk+1 ) + uik
xmin ≤ xk ≤ xmax , ∀k = 1, ..., N
umin ≤ uk ≤ umax

17
 hk (xk , uk ) = 0
gk (xk , uk ) ≤ 0
uik = 0, ∀k = 1, ..., N − 1
p p
dtmin ≤ hk ≤ dtmax
hk = hk+1 , ∀k = 1, ..., N − 2
xN = xf
x1 = x0

We form and solve the following Augmented Lagrangian:

N −1
1 X
L(x1:N , ū1:N , λ) = (xN − xf )T Qf (xN − xf ) + Lk (xk , uk , xk+1 , uk+1 )
2
k=1
N
X −1
+ Rm h2
k=1
N −1
X 1 T
+ u Ri uik
2 ik
k=1
N
1 X 1
+ c(xN , uN )T IµN c(xN , uN ) + λTN c(xN , uN ) + c(xk , uk , xk+1 , uk+1 )T Iµk c(xk , uk , xk+1 , uk+1 ) + λTk c(xk , uk , xk+1 , uk+1 )
2 2
k=1

We define:
h2
Lk (xk , uk , xk+1 , uk+1 ) = (`(xk , uk ) + 4`(xm , um ) + `(xk+1 , uk+1 ))
6
1 h2 h2
xm = (xk + xk+1 ) + ẋk − ẋk+1
2 8 8
uk + uk+1
um =
p 2
hk = dtk
 
uk
ūk =  hk 
uik
 
u
ûk = k
hk

With infeasible controls the dynamics become:

xk+1 = fd (xk , uk , uk+1 , hk ) + uik

Constraints are defined as:  

√k − umax
u
 (hk − dtmax )[k 6= N ] 
 
 √ umin − uk
 

 ( dtmin − hk )[k 6= N ] 
 
 xk − xmax 
c(xk , ūk , xk+1 , ūk+1 ) =  

 xmin − xk 


 cI (xk , uk ) 


 cE (xk , uk ) 

 uik [k 6= N ] 
(hk − hk+1 )[k < N − 1]

18
where the horizontal line indicates the separation between inequality (above) and equality (below) constraints.
Gradients  
0m̄×n

 0m̄×n 


 In 

∇xk c(xk , ūk , xk+1 , ūk+1 ) =   −In 

 ∇x cI (xk , uk ) 
 
∇x cE (xk , uk )
01×n
 
Im(m̄,m̄)[k6=N ] 0m̄×n
 −Im 0m̄×n 
 (m̄,m̄)[k6=N ] 

 0n×mm 

∇ūk c(xk , ūk , xk+1 , ūk+1 ) =  0n×mm 

 [∇ū cI (xk , uk ) 0] 
 
[∇ū cE (xk , uk ) 0] 
01×m 1[k < N − 1] 01×n
 
∇xk+1 c(xk , ūk , xk+1 , ūk+1 ) = 0p×n
 
0p−1×mm
∇ūk+1 c(xk , ūk , xk+1 , ūk+1 ) =
01×m −1[k < N − 1] 01×n

8.1 Notation
For convenience we define the following:

x = xk
u = uk
y = xk+1
v = uk+1

8.2 Backward pass

Normal
h2 4h2
Lx = `1x + `2x
6 6
h2 4h2
Lu = `1u + `2u
6 6
4h2 h2
Ly = `2y + `3y
6 6
4h2 h2
Lv = `2v + `3v
6 6
h2 4h2
Lxx = `1xx + `2xx
6 6
h2 4h2
Luu = `1uu + `2uu
6 6
4h2 h2
Lyy = `2yy + `3yy
6 6
4h2 h2
Lvv = `2vv + `3vv
6 6
4h2
Lxu = `2xu
6
4h2
Lxy = `2xy
6

19
 4h2
Lxv = `2xv
6
4h2
Luy = `2uy
6
4h2
Luv = `2uv
6
2
4h
Lyv = `2yv
6
Minimum time
Lx = Lx
 
Lu
Lû = 2h
6 `1 + L2h + 2h
6 `3 + 2Rm h
Ly = Ly
 
L
Lv̂ = v
0
Lxx = Lxx
( 2h
 
Luu 6 `1u + L2uh )
Lûû = 2h
( 6 `1u + L2hu )T ( 6 `1 + L2hh + 62 `3 + 2Rm )
2

Lyy = Lyy
 
L 0
Lv̂v̂ = vv
0 0
( 2h
 
Lxû = Lxu 6 `1x + L2xh )
Lxy = Lxy
 
Lxv̂ = Lxv 0
T
Lûy = Lyu ( 2h

6 `3y + L2yh )
T
( 2h

L 6 `3v + L2vh )
Lûv̂ = vu
0 0
 T
Lyv̂ = Lyv 0

2
xmh 7 = h(fc (x, u) − fc (y, v))
8
h2
xmu = Bu
8
h2
xmy = 0.5I − Ay
8
h2
xm v = Bb
8
`2h = xTmh Q(xm − xf )
2
`2hh = ((fc (x, u) − fc (y, v))T Q(xm − xf ) + h(fc (x, u) − fc (y, v))T Qxmh
8
4
L2h = (h2 `2h + 2h`2 )
6
4
L2hh = (2h`2h + 2`2 + h2 `2hh + 2h`2h )
6
4 2
L2hu = (2h`2u + h3 (BuT Q(xm − xf ) + xTmu Q(fc (x, u) − fc (y, v))))
6 8
4 h2 2
L2xh = (2h`2x + h2 (0.5I + ∗ (A)Tx Qxmh + hATx Q(xm − xf ))
6 8 8
2
4 h 2
L2uh = (2h`2u + h2 ( BuT Qxmh + hBu Q(xm − xf ))
6 8 8

20
 4 h2 2
L2yh = (2h`2y + h2 (− ATy Qxmh + hAy Q(xm − xf ))
6 8 8
4 h2 2
L2vh = (2h`2v − h2 ( BvT Qxmh + hBv Q(xm − xf ))
6 8 8
Infeasible
-Note: the bold variable is used to indicate that either u or û can be used if the problem is infeasible or minimum
time and infeasible, respectively

Lx = Lx
 
Lu
Lũ =
Ri ui
Ly = Ly
 
L
Lṽ = v
0
Lxx = Lxx
 
L 0
Lũũ = uu
0 Ri
Lyy = Lyy
 
L 0
Lṽṽ = vv
0 0
 
Lxũ = Lxu 0
Lxy = Lxy
 
Lxv = Lxv 0
 T
L y
Lũy = u
0
 T
Lvu 0
Lũṽ =
0 0
 T
Lyṽ = Lyv 0

8.3 Preliminaries
Similar to the setup for the regular iLQR problem (Section ??), we define a generic nonlinear cost function
Z tf
J = `N (xN ) + `(x, u)dt (40)
0

which we can approximate over N discrete time steps using Simpson integration:
N −1
X dt
J ≈ `N (xN ) + (`(xk , uk ) + 4`(xm , um ) + `(xk+1 , uk+1 ))
6
k=1
(41)
N
X −1
= `N (xN ) + L(xk , uk , xk+1 , uk+1 )
k=1

where xm , um are the controls at the midpoint of the interval [k, k + 1].
We choose to linearly interpolate the controls such that
uk + uk+1
um = (42)
2
and define a function g that such xm = g(xk , uk , xk+1 , uk+1 ), which will be derived later.

21
 We discretize the dynamics as before, but now include a dependence on the control at k + 1 to implement a first
order hold on the controls so that our dynamics are now xk+1 = fd (xk , uk , uk+1 ), with a first-order Taylor-series
approximation:

δxk+1 = A(xk , uk , uk+1 )δxk + B(xk , uk , uk+1 )δuk + C(xk , uk , uk+1 )δuk+1 (43)

8.4 Cost-To-Go
With a given cost function, we can apply Bellman’s Principle of Optimality to define the optimal cost-to-go Vk (x)
by the recurrence relation:

VN = `f (xN )
Vk = min {L(xk , uk , xk+1 , uk+1 ) + Vk+1 (fd (xk , uk , uk+1 ))} (44)
uk ,uk+1

However, the minimization is now over controls at time steps k and k + 1. Since the cost at the previous time step
k − 1 is dependent on the control uk , we cannot optimize over uk without taking into account the effect at k − 1. To
avoid this, we define a new control-dependent cost-to-go function:

P (xk , uk ) ≡ min L(xk , uk , xk+1 , uk+1 ) + Pk+1 (xk+1 , uk+1 )
uk+1
 (45)
= min Q(xk , uk , xk+1 , uk+1 )
uk+1

such that
Vk = min P (xk , uk ) (46)
uk

We now perform a second-order Taylor-series expansion on P to get:

 T    
1 δx δx δx
δP = S +s (47)
2 δu δu δu

where s ∈ Rn+m and S ∈ R(n+m)×(n+m) are the gradient and Hessian of δP , similar to their definitions for the
normal iLQR case, except that now they are functions of both the state and control and the current time step.

8.5 Terminal Cost-to-go

The terminal cost-to-go is found by taking the the second order Taylor-series approximation of the terminal cost
`N (xN ):
 T  ∂ 2 `   
1 δx 2
N
0  ∂`  δx
δPN = ∂x + ∂x 0 N
2 δu 0 0 δu
 T     (48)
1 δx δx δx
= SN + sN
2 δu δu δu

8.6 Solving the Dynamic Programming Problem

As before, we find the deviations of the cost-to-go by taking a Taylor-series expansion and plugging in our linearized
dynamics (and foregoing subscripts for the more succinct notation defined in Section 8.1) to get

δPk = min δL(δx, δu, δy, δv) + δP (δy, δv)
δv
= min δ Q̄(δx, δu, δy, δv) (49)
δv
= min δ Q̄(δx, δu, f (δx, δu, δv), δv) (50)
δv

Substituting in the dynamics produces:

 T     
δx Qxx Qxu Qxv δx  δx
1 
δQ(δx, δu, δv) = δu Qux Quu Quv  δu + Qx Qu Qv δu (51)
2
δv Qvx Qvu Qvv δv δv

22
The terms of the Q are composed of the elements of L(x, u, y, v) and P (y, v). These calculations and the substitution
of the dynamics are detailed in Sections to avoid clutter when deriving the dynamic programming step.

To calculate the optimal modification to the nominal control at the next time step δv ∗ , we take the derivative of δQ
with respect to δv and solve for the minimizing δv ∗ :
∂δQ
= Qv + Qvv δv + Qvx δx + Qvu δu
∂v
δv ∗ = −Q−1 (52)
vv (Qvx δx + Qvu δu + Qv )
≡ Kδx + bδu + d

It is important to note that the subscripts on the gains match the time index of the control deviation being calculated,
not the deviations by which they are multiplied (for more detail see Section 7.6)

By plugging in δv ∗ into Q we find our approximated cost-to-go which we can propagate backward one time step.
 T
Q∗xx Q∗xu δx
    
1 δx  δx
δP = δQ∗ = + Q∗x Q∗u

∗ ∗
2 δu Qux Quu δu δu
 T     (53)
∗ 1 δx δx δx
= δQ = S +s
2 δu δu δu

This completes the dynamic programming step. The equations to calculate S and s at the current time step are
derived in a following section.

8.6.1 Final time step of backwards recurssion

The recursive steps derived above are completed backwards, starting from the terminal state, as done for regular
iLQR. At the final step, however, we have a final optimization. Once we have calculated δP1 we have

δV1 = min δP (x1 , u1 ) (54)

δu1

from Equation 46. This minimization can now be carried out since u1 does not affect any previous time steps. This
problem is identical to that of iLQR:

δu∗1 = −Q−1
uu (Qux δx1 + Qx )
≡ K1 δx1 + d1

8.7 Calculating L
We now find a closed-form expression for δL(δx, δu, δy, δv) in Equation ??, which is the 2nd order approximation of
L(x, u, y, u)

8.7.1 Interpolation of states and controls

The state midpoint xm will be interpolated using a cubic polynomial. This choice is made because we are using RK3
integration (3rd order accuracy) which requires a cubic representation for the interpolation to be the same order of
accuracy [reference]. The control midpoint um will be linearly interpolated.

The state x(t) over each interval tk < t < tk+1 can be approximated using a cubic polynomial:

x(t) ≈ at3 + bt2 + ct + d

with the following boundary conditions:

x(0) = xk

23
 ẋ(0) = f (xk , uk )
x(dt) = xk+1
ẋ(dt) = f (xk+1 , uk+1 )

The coefficients enter into the system linearly and can be solved for as follows:

    
xk 0 0 0 1 a
 ẋk   0 0 1 0 b
xk+1  =  dt3
    
dt2 dt 1  c 
ẋk+1 3dt2 2dt 1 0 d

−2
   2 1 1
 
a dt3 dt2 dt3 dt2 xk
 b   −32 −2 3 −1  
ẋk 
  =  dt dt dt2 dt   
c  0 1 0 0  xk+1 
d 1 0 0 0 ẋk+1
To evaluate the midpoint xm :

xm = x(dt/2) = a(dt/2)3 + b(dt/2)2 + c(dt/2) + d

 
a
b
h 3 2
i 
= dt8 dt
4
dt
2 1 c
d
−2
 2 1 1
 
3 dt2 dt3 dt2 xk
h i  dt
−3 −2 3 −1  
ẋk 
= dt3 dt2 dt dt2 dt dt2 dt  
1  
8 4 2  0 1 0 0  xk+1 
1 0 0 0 ẋk+1

1 dt dt
xm = (xk + xk+1 ) + ẋk − ẋk+1
2 8 8
1 dt dt
= (xk + xk+1 ) + f (xk , uk ) − f (xk+1 , uk+1 )
2 8 8
= g(xk , uk , xk+1 , uk+1 )

The control midpoint is defined to be:

uk + uk+1
um =
2

8.7.2 Second order expansion of L(x, u, y, v)

(see section on Taylor Series expansions)
 T   
δxk Lxx Lxu Lxy Lxv δxk
1  δuk  Lux Luu Luy Luv 
  δuk 
 
δLk =   
2 δxk+1  Lyx Lyu Lyy Lyv   δxk+1 
δuk+1 Lvx Lvu Lvy Lvv δuk+1
  (55)
δxk
   δuk 
+ Lx Lu Ly Lv  
δxk+1 
δuk+1

We can consider the pieces that sum to form L(x, u, y, v) separately (i.e., `(x, u), `(xm , um ), `(y, v)). First, we
consider the second order expansion of `(x, u):

`(x + δx, u + δu) ≈ `(x, u)

24
 ∂` ∂`
+ |x,u δx + |x,u δu
∂x ∂u
1 ∂2` 1 ∂2`
+ δx 2 |x,u δx + δu 2 |x,u δu
2 ∂x 2 ∂u
∂`2 ∂`2
Note that for our quadratic stage cost there are no cross terms so ∂x∂u and ∂u∂x are both zero.
The terms of interest for our quadratic stage cost are:

∂`
|x,u = Q(x − xf )
∂x
∂`
|x,u = Ru
∂u
∂2`
|x,u =Q
∂x2
∂2`
|x,u =R
∂u2
The expansion for `(y, v) is defined identically, using y and v in the appropriate places.

`1x = Q(x − xf )
`1u = Ru
`1xx = Q
`1uu = R

`3y = Q(y − xf )
`3v = Rv
`3yy = Q
`3vv = R

The expansion of `(xm , um ) requires application of the matrix chain rule, our cubic interpolation for xm , linear
interpolation for um , and linear approximation of the continuous dynamics:

∂xm T ∂`
`2x =
∂x ∂xm
1 dt
= ( I + Ax )T Q(xm − xf )
2 8
T
∂xm ∂` ∂um T ∂`
`2u = +
∂u ∂xm ∂u ∂um
dt 1
= ( Bu )T Q(xm − xf ) + Rum
8 2
∂xm T ∂`
`2y =
∂y ∂xm
1 dt
= ( I − Ay )T Q(xm − xf )
2 8
∂xm T ∂` ∂um T ∂`
`2v = +
∂v ∂xm ∂v ∂um
dt 1
= (− Bv )T Q(xm − xf ) + Rum
8 2

∂xm T ∂ 2 ` ∂xm
`2xx =
∂x ∂x2m ∂x

25
 1 dt 1 dt
= ( I + Ax )T Q( I + Ax )
2 8 2 8
∂xm T ∂ 2 ` ∂xm ∂um T ∂ 2 ` ∂um
`2uu = +
∂u ∂x2m ∂u ∂u ∂u2m ∂u
dt dt 1 1
= ( Bu )T Q( Bu ) + R
8 8 2 2
∂xm T ∂ 2 ` ∂xm
`2yy =
∂y ∂x2m ∂y
1 dt 1 dt
= ( I − Ay )T Q( I − Ay )
2 8 2 8
∂xm T ∂ 2 ` ∂xm ∂um T ∂ 2 ` ∂um
`2vv = +
∂v ∂x2m ∂v ∂v ∂u2m ∂v
dt dt 1 1
= (− Bv )T Q(− Bv ) + R
8 8 2 2

∂xm T ∂ 2 ` ∂xm
`2xu =
∂x ∂x2m ∂u
1 dt dt
= ( I + Ax )T Q( Bu )
2 8 8
∂xm T ∂ 2 ` ∂xm
`2xy =
∂x ∂x2m ∂y
1 dt 1 dt
= ( I + Ax )T Q( I − Ay )
2 8 2 8
T 2
∂xm ∂ ` ∂xm
`2xv =
∂x ∂x2m ∂v
1 dt dt
= ( I + Ax )T Q(− Bv )
2 8 8
∂xm T ∂ 2 ` ∂xm
`2uy =
∂u ∂x2m ∂y
dt 1 dt
= ( Bu )T Q( I − Ay )
8 2 8
∂xm T ∂ 2 ` ∂xm ∂um T ∂ 2 ` ∂um
`2uv = +
∂u ∂x2m ∂v ∂u ∂u2m ∂v
dt dt 1 1
= ( Bu )T Q(− Bv ) + R
8 8 2 2
∂xm T ∂ 2 ` ∂xm
`2yv =
∂y ∂x2m ∂v
1 dt dt
= ( I − Ay )T Q(− Bv )
2 8 8

dt
Collect the expanded terms from `(x, u), `(xm , um ), `(y, v). Importantly, multiply by the correct scaling factor 6
4dt
or 6 :

dt 4dt 1 dt
Lx = Q(x − xf ) + ( I + Ax )T Q(xm − xf )
6 6 2 8
dt 4dt  dt T 1 
Lu = Ru + ( Bu ) Q(xm − xf ) + Rum
6 6 8 2
dt 4dt 1 dt
Ly = Q(y − xf ) + ( I − Ay )T Q(xm − xf )
6 6 2 8
dt 4dt  dt 1 
Lv = Rv + (− Bv )T Q(xm − xf ) + Rum
6 6 8 2

26
 dt 4dt 1 dt 1 dt
Lxx = Q+ ( I + Ax )T Q( I + Ax )
6 6 2 8 2 8
dt 4dt  dt T dt 1 1
Luu = R+ ( Bu ) Q( Bu ) + R
6 6 8 8 2 2
dt 4dt 1 dt T 1 dt
Lyy = Q+ ( I − Ay ) Q( I − Ay )
6 6 2 8 2 8
dt 4dt  dt T dt 1 1
Lvv = R+ (− Bv ) Q(− Bv ) + R
6 6 8 8 2 2

4dt 1 dt dt
Lxu = ( I + Ax )T Q( Bu )
6 2 8 8
4dt 1 dt T 1 dt
Lxy = ( I + Ax ) Q( I − Ay )
6 2 8 2 8
4dt 1 dt T dt
Lxv = ( I + Ax ) Q(− Bv )
6 2 8 8
4dt dt 1 dt
Luy = ( Bu )T Q( I − Ay )
6 8 2 8
4dt  dt dt 1 1
Luv = Bu )T Q(− Bv ) + R
6 8 8 2 2
4dt 1 dt dt
Lyv = ( I − Ay )T Q(− Bv )
6 2 8 8

8.7.3 Substituting in the Dynamics

Prior to taking the summation of the stage cost δL and the cost-to-go δP to form δQ(δx, δu, δv) = δL + δP we will
substitute in dynamics
First, we substitute the linearized discrete dynamics into δP (δy, δv) → δP (δx, δu, δv):

 T     
1 δy Syy Syv δy   δy
δP (δy, δv) = + Sy Sv (56)
2 δv Svy Svv δv δv
 T   
1 Aδx + Bδu + Cδv Syy Syv Aδx + Bδu + Cδv
δP (δx, δu, δv) = (57)
2 δv Svy Svv δv
 
  Aδx + Bδu + Cδv
+ Sy Sv
δv
 T     
δx Gxx Gxu Gxv δx δx
1  
= δu Gux Guu Guv  δu G + Gx Gy Gv δu (58)
2
δv Gvx Gvu Gvv δv δv

Gxx = AT Syy A
Guu = B T Syy B
Gvv = C T Syy C + C T Syv + Svy C + Svv (59)
Gxu = AT Syy B
Gxv = AT Syy C + AT Syv
Guv = B T Syy C + B T Syv

Gx = AT Sy
Gy = B T Sy (60)
T
Gv = C Sy + Sv

27
Similarly, we substitute the linearized discrete dynamics into δL(δx, δu, δy, δv) → δL(δx, δu, δv):

 T   
δx Lxx Lxu Lxy LLxv δx
1 δu  Lux Luu Luy LLuv   δu 
δLk =      (61)
2 Aδx + Bδu + Cδv  Lyx Lyu Lyy LLyv  Aδx + Bδu + Cδv 
δv Lvx Lvu Lvy LLvv δv
 
δx
  δu 
+ Lx Lu Ly Lv  
Aδx + Bδu + Cδv 
δv
This simplifies to: (62)
 T     
δx H Hxu Hxv δx  δx
1  xx 
δLk = δu Hux Huu Huv  δu + Hx Hy Hv δu
2
δv Hvx Hvu Hvv δv δv
where: (63)

Hxx = Lxx + Lxy A + AT Lyx + AT Lyy A

Huu = Luu + Luy B + B T Lyu + B T Lyy B
Hvv = Lvv + Lvy C + C T Lyv + C T Lyy C (64)
T T
Hxu = Lxu + Lxy B + A Lyu + A Lyy B
Hxv = Lxv + Lxy C + AT Lyv + AT Lyy C
Huv = Luv + Luy C + B T Lyv + B T Lyy C

Hx = Lx + AT Ly
Hu = Lu + B T Ly (65)
T
Hv = Lv + C Ly

This yields
T 
    
δx Qxx Qxu Qxv δx  δx
1 
δQ(δx, δu, δv) = δu Qux Quu Quv  δu + Qx Qu Qv δu (66)
2
δv Qvx Qvu Qvv δv δv
where:

Qxx = Gxx + Hxx

Quu = Guu + Huu
Qvv = Gvv + Hvv (67)
Qxu = Gxu + Hxu
Qxv = Gxv + Hxv
Quv = Guv + Huv

Qx = Gx + Hx
Qu = Gu + Hu (68)
Qv = Gv + Hv

28
8.8 Calculating S and s
With the gains obtained by optimizing over the control at the next timestep, we are ready to calculate the cost-to-go
Hessian and gradient at the current time step:
 T     
δx Qxx Qxu Qxv δx δx
1  
δP (x, u) = δu  Qux Quu Quv   δu  + Qx Qu Qv  δu 
2
Kδx + bδu + d Qvx Qvu Qvv Kδx + bδu + d Kδx + bδu + d
= Qx δx + Qu δu + (Qv Kδx + Qv bδu + Qv d)
1
+ (δxT Qxx δx + δuT Quu δu)
2
1
+ (δxT Qxu δu + δuT Qux δx)
2

1
+ (δxT Qxv Kδx + δxT Qxv bδu + δxT Qxv d)
2
1
+ (δuT Quv Kδx + δuT Quv bδu + δuT Quv d)
2
1
+ (δxT K T Qvx δx + δuT bT Qvx δx + dT Qvx δx)
2
1
+ (δxT K T Qvu δu + δuT bT Qvu δu + dT Qvx δu)
2

1
+ (δxT K T Qvv Kδx + δxT K T Qvv bδu + δxT K T Qvv d)
2
1
+ (δuT bT Qvv Kδx + δuT bT Qvv bδu + δuT bT Qvv d)
2
1
+ (dT Qvv Kδx + dT Qvv bδu + dT Qvv d)
2

1 δx Q∗xx Q∗xu δx
     
 δx
+ Q∗x Q∗u

= ∗ ∗
2 δu Qux Quu δu δu
 T    
1 δx δx δx
= Sk + sk
2 δu δu δu

Qxx ∗ = Qxx + Qxv K + K T Qvx + K T Qvv K

Quu ∗ = Quu + Qvu b + bT Qvu + bT Qvv b
Qux ∗ = Qux + Quv K + bT Qvx + bT Qvv K

Q∗x = Qx + Qv K + dT Qvx + dT Qvv K

Q∗u = Qu + Qv b + DT Qvu + dT Qvv b

1
∆Q = Qv d + dT Qvv d
2

8.9 Forward Pass

8.9.1 Initial Rollout
We begin the first rollout with a set of controls, U ∈ Rm×N and an initial state, x0 . To calculate the state at the
next time step we use the discrete dynamics with a first-order hold on the control:

xk+1 = fd (xk , uk , uk+1 ), for k = 1, . . . , N − 1 (69)

29
8.9.2 Rollout with Gains
We assume we have a control U and a trajectory X from the previous iteration, along with the gains K ∈ Rm×n×N , b ∈
Rm×m×N and d ∈ Rm×N . (Note: α ∈ [0, 1] is the line search paramter)
Prior to iterating over the time steps we calculate the deviation of the first control:

δu1 = K1 δx1 + αd1

= αd1 (70)
ū1 = u1 + δu1

since δx1 = 0.

We can then perform the rollout as follows for k = 1, . . . , N − 1:

δxk = x̄k − xk
δuk+1 = Kk+1 δxk + bk+1 δuk + αdk+1
ūk+1 = uk+1 + δuk+1
x̄k+1 = fd (x̄k , ūk , ūk+1 )

8.10 Regularization
As before, there are two types of regularization that can be employed: control and state:
Option 1:

Q̃vv = Qvv + ρI

K = −Q̃−1
vv Qvx

b = −Q̃−1
vv Qvu

d = −Q̃−1
vv Qv

Q∗xx = Qxx + Qxv K + K T Qvx + K T Qvv K

Q∗uu = Quu + Qvu b + bT Qvu + bT Qvv b
Q∗ux = Qux + Quv K + bT Qvx + bT Qvv K

Q∗x = Qx + K T Qv + Qxv d + K T Qvv d

Q∗u = Qu + bT Qv + Quv d + bT Qvv d

1
∆Q = Qv d + dT Qvv d
2
Option 2:

Q̃vv = Lvv + LTyv C + C T Lyv + C T Lyy C + C T (Syy + ρI)C + C T Syv + Syv

T
C + Svv
Q̃xv = Lxv + Lxy C + AT Lyv + AT Lyy C + AT (Syy + ρI)C + AT Syv
Q̃uv = Luv + Luy C + B T Lyv + B T Lyy C + B T (Syy + ρI)C + B T Syv

K = −Q̃−1
vv Q̃vx

b = −Q̃−1
vv Q̃vu

d = −Q̃−1
vv Qv

30
 Q∗xx = Qxx + Qxv K + K T Qvx + K T Qvv K
Q∗uu = Quu + Qvu b + bT Qvu + bT Qvv b
Q∗ux = Qux + Quv K + bT Qvx + bT Qvv K

Q∗x = Qx + K T Qv + Qxv d + K T Qvv d

Q∗u = Qu + bT Qv + Quv d + bT Qvv d

1
∆Q = Qv d + dT Qvv d
2
For both regularization types, when timestep k = 1:
Q̃∗uu = Q∗uu + ρI

K1 = −(Q̃∗uu )−1 Q∗xu

b1 = 0
d1 = −(Q̃∗uu )−1 Q∗u

8.11 Constraints (Augmented Lagrange Method)

Previously, we defined the constraint stage cost as:

1
g(xk , uk ) = ck (xk , uk )T Iµk ck (xk , uk ) + λTk ck (xk , uk )
2
While we are using Simpson integration to approximate the state and control stage cost, here we use trapezoidal
integration (which is not dependent on the midpoint state).
LA (X, U ; µ, λ) = `f (xN )
1
+ cN (xN )T IµN cN (xN ) + λTN cN (xN )
2
N −1
X dt  
+ `(xk , uk ) + 4`(xm , um ) + `(xk+1 , uk+1 )
6
k=1
N
X
+ g(xk , uk )
k=1

Importantly, we must modify the backward pass. We defined δ Q̄(x, u, y, v) for the first order holdand will modify
it in a similar manner to the Section 3.2:
ˆ = Q̄ + cT I c + cT λ
Q̄ x x x µ x
ˆ
Q̄u = Q̄u + cu Iµ c + cTu λ
T

ˆ = Q̄ + cT I c
Q̄ xx xx x µ x
ˆ
Q̄uu = Q̄uu + cTu Iµ cu

ˆ = Q̄ + cT I c
Q̄xu xu x µ u

The boundary conditions are augmented as well, similar to Section 3.2, they fill the P Hessian and gradient.

ŝx = sx + (cTxN IµN cN + cTxN λN )

31
 ŝu = su + (cTuN IµN cN + cTuN λN )

Ŝxx = Sxx + (cTxN IµN cxN ) + (cTx Iµ cx )

Ŝuu = (cTu Iµ cu )

Ŝxu = (cTx Iµ cu )
Ŝux = (cTu Iµ cx )

9 Minimum Time: NOTE: THIS FORMULATION IS NOT UP TO

DATE
It is often desirable to minimize the time required to reach the goal state. Minimum-time policies very often results
in “bang-bang” control policies, where the control is saturated at the limits. It is therefore imperative to specify
control bounds when solving minimum time problems. Here we set forth the necessary modifications to the iLQR
algorithm to compute minimum-time policies.

9.1 Notation
As will be shown, it is natural to treat the time step dt as an additional control. We then define the following:
1. u ∈ Rm = vector of controls for the original problem
2. û ∈ Rm̂ = controls augmented with time
In general, any value related to the controls with a hat will denote values augmented with the time step.

9.2 General Methodology

In order to minimize over time, we must make time a decision variable. It is very important that all decision variables
are only locally coupled between time-steps in order to leverage the sparsity structure of the Hessian and solve the
problem using dynamic programming. Therefore, rather than have a single decision variable represent the total time
or even the time step, we assign a separate time step variable to each time step: dtk = the time between time step k
and k + 1, i.e. dtk = tk+1 − tk . We then constrain each of these values to be positive and less than some maximum
threshold: 0 < dtk < dtmax .
We also want the time steps to be equal, since we want to avoid having the algorithm exploit the time step to
artificially lower the cost. We then impose N − 2 constraints: dtk = dtk+1 , k = 1, . . . , N − 2 to set the time steps
equal.
It is also imperative that we never allow the time steps to become negative when plugged into the dynamics,
since this effectives “reverses” time and will result in strange behavior. To ensure dt is strictly positive we store the
square root of the time step, which is then squared whenever plugged into dynamics, ensuring strict positivity. This
these high-level insights and design decision, we formulate the problem.

9.3 Problem Formulation

9.3.1 Cost Function
We first set up the cost function, or Lagrangian since we are solving a constrained problem:
N
X −1
J = `(xN ) + (`(xk , uk ) + cdt)dt
k=1
√
we now define h := dt so that
N
X −1
J = `(xN ) + (`(xk , uk ) + ch2 )h2
k=1

32
9.3.2 Optimization Problem
Using the previous cost function we now formulate the optimization problem:
N
X −1
min `(xN ) + (`(xk , uk ) + ch2 )h2
uk ,hk ∀k
k=1
subject to xlb ≤ xk ≤ xub , ∀k
ulb ≤ uk ≤ uub , ∀k
xN = xf
p
0 ≤ hk ≤ dtmax , ∀k
hk = hk+1 , ∀k

Note that enforcing equality between hk , hk+1 and dtk , dtk+1 are equivalent, so we avoid unnecessary squares in the
constraints by simply enforcing equality on h.

9.3.3 Augmented Lagrangian

We use the Augmented Lagrangian method to convert the problem to an unconstrained optimization problem. As
mentioned previously, we can conveniently treat hk as an additional control and form ûk = [uTk hk ]T . The Lagrangian
is formed as usual, except this time we special case the equality constraint on the time steps:
N −1
X 1 1
LA = `(xN ) + c(xN )T Iµ,N c(xN ) + λTN c(xN ) + (`(xk , uk ) + ch2 )h2 + c(xk , ûk )T Iµ,k c(xk , ûk ) + λTk c(xk , ûk ) +
 
2 2
k=1
N −2
X 1 t
µ (hk − hk+1 )2 + λtk (hk − hk+1 )
2 k
k=1

which turns our problem into:

min LA
uk ,hk ,λk ,λtk ∀k

We define c(xk , ûk ) in the next section.

9.3.4 Discrete Dynamics

It is also necessary to account for the impact of the changing dt on the dynamics. In general, the discrete dynamics
are a function of the state, control, and time step:

xk+1 = f (xk , uk , dtk ) = f (xk , uk , h2k )

So when taking the first-order Taylor Series expansion we also take the expansion about hk :

∂f ∂f ∂f
δxk+1 = δxk + δuk + δhk
∂x xk ,uk ,hk ∂u xk ,uk ,uk ∂h xk ,uk ,hk
= A(xk , uk , hk )δxk + B(xk , uk , hk )δuk + H(xk , uk , hk )δhk
= A(xk , ûk )δxk + B̂(xk , ûk )
 
where B̂(xk , ûk ) = B(xk , uk , hk ) H(xk , uk , hk .

9.4 Backwards Pass

With these changes, we just treat the time step as an additional control, so nothing changes about the derivation
of the Dynamic Programming steps to calculate the closed-loop gains during the backwards pass. However, since
we now have a different Lagrangian, we need to be careful to correctly the specify the expansion for the augmented
controls.

33
9.4.1 Terminal Cost-to-Go
Nothing changes here, since the Lagrangian at the terminal state is not a function of dt. So,
∂`N
sN = + ∇x c(xN )T (Iµ,N )c(xN ) + ∇Tx λN
∂x
∂ 2 `N
SN = + ∇x c(xN )T (Iµ,N )∇x c(xN )
∂x2

9.4.2 Dynamic Programming Step

Similar to before, our goal is to find the cost-to-go at step k:
δVk = min {δQk (xk , uk , hk )}
uk ,hk

where
δQ = Qx δx + Qu δu + Qh δh+
δxT Qxx δx + δuT Quu δu + δhT Qhh δh+
δxT Qxu δu + δxT Qxh δh + δuT Quh δh
and Qxx = Lxx + ∇xx Vk+1 (f (xk , uk , hk )) and so forth for the other terms of the expansion.

We re-state the Lagrangian here for reference, prior to defining the partials:
N −1
X 1 1
LA = `(xN ) + c(xN )T Iµ,N c(xN ) + λTN c(xN ) + (`(xk , uk ) + ch2 )h2 + c(xk , ûk )T Iµ,k c(xk , ûk ) + λTk c(xk , ûk ) +
 
2 2
k=1
N −2
X 1 t
µ (hk − hk+1 )2 + λtk (hk − hk+1 )
2 k
k=1

Taking the expansion of the Lagrangian and the cost-to-go we get the following terms. To simplify the notation,
any variable without a subscript is assumed to at the current time step k, and terms with a superscript (+) or (-)
are the values at the next (k + 1) or previous (k − 1) time step.
Lx = `x h2 + cTx Iµc + cTx λ
Lu = `u h2 + cTu Iµc + cTu λ
Lh = 2`h + 2ch3 + cTh Iµc + cTh λ + µt (h − h+ ) + λt − µt (h− − h) − λ−
t

Lxx = `xx h2 + cTx Iµcx

Luu = `uu h2 + cTu Iµcu
Lhh = 2` + 6ch2 + cTh Iµch + µt + µ−
t

Lxu = `xu h2 + cTx Iµcu

Lxh = 2`x h + cTx Iµch
Luh = 2`u h + cTu Iµch
by augmenting the control with dt we get the following

Lx = `x h2 + cTx Iµc + cTx λ

Lû = `u h2 + cTû Iµc + cTû λ + 0 2`h + 2ch3 + µt (h − h+ ) + λt − µt (h− − h) − λ−
 
t

Lxx = `xx h2 + cTx Iµcx

 
2 0 2h`u
Lûû = `ûû h + cTu Iµcu +
2h`Tu 2` + 6ch2 + µt + µ−
t

Lxû = `xû h2 + cTx Iµcû + 0 2h`x

 

34
and
Qx = Lx + s+ A
Qû = Lû + s+ B̂
Qxx = Lxx + AT S + A
Qûû = Lûû + B̂ T S + B̂
Qxû = Lxû + AT s+ B̂
The gains can then be readily calculated used an identical procedure as in the non-minimum time case by using these
augmented partials of Q.

9.5 Forward Pass

The forward pass is identical, just make sure to extract out the updated h and plug into the dynamics, i.e.

δx = x̄k − xk
ūk = uk + Kk δxk + αdk
h̄k = ūk [m̄]
x̄k+1 = f (x̄k , ūk [1 : m], h̄)

where the square brackets are vector indexing operations (similar to syntax used in Julia or Python). Constraints:
In the previous section we used c(xk , ûk ) ∈ Rp , which is equivalent to c(xk , uk , hk ), which we define as:
 
u −√umax  

 h − dtmax 
 û − ûmax

 umin − u 


 ûmin − û 


 −h 


 x − x max


c(x, u, h) = 
 x − x max
 = c(x, û) = 
  x min − x 


 x min − x 


 c I (x, u) 


 cI (x, u) 

 c E (x, u) 
 cE (x, u)  (hk − hk+1 )[k < N − 1]
(hk − hk+1 )[k < N − 1]

where the horizontal line indicates the separation between inequality (above) and equality (below) constraints.
We now write down the gradients of this equation, since they will be used when re-deriving the backwards pass later:
 
0m̂×n
 0m̂×n 
 

 In 

∇x c(x, û) =   −In 

 ∇x cI (x, u) 
 
∇x cE (x, u)
01×n
 
Im

 −Im 


 0n̂×m̂ 

∇û c(x, û) = 
 0n̂×m̂ 

 [∇u cI (x, u) 0] 
 
[∇u cE (x, u) 0]
1[k < N − 1]

35