DDP

    Differential Dynamic Programming
PDDP

    Parameterized Differential Dynamic Programming
STO

    switching time optimization
MLE

    maximum likelihood estimation
iLQR

    iterative Linear-Quadratic Regulator
MHE

    moving horizon estimation
MPC

    model predictive control
MLE

    maximum likelihood estimation
UAM

    urban air mobility
VTOL

    vertical takeoff and landing
RVLT

    Revolutionary Vertical Lift Technology
STO-DDP

    Switching Time Optimization Differential Dynamic Programming

# Parameterized Differential Dynamic Programming

Alex Oshin123, Matthew D. Houghton2, Michael J. Acheson2, Irene M. Gregory2
and Evangelos A. Theodorou1  1School of Aerospace Engineering, Georgia
Institute of Technology, Atlanta, GA  2NASA Langley Research Center, Hampton,
VA  3Correspondence to<EMAIL_ADDRESS>
###### Abstract

Differential Dynamic Programming (DDP) is an efficient trajectory optimization
algorithm relying on second-order approximations of a system’s dynamics and
cost function, and has recently been applied to optimize systems with time-
invariant parameters. Prior works include system parameter estimation and
identifying the optimal switching time between modes of hybrid dynamical
systems. This paper generalizes previous work by proposing a general
parameterized optimal control objective and deriving a parametric version of
DDP, titled Parameterized Differential Dynamic Programming (PDDP). A rigorous
convergence analysis of the algorithm is provided, and PDDP is shown to
converge to a minimum of the cost regardless of initialization. The effects of
varying the optimization to more effectively escape local minima are analyzed.
Experiments are presented applying PDDP on multiple robotics systems to solve
model predictive control (MPC) and moving horizon estimation (MHE) tasks
simultaneously. Finally, PDDP is used to determine the optimal transition
point between flight regimes of a complex urban air mobility (UAM) class
vehicle exhibiting multiple phases of flight.

##  I Introduction

Classically, optimal control research has focused on the study of methods for
trajectory optimization of underactuated nonlinear systems. This attention is
due to the fact that underactuated systems are more difficult to control due
to having more degrees of freedom relative to the number of controls available
[25]. However, the emerging urban air mobility (UAM) sector allows the
opportunity to study the application of optimal control methods to
overactuated aircraft whose complexity appears in the nonlinear transition
dynamics between flight phases [12, 11]. These UAM class vehicles commonly
exhibit three phases of flight — including vertical takeoff and landing
(VTOL), fixed-wing cruise, and a complex transition phase between the two —
and can thus be classified as hybrid systems [6, 9, 13], transitioning between
multiple modes of the dynamics during a flight. While many algorithms have
been developed for hybrid systems trajectory optimization in the past, most
adopt a linear or reduced-order model, with the focus on bipedal or
quadrupedal robotics systems, e.g. [22, 3, 7]. The lack of applications to UAM
class vehicles reveals an opportunity to develop hybrid systems optimization
techniques for unique aircraft dynamics which have not been previously
studied.

Past research has shown Differential Dynamic Programming (DDP) is an effective
algorithm for planning in high-dimensional state spaces [28], and DDP has
shown recent success planning trajectories for UAM vehicles [14]. DDP is a
shooting method that achieves computational efficiency using second-order
approximations along a nominal trajectory and admits quadratic convergence
properties under mild assumptions [21, 15]. DDP is beneficial in that it
requires no reduction in the complexity of the dynamics model, and its
convergence properties make it a solid candidate for solving realtime model
predictive control (MPC) tasks [27].

Figure 1: Rendering of the NASA Lift+Cruise aircraft.

Recent work has shown the value function derivatives produced by DDP can be
used to update the initial conditions of the nominal trajectory, which allows
optimization over time-invariant parameters [16, 17]. While this update
corresponds to a Newton step on the parameters, modifying the initial
condition of DDP significantly affects the convergence of the algorithm since
the change induces a large shift of the nominal trajectory in the state space.
To remedy this issue, this work introduces a general parameterized optimal
control objective. Explicitly introducing the parameters into the system
dynamics and cost function allows a second-order algorithm to be derived for
iteratively updating both the controls and parameters simultaneously,
independent of the form of the parameterization. This parameterized version of
DDP is referred to as Parameterized Differential Dynamic Programming (PDDP),
and a rigorous convergence analysis is provided showing the derived algorithm
converges to a minimum of the cost regardless of state or parameter
initialization. Further, the effects of the simultaneous optimization of
parameters and controls is studied. An optimization scheme is proposed that
more effectively escapes local minima, which is a common issue of local
methods such as DDP. The PDDP algorithm is applied to two important robotics
tasks, model parameter estimation and hybrid systems optimization, and is used
to identify the optimal transition points between flight regimes for the NASA
Lift+Cruise UAM class vehicle developed under NASA’s Revolutionary Vertical
Lift Technology (RVLT) project [24]. A rendering of the Lift+Cruise aircraft
is provided in Fig. 1.

The contributions of this work are as follows:

  1. 1.

Generalize previous work by deriving a parameterized form of DDP, referred to
as PDDP.

  2. 2.

Provide theoretical analysis of the convergence behavior of the proposed PDDP
algorithm and show it is globally convergent to a minimum of the cost for
optimization problems with arbitrary dynamics and cost function parameters.

  3. 3.

Discuss and analyze three optimization choices for iteratively solving for the
optimal controls and parameters.

  4. 4.

Apply the proposed method on multiple robotics systems, including a cartpole,
a quadrotor, and an ant quadruped system using the open-source physics engine
Brax [10], and show PDDP is able to solve adaptive MPC tasks using moving
horizon estimation (MHE).

  5. 5.

Apply PDDP to find the optimal transition point between multi-modal dynamical
systems, including a UAM class vehicle exhibiting multiple phases of flight.

The paper is organized as follows. Section II derives the PDDP algorithm and
proves the convergence of the method. Section III discusses applications of
PDDP to system parameter estimation and switching time optimization for hybrid
systems. Section IV analyzes experimental results applying PDDP to the
previously discussed tasks. The paper concludes with Section V by discussing
future work.

##  II Parameterized Differential Dynamic Programming

In this section, the parameterized version of DDP is derived. While the
similarities are highlighted here, a full derivation of standard DDP can be
found in recent works such as [28].

###  II-A Problem Formulation

This work considers parameterized discrete-time dynamics that evolve according
to

$\displaystyle\mathbf{x}_{t+1}$
$\displaystyle=\mathbf{F}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta}),$
(1)

where
$\mathbf{F}:\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{u}}\times\mathbb{R}^{n_{\theta}}\to\mathbb{R}^{n_{x}}$
with $\mathbf{x}_{t}\in\mathbb{R}^{n_{x}}$,
$\mathbf{u}_{t}\in\mathbb{R}^{n_{u}}$, and
$\boldsymbol{\theta}\in\mathbb{R}^{n_{\theta}}$, and
$\mathbf{x}_{1}\in\mathbb{R}^{n_{x}}$ denoting the initial condition. The
semicolon emphasizes that the parameters are time-invariant and separate from
states or controls that are time-varying. The same assumptions made in
standard DDP [15] are adopted here. Namely, the dynamics are assumed to be
twice differentiable with respect to state and control, with the additional
assumption that the dynamics are twice differentiable with respect to the
parameters.

Defining the control sequence
$\mathbf{U}\vcentcolon=\\{\mathbf{u}_{1},\ldots,\mathbf{u}_{T}\\}$, the
parameterized discrete-time optimal control problem is given by

$\min_{\mathbf{U},\boldsymbol{\theta}}\mathcal{J}(\mathbf{U};\boldsymbol{\theta})=\min_{\boldsymbol{\theta}}\min_{\mathbf{U}}\sum_{t=1}^{T}\mathcal{L}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})+\phi(\mathbf{x}_{T+1};\boldsymbol{\theta}),$
(2)

where $\mathcal{L}$ and $\phi$ are the twice differentiable running cost and
terminal cost function, respectively, and $T$ is the time horizon. In the most
general case, the parameters $\boldsymbol{\theta}$ affect both the dynamics
and the cost.

The inner objective of Eq. 2 with $\boldsymbol{\theta}$ fixed is equivalent to
the standard DDP optimal control problem. The value function, now
parameterized by $\boldsymbol{\theta}$, is given recursively by

$\displaystyle V(\mathbf{x}_{t};\boldsymbol{\theta})$
$\displaystyle=\min_{\mathbf{u}_{t}}\Big{[}\underbrace{\mathcal{L}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})+V(\mathbf{x}_{t+1};\boldsymbol{\theta})}_{\vcentcolon=Q(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})}\Big{]},$
(3)

with
$V(\mathbf{x}_{T+1};\boldsymbol{\theta})=\phi(\mathbf{x}_{T+1};\boldsymbol{\theta})$.
Thus, the optimal $\boldsymbol{\theta}$ is given at the initial time $t=1$ by

$\displaystyle\boldsymbol{\theta}^{*}$
$\displaystyle=\operatorname*{argmin}_{\boldsymbol{\theta}}V(\mathbf{x}_{1};\boldsymbol{\theta}).$
(4)

###  II-B Algorithm Derivation

Given the parameterized dynamics defined by Eq. 1, this subsection will derive
an iterative algorithm for finding the control trajectory $\mathbf{U}$ and
parameters $\boldsymbol{\theta}$ that minimize the cost function defined in
Eq. 2.

As in standard DDP, the parameterized optimal control problem is solved by
considering quadratic approximations of the value function along a nominal
trajectory $\bar{\mathbf{x}}_{t}$, $\bar{\mathbf{u}}_{t}$. However, this
derivation also includes terms expanding the value function about a set of
nominal parameters $\bar{\boldsymbol{\theta}}$. Let
$\delta\mathbf{x}_{t},\delta\mathbf{u}_{t},\delta\boldsymbol{\theta}$ be the
variation in state, control, and parameters, respectively, so that

$\displaystyle\mathbf{x}_{t}\vcentcolon=\bar{\mathbf{x}}_{t}+\delta\mathbf{x}_{t},$
$\displaystyle\mathbf{u}_{t}\vcentcolon=\bar{\mathbf{u}}_{t}+\delta\mathbf{u}_{t},$
$\displaystyle\boldsymbol{\theta}\vcentcolon=\bar{\boldsymbol{\theta}}+\delta\boldsymbol{\theta}.$
(5)

The quadratic expansion of the value function about
$\bar{\mathbf{x}}_{t},\bar{\boldsymbol{\theta}}$ has the form

$\displaystyle\begin{aligned}
V(\mathbf{x}_{t};\boldsymbol{\theta})\approx{}&V^{0}_{t}+(V^{x}_{t})^{\top}\delta\mathbf{x}_{t}+(V^{\theta}_{t})^{\top}\delta\boldsymbol{\theta}\\\
&+\frac{1}{2}\begin{bmatrix}\delta\mathbf{x}_{t}\\\
\delta\boldsymbol{\theta}\end{bmatrix}^{\top}\begin{bmatrix}V^{xx}_{t}&V^{x\theta}_{t}\\\
V^{\theta
x}_{t}&V^{\theta\theta}_{t}\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}_{t}\\\
\delta\boldsymbol{\theta}\end{bmatrix},\end{aligned}$ (6)

where the partial derivatives of $V$ are denoted using superscripts and are
evaluated at $\bar{\mathbf{x}}_{t},\bar{\boldsymbol{\theta}}$, e.g.,
$V^{0}_{t}=V(\bar{\mathbf{x}}_{t};\bar{\boldsymbol{\theta}})$,
$V^{x}_{t}=\nabla_{x}V(\bar{\mathbf{x}}_{t};\bar{\boldsymbol{\theta}})$, etc.
This superscript notation will be adopted throughout the paper. In order to
solve for the partial derivatives of $V$, the quadratic expansion of the $Q$
function defined in Eq. 3 is taken about $\bar{\mathbf{x}}_{t}$,
$\bar{\mathbf{u}}_{t},\bar{\boldsymbol{\theta}}$:

$\displaystyle\begin{aligned}
Q(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})\approx{}&Q^{0}_{t}+(Q^{x}_{t})^{\top}\delta\mathbf{x}_{t}+(Q^{u}_{t})^{\top}\delta\mathbf{u}_{t}+(Q^{\theta}_{t})^{\top}\delta\boldsymbol{\theta}\\\
&+\frac{1}{2}\begin{bmatrix}\delta\mathbf{x}_{t}\\\ \delta\mathbf{u}_{t}\\\
\delta\boldsymbol{\theta}\end{bmatrix}^{\top}\begin{bmatrix}Q^{xx}_{t}&Q^{xu}_{t}&Q^{x\theta}_{t}\\\
Q^{ux}_{t}&Q^{uu}_{t}&Q^{u\theta}_{t}\\\ Q^{\theta x}_{t}&Q^{\theta
u}_{t}&Q^{\theta\theta}_{t}\end{bmatrix}\begin{bmatrix}\delta\mathbf{x}_{t}\\\
\delta\mathbf{u}_{t}\\\ \delta\boldsymbol{\theta}\end{bmatrix},\end{aligned}$
(7)

where

$\begin{split}Q^{0}_{t}&=\mathcal{L}^{0}_{t}+V^{0}_{t+1},\\\
Q^{x}_{t}&=\mathcal{L}^{x}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{x}_{t+1},\\\
Q^{u}_{t}&=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{x}_{t+1},\\\
Q^{\theta}_{t}&=\mathcal{L}^{\theta}_{t}+V^{\theta}_{t+1}+(\mathbf{F}^{\theta}_{t})^{\top}V^{x}_{t+1},\\\
Q^{xx}_{t}&=\mathcal{L}^{xx}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{x}_{t},\\\
Q^{xu}_{t}&=\mathcal{L}^{xu}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{u}_{t}=(Q^{ux}_{t})^{\top},\\\
Q^{x\theta}_{t}&=\mathcal{L}^{x\theta}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{x\theta}_{t+1}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}=(Q^{\theta
x}_{t})^{\top},\\\
Q^{uu}_{t}&=\mathcal{L}^{uu}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{u}_{t},\\\
Q^{u\theta}_{t}&=\mathcal{L}^{u\theta}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{x\theta}_{t+1}+(\mathbf{F}^{u}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}=(Q^{\theta
u}_{t})^{\top},\\\
Q^{\theta\theta}_{t}&=\mathcal{L}^{\theta\theta}_{t}+V^{\theta\theta}_{t+1}+2(\mathbf{F}^{\theta}_{t})^{\top}V^{x\theta}_{t+1}+(\mathbf{F}^{\theta}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}.\end{split}$
(8)

Note the partial derivatives with respect to $\boldsymbol{\theta}$ include
extra terms because both the value function and the dynamics depend on
$\boldsymbol{\theta}$. In Eq. 8, the second-order dynamics terms have been
dropped following the iterative Linear-Quadratic Regulator (iLQR) algorithm
[18]. A full derivation with all second-order terms is given in Section -A.

Substituting this quadratic approximation into Eq. 3 and dropping the terms
not dependent on $\delta\mathbf{u}_{t}$ gives

$\displaystyle\begin{aligned}
V(\mathbf{x}_{t};\boldsymbol{\theta})=\min_{\delta\mathbf{u}_{t}}\Big{[}&(Q^{u}_{t})^{\top}\delta\mathbf{u}_{t}+\delta\mathbf{x}_{t}^{\top}Q^{xu}_{t}\delta\mathbf{u}_{t}\\\
&+\delta\boldsymbol{\theta}^{\top}Q^{\theta
u}_{t}\delta\mathbf{u}_{t}+\frac{1}{2}\delta\mathbf{u}_{t}^{\top}Q^{uu}_{t}\delta\mathbf{u}_{t}\Big{]}.\end{aligned}$
(9)

Taking the gradient of the argument in the minimization and setting it equal
to zero yields the optimal control update

$\displaystyle\delta\mathbf{u}_{t}^{*}$
$\displaystyle=\mathbf{k}_{t}+\mathbf{K}_{t}\delta\mathbf{x}_{t}+\mathbf{M}_{t}\delta\boldsymbol{\theta}$
(10)

with

$\displaystyle\mathbf{k}_{t}$
$\displaystyle\vcentcolon=-(Q^{uu}_{t})^{-1}Q^{u}_{t},$ (11)
$\displaystyle\mathbf{K}_{t}$
$\displaystyle\vcentcolon=-(Q^{uu}_{t})^{-1}Q^{ux}_{t},$
$\displaystyle\mathbf{M}_{t}$
$\displaystyle\vcentcolon=-(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}.$

Note the gains $\mathbf{k}_{t}$ and $\mathbf{K}_{t}$ match the standard DDP
gains, with an additional feedback gain $\mathbf{M}_{t}$ on
$\delta\boldsymbol{\theta}$ providing a correction because the algorithm is
optimizing over $\delta\mathbf{u}_{t}$ and $\delta\boldsymbol{\theta}$
simultaneously. Taking $\delta\boldsymbol{\theta}=\mathbf{0}$ in Eq. 10 means
the parameters are held constant which yields the usual DDP control update.

The optimal $\boldsymbol{\theta}$ minimizes Eq. 3 at time $t=1$. Since the
initial condition is fixed, $\mathbf{x}_{1}=\bar{\mathbf{x}}_{1}$, which
implies $\delta\mathbf{x}_{1}=\mathbf{0}$. Substituting in the quadratic
approximation of $Q$ into Eq. 4 and dropping the terms not dependent on
$\delta\boldsymbol{\theta}$, yields

$\displaystyle\delta\boldsymbol{\theta}^{*}$
$\displaystyle=\operatorname*{argmin}_{\delta\boldsymbol{\theta}}\Big{[}(Q^{\theta}_{1})^{\top}\delta\boldsymbol{\theta}+\delta\mathbf{u}_{1}^{\top}Q^{u\theta}_{1}\delta\boldsymbol{\theta}+\frac{1}{2}\delta\boldsymbol{\theta}^{\top}Q^{\theta\theta}_{1}\delta\boldsymbol{\theta}\Big{]}$
$\displaystyle=-(Q^{\theta\theta}_{1})^{-1}Q^{\theta}_{1}-(Q^{\theta\theta}_{1})^{-1}Q^{\theta
u}_{1}\delta\mathbf{u}_{1}.$ (12)

At the initial time $t=1$, there are two equations and two unknowns
$\delta\boldsymbol{\theta}^{*}$ and $\delta\mathbf{u}_{1}^{*}$. Substituting
in $\delta\mathbf{u}_{1}^{*}$ from Eq. 10 results in the parameter update

$\displaystyle\begin{aligned}
\delta\boldsymbol{\theta}^{*}=\mathbf{m}\vcentcolon=-(Q^{\theta\theta}_{1}-Q^{\theta
u}_{1}(Q^{uu}_{1})^{-1}Q^{u\theta}_{1})^{-1}\\\ (Q^{\theta}_{1}-Q^{\theta
u}_{1}(Q^{uu}_{1})^{-1}Q^{u}_{1}).\end{aligned}$ (13)

When the feedforward gains $\mathbf{k}_{t}$ of Eq. 10 and $\mathbf{m}$ of Eq.
13 are too aggressive, the state trajectory may stray from the region where
the quadratic approximation is accurate, and the cost may not decrease. To
ensure convergence, the feedforward gains are scaled by the parameter
$0<\epsilon\leq 1$, such that

$\displaystyle\delta\mathbf{u}_{t}^{*}$
$\displaystyle=\epsilon\mathbf{k}_{t}+\mathbf{K}_{t}\delta\mathbf{x}_{t}+\mathbf{M}_{t}\delta\boldsymbol{\theta}^{*},$
(14) $\displaystyle\delta\boldsymbol{\theta}^{*}$
$\displaystyle=\epsilon\mathbf{m}.$ (15)

The parameter $\epsilon$ is determined using line search and ensures the
optimizer makes sufficient progress towards the minimum at each iteration,
i.e. ensuring the update step satisfies Armijo’s condition or the Wolfe
conditions [5]. Further discussion is given in Section II-C.

What remains is to solve the value function derivatives by substituting in the
expression for $\delta\mathbf{u}_{t}^{*}$. The expression for
$\delta\boldsymbol{\theta}^{*}$ is not substituted back in, otherwise the
terms in $\delta\boldsymbol{\theta}^{*}$ are incorporated into the zero-order
value function term and the derivatives with respect to $\boldsymbol{\theta}$
go to zero. This means that the information for $\boldsymbol{\theta}$ cannot
be propagated backwards in time. In Section II-C, to prove the cost reduction
achievable, the full expression for $\delta\boldsymbol{\theta}^{*}$ is
substituted into Eq. 7.

Substituting Eq. 14 into Eq. 7 and equating like powers of the value function
expansion in Eq. 6 gives the following expressions for the value function
derivatives:

$\begin{split}V^{0}_{t}&=Q^{0}_{t}+(\frac{1}{2}\epsilon^{2}-\epsilon)(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t},\\\
V^{x}_{t}&=Q^{x}_{t}-Q^{xu}_{t}(Q^{uu}_{t})^{-1}Q^{u}_{t},\\\
V^{\theta}_{t}&=Q^{\theta}_{t}-Q^{\theta u}_{t}(Q^{uu}_{t})^{-1}Q^{u}_{t},\\\
V^{xx}_{t}&=Q^{xx}_{t}-Q^{xu}_{t}(Q^{uu}_{t})^{-1}Q^{ux}_{t},\\\
V^{x\theta}_{t}&=Q^{x\theta}_{t}-Q^{xu}_{t}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}=(V^{\theta
x}_{t})^{\top},\\\ V^{\theta\theta}_{t}&=Q^{\theta\theta}_{t}-Q^{\theta
u}_{t}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}.\end{split}$ (16)

Eq. 16 and Eq. 8 provide the equations for the backward pass of PDDP, with
boundary conditions
$V^{0}_{T+1}=\phi(\bar{\mathbf{x}}_{T+1};\bar{\boldsymbol{\theta}})$,
$V^{x}_{T+1}=\nabla_{x}\phi(\bar{\mathbf{x}}_{T+1};\bar{\boldsymbol{\theta}})$,
etc. The derivatives of $V$ and $Q$ are solved for backwards in time starting
from $t=T+1$ down to the initial time $t=1$ along the nominal trajectory. The
forward pass of the algorithm consists of using the control updates from Eq.
14 and parameter updates from Eq. 15 to compute the new state trajectory
starting from the initial condition $\mathbf{x}_{1}$. This yields an updated
nominal trajectory, and this process can be repeated until some convergence
criteria is met. Discussion on regularization of the value function
derivatives as well as methods for updating the controls and parameters is
given in Section II-D once the convergence of the method is proven.

###  II-C Convergence Analysis

This section provides a mathematically rigorous convergence analysis of the
proposed PDDP algorithm. The main result is summarized in Theorem 1, which
shows PDDP is globally convergent to a minimum of the cost function. The same
assumptions as in [15] for classic DDP are made in this work, including the
differentiability of the dynamics and cost function up to second-order.

The following proposition establishes the optimality of the parameter update
step by drawing comparisons to Newton’s method.

######  Proposition 1.

The parameter update $\delta\boldsymbol{\theta}^{*}$ is a (damped) Newton step
towards the minimum of the value function.

###### Proof.

See Section -B. ∎

Proposition 1 implies PDDP converges quadratically fast to the optimal
parameters $\boldsymbol{\theta}^{*}$, adopting the convergence rate of
Newton’s method. It is important to note that PDDP differs from a pure
stagewise Newton’s method in that it uses the full nonlinear dynamics during
the forward pass, thus achieving better numerical convergence properties [19].

However, for general nonlinear, nonconvex problems, it cannot be guaranteed
that $V^{\theta\theta}_{1}$ or $Q^{uu}_{1}$ remain positive definite, which
motivates the addition of regularization to ensure convergence. Regularization
can be accomplished through addition of a Levenberg-Marquardt parameter
ensuring the Hessian matrices are always positive definite [29]. Throughout
this work, the regularization scheme proposed in [27] is adopted. Further
discussion is given in Section II-D.

Next, the control updates are proven to reduce the cost after each iteration
of PDDP. To this end, an expression for the gradient of the cost function with
respect to an individual control is derived, and it is shown that the
variations in control, state, and parameters are $O(\epsilon)$.

######  Lemma 1.

The gradient of the cost function with respect to the control at time $t$ is
given by

$\displaystyle\nabla_{\mathbf{u}_{t}}\mathcal{J}$
$\displaystyle=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}\eta_{t+1},$
(17)

with $\eta_{t}=\mathcal{L}^{x}_{t}+(\mathbf{F}^{x}_{t})^{\top}\eta_{t+1}$ for
$t=1,\ldots,T$ and $\eta_{T+1}=\phi^{x}_{T+1}$.

###### Proof.

See Section -C. ∎

######  Lemma 2.

For the form of $\delta\boldsymbol{\theta}$ given in Eq. 15, it is true that

$\displaystyle\delta\boldsymbol{\theta}$ $\displaystyle=O(\epsilon),$ (18)

where $O(\cdot)$ corresponds to big-O notation in $\norm{\cdot}_{2}$.

Further, for all $t=1,\ldots,T$,

$\displaystyle\delta\mathbf{u}_{t}$ $\displaystyle=O(\epsilon),$ (19a)
$\displaystyle\delta\mathbf{x}_{t+1}$ $\displaystyle=O(\epsilon).$ (19b)

###### Proof.

See Section -D. ∎

Next, the control updates are shown to be a descent direction of the cost.

######  Proposition 2.

The optimal control updates satisfy

$\displaystyle(\nabla_{\mathbf{U}}\mathcal{J})^{\top}\Delta\mathbf{U}$
$\displaystyle=\sum_{t=1}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}<0.$
(20)

###### Proof.

It can be shown that

$\begin{split}\sum_{t=1}^{T}&(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}\\\
&=-\epsilon\sum_{t=1}^{T}\lambda_{t}+\epsilon\left(\sum_{t=1}^{T}\gamma_{t}\right)(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}+O(\epsilon^{2}),\end{split}$

where $\lambda_{t}=(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t}$ and
$\gamma_{t}=(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}$. This implies
there exists some $\epsilon$ sufficiently small such that Eq. 20 holds. The
full proof is given in Section -E. ∎

######  Remark 1.

The row vector $\sum_{t=1}^{T}\gamma_{t}$ can be thought of as correcting for
the change in $\boldsymbol{\theta}$, as both the parameters and the controls
are optimized simultaneously. The inner product between
$\sum_{t=1}^{T}\gamma_{t}$ and the parameter ascent direction
$(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}$ ensures that the optimizer does
not take too large of a step in the case when the controls and the parameters
both contribute to a reduction in cost.

To show the cost reduction achievable after a single iteration of PDDP, the
expression for $\delta\boldsymbol{\theta}^{*}$ must be substituted into the
value function approximation given by Eq. 7. This substitution means the terms
in $\delta\boldsymbol{\theta}$ are now incorporated into the zero-order term
$V^{0}_{t}$. The previous expression for $V^{0}_{t}$ given in Eq. 16 only
accounts for the change in controls $\delta\mathbf{u}_{t}^{*}$, which was
necessary to derive a backwards rule for the value function derivatives with
respect to $\boldsymbol{\theta}$. The following lemma establishes the value
function change by applying the updates $\delta\mathbf{u}_{t}^{*}$ and
$\delta\boldsymbol{\theta}^{*}$.

######  Lemma 3.

The zero-order value function approximation term satisfies

$\displaystyle V^{0}_{t}$
$\displaystyle=Q^{0}_{t}-\epsilon(1-\frac{1}{2}\epsilon)\lambda_{t}+\epsilon(V^{\theta}_{t})^{\top}\mathbf{m}+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}V^{\theta\theta}_{t}\mathbf{m}.$
(21)

Further, at time $t=1$, the following is true:

$\displaystyle V^{0}_{1}$
$\displaystyle=Q^{0}_{1}-\epsilon(1-\frac{1}{2}\epsilon)(\lambda_{1}+\psi),$
(22)

where $\psi=(V^{\theta}_{1})^{\top}(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}$.

###### Proof.

See Section -F. ∎

######  Remark 2.

$\psi$ can be interpreted as the Newton decrement on the parameters. Likewise,
$\lambda_{t}$ can be interpreted as the Newton decrement on the controls at
each timestep. These values can be used as an effective stopping criteria for
the algorithm as well as verification for the line search parameter $\epsilon$
[5]. Further discussion is provided in the following proposition.

Now, it is possible to show the cost reduction achievable after an iteration
of PDDP.

######  Proposition 3.

The total cost reduction after each iteration of PDDP is

$\displaystyle\Delta\mathcal{J}$
$\displaystyle=-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right)+O(\epsilon^{3}).$
(23)

###### Proof.

See Section -G. ∎

######  Remark 3.

Proposition 3 implies that a valid line search candidate at each iteration
should satisfy the following cost reduction:

$\displaystyle\Delta\mathcal{J}\leq-\kappa\epsilon\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right),$
(24)

for a small constant $\kappa>0$. This condition is similar to the line search
criteria for Newton’s method, and ensures that Armijo’s condition or the Wolfe
conditions hold [5].

Finally, the convergence of PDDP to a minimum of the cost is proven.

######  Theorem 1.

As the number of iterations of PDDP approaches infinity, the cost
$\mathcal{J}$, the control trajectory $\mathbf{U}$, and the parameters
$\boldsymbol{\theta}$ converge to a stationary point regardless of
initialization.

###### Proof.

See Section -H. ∎

###  II-D Algorithm Design

The convergence result presented in Section II-C assumes that the Hessian
matrices $Q^{uu}_{t}$ and $V^{\theta\theta}_{1}$ remain positive definite
throughout the optimization process. One method of ensuring this condition
always holds is to use Levenberg-Marquardt regularization corresponding to
adding a quadratic cost around the nominal trajectory [29]. This
regularization can be employed to ensure $V^{\theta\theta}_{1}$ is positive
definite since this Hessian matrix only needs to be invertible at the first
timestep. However, the authors of [27] have shown a more robust approach for
regularizing $Q^{uu}_{t}$ places the regularization on the states rather than
the controls, ensuring the control perturbation does not have different
effects at different timesteps. Therefore, the regularized derivatives can be
computed using

$\begin{split}\tilde{V}^{xx}_{t+1}&=V^{xx}_{t+1}+\mu\mathbf{I}\\\
\tilde{V}^{\theta\theta}_{1}&=V^{\theta\theta}_{1}+\nu\mathbf{I},\end{split}$
(25)

with hyperparameters $\mu,\nu\geq 0$. The regularized Hessian
$\tilde{V}^{xx}_{t+1}$ is used in place of the original during the backward
pass calculation of Eq. 8. The regularization of $V^{xx}_{t+1}$ and
$V^{\theta\theta}_{1}$ correspond to adding a quadratic cost to the deviation
of the state and parameters from the nominal values, thus ensuring the update
step does not stray too far from the region where the quadratic approximation
is accurate. However, the addition of the regularization on $V^{xx}_{t+1}$
when calculating the derivatives in Eq. 16 means the same cancellations of
$Q^{uu}_{t}$ and its inverse are not possible. The updated derivatives are
given as

$\begin{split}V^{x}_{t}&=Q^{x}_{t}+\mathbf{K}_{t}^{\top}Q^{u}_{t}+Q^{xu}_{t}\mathbf{k}_{t}+\mathbf{K}_{t}^{\top}Q^{uu}_{t}\mathbf{k}_{t},\\\
V^{\theta}_{t}&=Q^{\theta}_{t}+\mathbf{M}^{\top}_{t}Q^{u}_{t}+Q^{\theta
u}_{t}\mathbf{k}_{t}+\mathbf{M}^{\top}_{t}Q^{uu}_{t}\mathbf{k}_{t},\\\
V^{xx}_{t}&=Q^{xx}_{t}+Q^{xu}_{t}\mathbf{K}_{t}+\mathbf{K}_{t}^{\top}Q^{ux}_{t}+\mathbf{K}_{t}^{\top}Q^{uu}_{t}\mathbf{K}_{t},\\\
V^{x\theta}_{t}&=Q^{x\theta}_{t}+Q^{xu}_{t}\mathbf{M}_{t}+\mathbf{K}_{t}^{\top}Q^{u\theta}_{t}+\mathbf{K}_{t}^{\top}Q^{uu}_{t}\mathbf{M}_{t}\\\
V^{\theta\theta}_{t}&=Q^{\theta\theta}_{t}+Q^{\theta
u}_{t}\mathbf{M}_{t}+\mathbf{M}^{\top}_{t}Q^{u\theta}_{t}+\mathbf{M}^{\top}_{t}Q^{uu}_{t}\mathbf{M}_{t}.\end{split}$
(26)

An additional benefit of the proposed method is that the optimization of the
controls and parameters can be coupled or decoupled without affecting the
convergence. The result in Theorem 1 assumes both the control and parameter
feedforward gains are applied simultaneously, but the same result holds if the
gains are applied in an alternating fashion, e.g. at every odd iteration the
controls are updated by applying the control feedforward gain $\mathbf{k}_{t}$
and at every even iteration the parameters are updated by applying the
parameter feedforward gain $\mathbf{m}$. The addition of the feedback term
$\mathbf{M}_{t}\delta\boldsymbol{\theta}$ on the parameter update when
calculating the updated controls in Eq. 14 ensures stability in the controls
even when only the parameters are being updated. Likewise, [17] adopt an
approach where the parameters are only updated once the controls have fully
converged. This approach is referred to as Switching Time Optimization
Differential Dynamic Programming (STO-DDP). The alternating and simultaneous
schemes are benchmarked against STO-DDP through a numerical comparison on
three systems, which is presented in Section IV-B.

In summary, Algorithm 1 describes the proposed PDDP algorithm.

Input: Nominal trajectory $\bar{\mathbf{x}}_{t},\bar{\mathbf{u}}_{t}$ and
parameters $\bar{\boldsymbol{\theta}}$

1 while  _not converged_ do

2 Calculate derivatives of $\cal{L}$, $\phi$ along
$\bar{\mathbf{x}}_{t},\bar{\mathbf{u}}_{t},\bar{\boldsymbol{\theta}}$

// Backward pass

3 $V^{0}_{T+1}\leftarrow\phi^{0}_{T+1}$,
$V^{x}_{T+1}\leftarrow\phi^{x}_{T+1}$,
$V^{\theta}_{T+1}\leftarrow\phi^{\theta}_{T+1}$,

4 $V^{xx}_{T+1}\leftarrow\phi^{xx}_{T+1}$,
$V^{x\theta}_{T+1}\leftarrow\phi^{x\theta}_{T+1}$,
$V^{\theta\theta}_{T+1}\leftarrow\phi^{\theta\theta}_{T+1}$

5 for  _$t=T,\ldots,1$_ do

6 Calculate derivatives of $Q$ according to Eq. 8 and Eq. 25

7 Calculate gains $\mathbf{k}_{t},\mathbf{K}_{t},\mathbf{M}_{t}$ according to
Eq. 11

8 Calculate derivatives of $V$ according to Eq. 26

// End backward pass

9 Calculate gain $\mathbf{m}$ according to Eq. 13

// Line search

10 $\epsilon\leftarrow 1$

11 while  _cost decrease not sufficient_ do

// Forward pass

12 if  _update parameters_ then

13 $\delta\boldsymbol{\theta}\leftarrow\epsilon\mathbf{m}$

14

15 else

16 $\delta\boldsymbol{\theta}\leftarrow\mathbf{0}$

17
$\boldsymbol{\theta}\leftarrow\bar{\boldsymbol{\theta}}+\delta\boldsymbol{\theta}$

18 $\mathbf{x}_{1}\leftarrow\bar{\mathbf{x}}_{1}$

19 for  _$t=1,\ldots,T$_ do

20 $\delta\mathbf{x}_{t}\leftarrow\mathbf{x}_{t}-\bar{\mathbf{x}}_{t}$

21 if  _update controls_ then

22
$\delta\mathbf{u}_{t}\leftarrow\epsilon\mathbf{k}_{t}+\mathbf{K}_{t}\delta\mathbf{x}_{t}+\mathbf{M}_{t}\delta\boldsymbol{\theta}$

23 else

24
$\delta\mathbf{u}_{t}\leftarrow\mathbf{K}_{t}\delta\mathbf{x}_{t}+\mathbf{M}_{t}\delta\boldsymbol{\theta}$

25 $\mathbf{u}_{t}\leftarrow\bar{\mathbf{u}}_{t}+\delta\mathbf{u}_{t}$

26
$\mathbf{x}_{t+1}\leftarrow\mathbf{F}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})$

// End forward pass

$\epsilon\leftarrow\rho\epsilon$ // Reduce $\epsilon$

27

// End line search

28 $\bar{\mathbf{x}}_{t}\leftarrow\mathbf{x}_{t}$,
$\bar{\mathbf{u}}_{t}\leftarrow\mathbf{u}_{t}$,
$\bar{\boldsymbol{\theta}}\leftarrow\boldsymbol{\theta}$

return _optimal controls $\mathbf{u}_{t}$, optimal parameters
$\boldsymbol{\theta}$_

Algorithm 1 Parameterized Differential Dynamic Programming

##  III Applications

In this section the proposed PDDP algorithm is applied to two important
robotics control tasks: parameter estimation and switching time optimization.

###  III-A Parameter Estimation

Estimating the unknown parameters and states of a dynamical system can be
achieved through moving horizon estimation (MHE). This framework reformulates
the state estimation problem as an optimization problem dual to MPC [8]. This
work assumes full state observability, with the realized discrete-time
dynamics given by

$\begin{split}\mathbf{x}_{t+1}&=\mathbf{F}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta}^{*})+\mathbf{w}_{t},\\\
\mathbf{x}_{1}&\sim\mathcal{N}(\hat{\mathbf{x}}_{1},\boldsymbol{\Sigma}_{\mathbf{x}_{1}}),\end{split}$

where $\mathbf{w}_{t}\sim\mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}_{w})$ is
some unknown additive Gaussian process noise. If the controls are given, this
corresponds to a trajectory smoothing problem where the goal is to identify
the parameters $\boldsymbol{\theta}$ and the realized disturbances
$\mathbf{w}_{t}$ that maximize the likelihood of the observed states. The
objective can be formulated through maximum likelihood estimation (MLE) by
minimizing the negative log-likelihood

$\displaystyle\mathcal{J}_{\text{est}}(\boldsymbol{\theta},\mathbf{x}_{1})$
$\displaystyle=-\log
p(\mathbf{x}_{1},\boldsymbol{\theta})\prod_{t=1}^{T}p(\mathbf{x}_{t+1}|\mathbf{x}_{t},\boldsymbol{\theta})$
$\displaystyle=-\log p(\mathbf{x}_{1},\boldsymbol{\theta})-\sum_{t=1}^{T}\log
p(\mathbf{x}_{t+1}|\mathbf{x}_{t},\boldsymbol{\theta}),$

with $p(\mathbf{x}_{t+1}|\mathbf{x}_{t},\boldsymbol{\theta})$ describing the
likelihood of observing the next state $\mathbf{x}_{t+1}$ given the current
state $\mathbf{x}_{t}$ and parameters $\boldsymbol{\theta}$, and
$p(\mathbf{x}_{1},\boldsymbol{\theta})$ describing the prior distribution over
the initial state and parameters.

Assuming a Gaussian prior over the parameters
$\boldsymbol{\theta}\sim\mathcal{N}(\hat{\boldsymbol{\theta}},\boldsymbol{\Sigma}_{\theta})$,
the estimation cost takes the form

$\displaystyle\begin{aligned}
\mathcal{J}_{\text{est}}(\boldsymbol{\theta},\mathbf{x}_{1})&=\frac{1}{2}\sum_{t=1}^{T_{\text{est}}}\norm{\mathbf{x}_{t+1}-\mathbf{F}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})}_{\boldsymbol{\Sigma}_{w}^{-1}}^{2}\\\
&\qquad+\frac{1}{2}\lVert\boldsymbol{\theta}-\hat{\boldsymbol{\theta}}\rVert_{\boldsymbol{\Sigma}_{\theta}^{-1}}^{2}+\frac{1}{2}\norm{\mathbf{x}_{1}-\hat{\mathbf{x}}_{1}}_{\boldsymbol{\Sigma}_{\mathbf{x}_{1}}^{-1}}^{2}.\end{aligned}$
(27)

The terms
$\lVert\boldsymbol{\theta}-\hat{\boldsymbol{\theta}}\rVert_{\boldsymbol{\Sigma}_{\theta}^{-1}}^{2}$
and
$\norm{\mathbf{x}_{1}-\hat{\mathbf{x}}_{1}}_{\boldsymbol{\Sigma}_{\mathbf{x}_{1}}^{-1}}^{2}$
ensure the new estimates of the parameters and initial state do not deviate
too far from the prior, while
$\norm{\mathbf{x}_{t+1}-\mathbf{F}(\mathbf{x}_{t},\mathbf{u}_{t};\boldsymbol{\theta})}_{\boldsymbol{\Sigma}_{w}^{-1}}^{2}$
ensures the predicted state trajectory matches the actual observed states.
This cost function setup allows PDDP to be used to identify the true system
parameters as well as an updated estimate of the true state. The observed
state trajectory is assumed to be given, and the parameters are updated to
match the model predictions to the observed states through minimization of Eq.
27.

The time horizon $T_{\text{est}}$ corresponds to the estimation horizon, i.e.
the number of steps in the history to estimate. In practice, the horizon is
shifted forward every step when the number of observed states exceeds the time
horizon $T_{\text{est}}$ to maintain computational tractability, particularly
in realtime applications. This approach is why MHE is often referred to as the
dual problem to MPC [8], as the same receding horizon approach is adopted to
solve realtime tasks using solvers such as DDP [27]. In the MPC problem, the
goal is to find the sequence of controls that minimizes a finite-horizon cost
function of the form

$\displaystyle\mathcal{J}_{\text{mpc}}(\mathbf{U})$
$\displaystyle=\sum_{t=1}^{T_{\text{mpc}}}\mathcal{L}(\mathbf{x}_{t},\mathbf{u}_{t})+\phi(\mathbf{x}_{T_{\text{mpc}}+1}).$
(28)

While MHE optimizes over the past horizon $T_{\text{est}}$ up to the current
state at time $t$, MPC plans over the future time horizon from the current
time $t$ up to $T_{\text{mpc}}$. In both methods, as a new state observation
is made, the time horizon is shifted forward one step and the optimization
proceeds again. A combined cost can be derived to optimize over both tasks
simultaneously using PDDP:

$\displaystyle\mathcal{J}_{\text{combined}}(\mathbf{U};\boldsymbol{\theta},\mathbf{x}_{1})$
$\displaystyle=\mathcal{J}_{\text{est}}(\boldsymbol{\theta},\mathbf{x}_{1})+\mathcal{J}_{\text{mpc}}(\mathbf{U}).$
(29)

This combined cost function is used in Section IV-A to find the optimal
parameters of a system while solving an MPC task. At each timestep $t$, PDDP
minimizes the MHE cost over the past time horizon $[t-T_{\text{est}},t]$,
updating the parameter estimate to best match the observed states.
Simultaneously, PDDP minimizes the MPC cost function, planning over the future
time horizon $[t,t+T_{\text{mpc}}]$ and generating a control update
corresponding to standard DDP plus feedback based on the simultaneous update
of the parameters. This approach can be interpreted as a variation of adaptive
MPC [1] since the system parameters are adapted to fit the true model at every
step of the MPC algorithm. The simultaneous optimization is possible due to
the explicit feedback gains provided during the derivation of PDDP. This
generalizes previous work by [16], which uses DDP to only solve the MHE task.

###  III-B Switching Time Optimization

Switching time optimization (STO) is proposed by the authors of [17] and
reformulates the trajectory optimization of hybrid systems into an equivalent
objective that is easier to solve using iterative numerical solvers, including
DDP.

A hybrid system is defined by a sequence of $N$ modes such that for each mode
$i=1,\ldots,N$, the dynamics obey

$\displaystyle\dot{\mathbf{x}}$
$\displaystyle=\mathbf{f}_{i}(\mathbf{x}(t),\mathbf{u}(t)).$ (30)

Example hybrid systems include bipedal robotic animals, which transition
between modes of flight and contact with the ground while running, and UAM
class vehicles, which exhibit modes of vertical takeoff and landing, fixed-
wing cruise, and a transition phase between the two.

Given this fixed sequence of modes, the goal in hybrid systems trajectory
optimization is to find the optimal control trajectory and the optimal set of
times $\mathbf{t}=[t_{1},\ldots,t_{N}]^{\top}$ that minimize the cost, with
each $t_{i}$ describing the terminal time of mode $i$. This problem can be
formulated through the continuous time objective

$\displaystyle\min_{\mathbf{u},\mathbf{t}}\sum_{i=1}^{N}\left[\int_{t_{i-1}}^{t_{i}}\ell_{i}(\mathbf{x}(t),\mathbf{u}(t))\differential{t}+\varphi_{i}(\mathbf{x}(t_{i}))\right],$
(31)

with the dynamics transitioning accordingly through the modes defined in Eq.
30.

The authors of [17] show that this objective can be equivalently expressed as
trying to find the optimal amount of time to spend in each mode through
switching time optimization (STO). The dynamics in Eq. 30 and objective in Eq.
31 are reformulated over fixed time intervals of unit length, with the
dynamics and running cost scaled by the amount of time that is spent in each
mode. Letting $\theta_{i}=t_{i}-t_{i-1}$ and adopting the change of variable
$\tau=(t-t_{i-1})/\theta_{i}+i-1$ results in the time scaled dynamics

$\displaystyle\dot{\mathbf{x}}$
$\displaystyle=\theta_{i}\mathbf{f}_{i}(\mathbf{x}(\tau),\mathbf{u}(\tau)),$
(32)

and the objective can be rewritten as

$\displaystyle\min_{\mathbf{u},\boldsymbol{\theta}}\sum_{i=1}^{N}\left[\int_{i-1}^{i}\theta_{i}\ell_{i}(\mathbf{x}(\tau),\mathbf{u}(\tau))\differential{\tau}+\varphi_{i}(\mathbf{x}(i))\right].$
(33)

The equivalent discrete-time optimal control problem is given as

$\displaystyle\min_{\mathbf{U},\mathbf{t}}\sum_{i=1}^{N}\left[\phi_{i}(\mathbf{x}_{T_{i}+1})+\sum_{t=T_{i-1}+1}^{T_{i}}\theta_{i}\mathcal{L}_{i}(\mathbf{x}_{t},\mathbf{u}_{t})\right]$
(34a) $\displaystyle\text{subject to
}\mathbf{x}_{t+1}=\mathbf{F}_{i}(\mathbf{x}_{t},\mathbf{u}_{t};\theta_{i}),$
(34b)

with a chosen integration step size $h$ and $T_{i}=T_{i-1}+\theta_{i}/h$,
$T_{0}=0$ denoting the number of timesteps in the horizon up to the end of
mode $i$. This objective is a parameterized discrete-time optimal control
problem that can be solved using PDDP. In this problem, the optimized
parameters are the intervals of time spent in each mode $\theta_{i}$. For a
single mode, this objective is an approximation to a free-horizon optimal
control problem, where the terminal time of the trajectory parameterizes the
dynamics and can be optimized using PDDP. Similar parameterizations have been
discussed in previous works such as [20, 26].

Three key improvements are introduced to stabilize the optimization using a
numerical solver such as PDDP. First, the number of timesteps per mode is set
to be constant, meaning timesteps do not have to be interpolated when the
amount of time in a mode changes during the optimization. This improvement
ensures that the convergence properties described in Section II-C hold, as
changing the number of timesteps in the optimization modifies the objective,
which can cause unexpected problems and results in poor convergence.

Second, an improved integration scheme is adopted for calculating the
discrete-time dynamics. Using the typical forward Euler integration with fixed
step size $h$, the discrete-time dynamics from Eq. 34b are given as

$\displaystyle\mathbf{x}_{t+1}=\mathbf{x}_{t}+\theta_{i}\mathbf{f}_{i}(\mathbf{x}_{t},\mathbf{u}_{t})h.$
(35)

Note the time scaling in the dynamics by the amount of time spent in each mode
$\theta_{i}$ results in an equivalent scaling of the integration step size $h$
used in the forward Euler integration, which causes inaccuracies in the
dynamics, particularly as the time scale $\theta_{i}$ becomes large. To remedy
this issue, the dynamics are integrated at a smaller underlying step size
$\Delta t$ for multiple substeps, which prevents numerical errors in the
dynamics causing the optimization to fail.

Finally, constraints are necessary in order to ensure the time scales are
always nonnegative. A popular choice to handle box control limits is the
projected Newton quadratic program solver proposed in [28]. The same projected
Newton solver is used when calculating the parameter feedforward gain
$\mathbf{m}$ to ensure that the time scales never become negative.

##  IV Experiments

In this section, PDDP is applied to two separate tasks: optimal parameter
estimation and switching time optimization. The results show PDDP can solve
MHE and MPC problems simultaneously, as well as optimize for time-optimal
trajectories through STO.

###  IV-A Parameter Estimation

The dual optimization over the combined MHE and MPC tasks proposed in Section
III-A is solved by PDDP on multiple nonlinear, underactuated systems including
a cartpole, quadrotor, and ant quadruped.

####  IV-A1 Cartpole

Figure 2: Pole mass estimate for the cartpole system.

The cartpole is an underactuated, nonlinear dynamical system with four states
and one control. PDDP is given a bad initial estimate of the pole mass of
$2\text{\,}\mathrm{kg}$ in its dynamics model and is tasked with finding the
correct pole mass of $0.5\text{\,}\mathrm{kg}$ while simultaneously bringing
the pole to the upright position starting from the downward configuration. The
MPC and MHE horizons are chosen to be $T_{\text{mpc}}=T_{\text{est}}=100$
steps long, while the underlying discretization step of the environment is
given as $\Delta t=$0.02\text{\,}\mathrm{s}$$. The optimization is run for 200
MPC steps.

The convergence of PDDP to the true mass is given in Fig. 2. PDDP makes little
progress for the first 15 steps since the dynamics of the pole evolve slowly
and the true mass is hard to estimate. Once the pole velocity increases, the
mass can be estimated successfully within 50 steps.

####  IV-A2 Quadrotor

(a) Mass $m$ estimate

(b) Inertia $J_{x}$ and $J_{y}$ estimates

(c) Inertia $J_{z}$ estimate

(d) Quadrotor true trajectory

Figure 3: (a-c): Parameter estimates for the quadrotor system. PDDP converges
to the true values within 15 MHE steps. (d): The spatial trajectory of the
quadrotor as it arrives at the target state. Note the slow progress at the
start while the dynamics model is inaccurate.

(a) Initial estimate

(b) Estimate, MHE step 1

(c) Estimate, MHE step 2

(d) Upper limb length $\ell_{1}$

(e) Lower limb length $\ell_{2}$

Figure 4: (a-c): Leg length estimates for the ant system, rendered using Brax
[10]. The estimate after MHE step 2 very closely matches the true model. (d,
e): Estimate history. PDDP converges to the true values within 4 MHE steps.

The quadrotor has 6 degrees of freedom, resulting in twelve states
$\mathbf{x}=[x,y,h,\phi,\theta,\psi,\dot{x},\dot{y},\dot{h},p,q,r]^{\top}$ and
four controls $\mathbf{u}=[F,\tau_{\phi},\tau_{\theta},\tau_{\psi}]^{\top}$
that correspond to the total thrust produced by the rotors, and the rolling,
pitching, and yawing torques, respectively. The full description of the
dynamics is given by [4].

PDDP is tasked with estimating the mass and diagonal inertia terms of the
quadrotor while driving the quadrotor to a target position of (5, 5, 5)
$\text{\,}\mathrm{m}$ starting from the origin. It is assumed that the
quadrotor is symmetric about all three axes, so the off-diagonal terms of the
inertia matrix are zero. The initial guesses of the parameters are given as
$m=$1\text{\,}\mathrm{kg}$$, $J_{x}=$
$1\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$, $J_{y}=$
$1\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$, and $J_{z}=$
$1\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$, while the true parameters
values are $m=$ $0.468\text{\,}\mathrm{kg}$, $J_{x}=$
$4.856\text{\times}{10}^{-3}\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$,
$J_{y}=$
$4.856\text{\times}{10}^{-3}\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$,
and $J_{z}=$
$8.801\text{\times}{10}^{-3}\text{\,}\mathrm{kg}\text{\,}{\mathrm{m}}^{2}$,
which are adapted from [2]. The initial estimate is very poor, and assumes the
rotors are very heavy and/or far from the center of mass of the quadrotor. For
this task, the MPC and MHE horizons are chosen as
$T_{\text{mpc}}=T_{\text{est}}=100$ steps, and the dynamics is integrated at a
step size of $\Delta t=$0.01\text{\,}\mathrm{s}$$.

The results are plotted in Fig. 3. PDDP converges to the true values within 15
MHE steps, meaning PDDP is able to find the true parameters in under
$0.2\text{\,}\mathrm{s}$ of total execution time. The MPC task is then solved
using the corrected dynamics model, taking a total of $3\text{\,}\mathrm{s}$
to arrive at the target state.

####  IV-A3 Ant

The ant is a popular robotics system for continuous control and reinforcement
learning. The physics used for this system is adapted from Brax [10]. This
quadruped system is described by nine rigid bodies and has a state dimension
of 117 in Brax, 13 states for each rigid body describing their position,
quaternion rotation, linear velocity, and angular velocity. The four legs of
the ant have two torque-controlled joints each, resulting in a control
dimension of 8.

The MPC task is to maximize the forward velocity of the robot, requiring
periodicity in the motion of the legs to generate the necessary forward force.
The MPC horizon was chosen as $T_{\text{mpc}}=100$ steps with an underlying
environment discretization step size of $\Delta t=$0.05\text{\,}\mathrm{s}$$,
resulting in a total planning horizon of $5\text{\,}\mathrm{s}$. The MPC cost
function rewards positive $x$ velocity and penalizes deviation of the $y$
position and total control effort.

(a) Cartpole

(b) Quadrotor

(c) Lift+Cruise

Figure 5: Convergence of PDDP on three switching time tasks for different
choices of optimization scheme. A normal distribution is fit to the cost at
each iteration, with the dark lines corresponding to the mean and the shaded
region showing the 95% confidence interval. To make the comparison clearer,
the total costs for each task have been normalized so the minimum cost
trajectory corresponds to a cost of 1.

The simultaneous MHE task is to estimate the length of the eight rigid bodies
of the legs of the ant. Symmetry is enforced to prevent instabilities, so the
legs lengths are parameterized by the upper limb length $\ell_{1}$ and the
lower limb length $\ell_{2}$. The true values are
$\ell_{1}=$0.443\text{\,}\mathrm{m}$$ and
$\ell_{2}=$0.726\text{\,}\mathrm{m}$$, respectively, with the initial estimate
given by $\ell_{1}=$1.0\text{\,}\mathrm{m}$$ and
$\ell_{2}=$2.0\text{\,}\mathrm{m}$$, resulting in vastly different dynamics.

The optimization is run for 250 total timesteps, with the parameter estimate
history shown in Figs. 4(d) and 4(e). PDDP converges to the true leg lengths
in 4 steps. While the dynamics model is inaccurate, the ant makes little
progress moving forward. However, once the true parameters are found, PDDP is
able to successfully drive the ant forward. The fast convergence to the
optimal parameters is in large part due to the choice of parameterization.
Since the limb lengths $\ell_{1}$ and $\ell_{2}$ parameterize all four legs of
the ant simultaneously, only a small subset of lengths can accurately describe
the observed change in the state of all four legs.

###  IV-B Switching Time Optimization

In this section, PDDP is used to solve an STO task for three systems, finding
the minimum time to bring a cartpole and quadrotor to two sequential target
orientations, and solving for the optimal transition point between flight
regimes for an overactuated UAM class vehicle. A numerical comparison is
performed to benchmark STO-DDP from [17] with the simultaneous and alternating
schemes proposed in Section II-D. The results are summarized in Fig. 5.

For each of the following experiments, the underlying environment integration
step size is chosen as $\Delta t=$0.01\text{\,}\mathrm{s}$$ (see discussion in
Section III-B).

####  IV-B1 Multi-target cartpole

Figure 6: Histogram of optimal time scales $\theta_{1}$ and $\theta_{2}$ for
the multi-target cartpole task. Bins are centered at each second from 1 up to
10. From Fig. 5(a), the alternating method consistently finds low cost
solutions despite different initializations of the switching times, while STO-
DDP and the simultaneous method converge to suboptimal solutions.

Starting from the origin with the pole in the downward position, the goal of
the multi-target cartpole problem is to first bring the pole to the upright
position at an $x$ position of $-5\text{\,}\mathrm{m}$ with zero velocity, and
then bring the pole to the upright position at an $x$ position of
$5\text{\,}\mathrm{m}$ with zero velocity.

In practice, the simultaneous optimization is highly dependent on the initial
guess of the nominal control trajectory. Therefore, the method is warm started
by only updating the controls for the first 5 iterations, corresponding to
standard DDP.

Figure 7: Time-optimal trajectory for quadrotor STO task found using PDDP. The
terminal times of each mode $T_{1}$ and $T_{2}$ are plotted as dashed black
lines. The target states are plotted as dashed red lines.

For each optimization scheme, PDDP was run for 1000 initial guesses of the
time scales $\theta_{1}$ and $\theta_{2}$, each sampled uniformly from the
interval $[1,10]$. The convergence behavior is plotted in Fig. 5(a). The
alternating scheme converges to a solution half an order of magnitude lower in
cost on average than both the simultaneous scheme and the STO-DDP approach
that waits for the controls to converge. Note that the simultaneous scheme
reduces the cost quicker than waiting for the controls to converge, but the
final solutions are comparable. Overall, the alternating scheme vastly
outperforms the others on average. This success is attributed to the fact that
alternating between updating the controls and updating the parameters allows
the solution to more effectively escape local minima throughout the
optimization process. Fig. 6 shows a histogram of the optimal time scales for
each of the different optimization schemes. The simultaneous and alternating
scheme converge to a solution of $\theta_{1}=$4\text{\,}\mathrm{s}$$ and
$\theta_{2}=$6\text{\,}\mathrm{s}$$ in the majority of cases. Waiting for the
controls to converge in STO-DDP results in a fairly flat distribution,
suggesting the method quickly falls into a suboptimal solution when analyzed
together with Fig. 5(a). Likewise, the simultaneous method converges to
similar switching times as the alternating method, but does not find an
optimal control solution, describing the poor convergence in Fig. 5(a). The
alternating scheme is able to consistently converge in both switching times
and controls, resulting in lower cost solutions.

####  IV-B2 Quadrotor

For the quadrotor STO task, starting from the origin, the goal is to reach a
first target position of $(5,5,5)$ $\text{\,}\mathrm{m}$ with zero velocity
and then to reach a second target of $(10,-5,-5)$ $\text{\,}\mathrm{m}$ with
zero velocity while optimizing for the amount of time to reach both targets.
The PDDP algorithm was run until convergence for 1000 initial guesses of the
switching times, sampled uniformly from the range [1, 10]. The convergence
behavior is plotted in Fig. 5(b). The simultaneous scheme quickly falls into
local minima, while waiting for the controls to converge in STO-DDP results in
linear convergence. The alternating scheme outperforms STO-DDP. The minimum
cost trajectory is shown in Fig. 7.

####  IV-B3 NASA Lift+Cruise vehicle

(a) $x$ position trajectory.

(b) $h$ altitude trajectory.

(c) Spatial trajectory.

Figure 8: Time-optimal vertical takeoff to cruise maneuver for the Lift+Cruise
vehicle found using PDDP. The terminal times of each mode $T_{1}$ and $T_{2}$
are plotted as dashed black lines. The target altitude is plotted as a dashed
red line. PDDP increases the altitude up to the target state during the first
mode, then increases the forward velocity while maintaining altitude during
the second mode.

(a) $x$ position trajectory.

(b) $h$ altitude trajectory.

(c) Spatial trajectory.

Figure 9: Time-optimal cruise to hover to vertical landing maneuver for the
Lift+Cruise vehicle found using PDDP. The terminal times of each mode $T_{1}$
and $T_{2}$ are plotted as dashed black lines. The target states are plotted
as dashed red lines. PDDP decreases the forward velocity of the aircraft by
pitching back, then initiates a vertical landing.

The NASA Lift+Cruise vehicle is a VTOL aircraft that operates as a fixed-wing
aircraft in forward flight, but has access to eight lifting rotors allowing
for VTOL capabilities. While optimal control algorithms, including DDP, have
been applied to these vehicles in the past [14], to the authors’ best
knowledge this work is the first application of switching time optimization
applied to changing flight regimes within trajectory planning. The STO task
performed here is a vertical takeoff followed by a partial transition from
hover into cruise. PDDP was run for 500 initial guesses of the switching times
sampled uniformly from [5, 15] seconds, with the convergence results of each
optimization scheme plotted in Fig. 5(c). On this complex task, the
simultaneous scheme converges to a poor solution. Meanwhile, the alternating
scheme and STO-DDP result in comparable performance, with STO-DDP beating the
alternating scheme for the first 50 iterations. The alternating scheme makes
little change to the switching times while the control solution is still
suboptimal during the first 50 iterations, but as the controls converge to a
solution that is better able to solve the problem, the alternating scheme is
able to successfully find a low cost solution for the switching times. A
representative time-optimal trajectory is shown in Fig. 8. The aircraft
requires $5.63\text{\,}\mathrm{s}$ to perform the vertical takeoff maneuver
and reach the desired altitude, then takes $9.79\text{\,}\mathrm{s}$ to
increase its forward velocity to the desired target speed.

To further demonstrate the planning capabilities of PDDP, the reverse partial
transition maneuver was tested. This task requires the aircraft to transition
from cruise to a hover configuration by bringing its forward velocity to zero
within $500\text{\,}\mathrm{f}\mathrm{t}$, and then perform a vertical
landing. The solved trajectory is shown in Fig. 9. This particular aircraft
model does not have access to flaps or a speed brake to decelerate, and thus
reduces its forward velocity by pitching back, pointing the body $z$-axis
forwards to generate the backwards thrust necessary to reduce the aircraft’s
speed. This cruise-to-hover transition is accomplished within
$6.87\text{\,}\mathrm{s}$. The aircraft then proceeds to lower its altitude
down to the target, requiring $4.87\text{\,}\mathrm{s}$ to complete the
landing.

##  V Conclusions and Future Work

This paper has derived a method generalizing previous work for solving
problems with time-invariant parameters using DDP. The proposed PDDP algorithm
is based on a general parameterized optimal control objective and allows
direct optimization over time-invariant parameters with theoretical
convergence guarantees. PDDP was applied to multiple robotics systems through
adaptive MPC using MHE and hybrid systems optimization via STO. In particular,
PDDP is able to identify the optimal transition point between flight regimes
for a UAM class vehicle exhibiting complex transition dynamics. Various
optimization schemes were analyzed and an alternating approach between
updating the controls and parameters was shown to converge to a better
solution by effectively escaping local minima throughout the optimization
process.

In terms of runtime, a purely CPU-based MATLAB implementation of Algorithm 1
with limited parallelization runs the NASA Lift+Cruise STO task presented in
Section IV-B3 in roughly $1.5\text{\,}\mathrm{s}$ per iteration (including the
line search) on an Intel i7-9850H $2.60\text{\,}\mathrm{GHz}$ processor. The
runtime is dominated by the complex dynamics calculations of the NASA
Lift+Cruise vehicle, especially the dynamics Jacobians calculations. The other
tasks run in under a second per iteration based on the complexity of the
dynamics. This is fairly slow for an MPC algorithm, and thus improvements can
be made, such as improved parallelization [23], in order to make the algorithm
more suitable for realtime MPC tasks.

Additionally, as described in [20], DDP can be used as a second-order
optimizer for speeding up the training process of neural networks. The
addition of parameter-based optimization into this approach can help improve
the training performance of modern machine learning methodologies.

## References

  * Adetola et al. [2009] Veronica Adetola, Darryl DeHaan, and Martin Guay.  Adaptive model predictive control for constrained nonlinear systems.  _Systems & Control Letters_, 58(5):320–326, 2009.
  * Ahmed and Chen [2018] Nigar Ahmed and Mou Chen.  Sliding mode control for quadrotor with disturbance observer.  _Advances in Mechanical Engineering_ , 10(7):1687814018782330, 2018.
  * Apgar et al. [2018] Taylor Apgar, Patrick Clary, Kevin Green, Alan Fern, and Jonathan W Hurst.  Fast online trajectory optimization for the bipedal robot Cassie.  In _Robotics: Science and Systems_ , volume 101, page 14, 2018.
  * Beard [2008] Randal W Beard.  Quadrotor dynamics and control.  _Brigham Young University_ , 19(3):46–56, 2008\.
  * Boyd and Vandenberghe [2004] Stephen Boyd and Lieven Vandenberghe.  _Convex Optimization_.  Cambridge University Press, 2004.
  * Branicky [1998] Michael S Branicky.  Multiple Lyapunov functions and other analysis tools for switched and hybrid systems.  _IEEE Transactions on Automatic Control_ , 43(4):475–482, 1998.
  * Di Carlo et al. [2018] Jared Di Carlo, Patrick M Wensing, Benjamin Katz, Gerardo Bledt, and Sangbae Kim.  Dynamic locomotion in the MIT Cheetah 3 through convex model-predictive control.  In _International Conference on Intelligent Robots and Systems (IROS)_ , pages 1–9. IEEE, 2018.
  * Diehl et al. [2009] Moritz Diehl, Hans Joachim Ferreau, and Niels Haverbeke.  _Efficient Numerical Methods for Nonlinear MPC and Moving Horizon Estimation_ , pages 391–417.  Springer Berlin Heidelberg, Berlin, Heidelberg, 2009.
  * Ezzine and Haddad [1989] Jelel Ezzine and AH Haddad.  Controllability and observability of hybrid systems.  _International Journal of Control_ , 49(6):2045–2055, 1989.
  * Freeman et al. [2021] C. Daniel Freeman, Erik Frey, Anton Raichuk, Sertan Girgin, Igor Mordatch, and Olivier Bachem.  Brax — a differentiable physics engine for large scale rigid body simulation, 2021.  URL http://github.com/google/brax.
  * Gregory [June 14, 2021] Irene M. Gregory.  Urban air mobility: A control-centric approach to addressing technical challenge.  IEEE FoRCE Webinar, June 14, 2021.  URL http://ieeecss.org/index.php/presentation/force-webinars/urban-air-mobility-control-centric-approach-addressing-technical.
  * Gregory et al. [2021] Irene M. Gregory, Natasha A. Neogi, Newton H. Campbell, Jon Holbrook, Barton J. Bacon, Patrick C. Murphy, Daniel D. Moerder, Benjamin M. Simmons, Michael J. Acheson, Thomas C. Britton, and Jacob Cook.  Intelligent contingency management for urban air mobility.  In _AIAA Scitech Forum_ , 2021.  doi: 10.2514/6.2021-1000.
  * Hedlund and Rantzer [1999] Sven Hedlund and Anders Rantzer.  Optimal control of hybrid systems.  In _Proceedings of the 38th IEEE Conference on Decision and Control_ , volume 4, pages 3972–3977. IEEE, 1999.
  * Houghton et al. [2022] Matthew D Houghton, Alexander B Oshin, Michael J Acheson, Evangelos A Theodorou, and Irene M Gregory.  Path planning: Differential dynamic programming and model predictive path integral control on VTOL aircraft.  In _AIAA Scitech Forum_ , page 0624, 2022.
  * Jacobson and Mayne [1970] David H Jacobson and David Q Mayne.  _Differential dynamic programming_.  Number 24. Elsevier Publishing Company, 1970.
  * Kobilarov et al. [2015] Marin Kobilarov, Duy-Nguyen Ta, and Frank Dellaert.  Differential dynamic programming for optimal estimation.  In _International Conference on Robotics and Automation (ICRA)_ , pages 863–869. IEEE, 2015.
  * Li and Wensing [2020] He Li and Patrick M Wensing.  Hybrid systems differential dynamic programming for whole-body motion planning of legged robots.  _IEEE Robotics and Automation Letters_ , 5(4):5448–5455, 2020.
  * Li and Todorov [2004] Weiwei Li and Emanuel Todorov.  Iterative linear quadratic regulator design for nonlinear biological movement systems.  In _ICINCO (1)_ , pages 222–229. Citeseer, 2004.
  * Liao and Shoemaker [1992] Li-zhi Liao and Christine A Shoemaker.  Advantages of differential dynamic programming over Newton’s method for discrete-time optimal control problems.  Technical report, Cornell University, 1992.
  * Liu et al. [2021] Guan-Horng Liu, Tianrong Chen, and Evangelos Theodorou.  Second-order neural ODE optimizer.  _Advances in Neural Information Processing Systems_ , 34, 2021.
  * Mayne [1966] David Mayne.  A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems.  _International Journal of Control_ , 3(1):85–95, 1966.
  * Orin et al. [2013] David E Orin, Ambarish Goswami, and Sung-Hee Lee.  Centroidal dynamics of a humanoid robot.  _Autonomous Robots_ , 35(2):161–176, 2013.
  * Plancher and Kuindersma [2018] Brian Plancher and Scott Kuindersma.  A performance analysis of parallel differential dynamic programming on a GPU.  In _International Workshop on the Algorithmic Foundations of Robotics_ , pages 656–672. Springer, 2018.
  * Silva et al. [2018] Christopher Silva, Wayne R Johnson, Eduardo Solis, Michael D Patterson, and Kevin R Antcliff.  VTOL urban air mobility concept vehicles for technology development.  In _Aviation Technology, Integration, and Operations Conference_ , page 3847, 2018.
  * Spong [1998] Mark W Spong.  Underactuated mechanical systems.  In _Control Problems in Robotics and Automation_ , pages 135–150. Springer, 1998.
  * Stachowicz and Theodorou [2021] Kyle Stachowicz and Evangelos A Theodorou.  Optimal-horizon model-predictive control with differential dynamic programming.  _arXiv preprint arXiv:2111.09207_ , 2021.
  * Tassa et al. [2012] Yuval Tassa, Tom Erez, and Emanuel Todorov.  Synthesis and stabilization of complex behaviors through online trajectory optimization.  In _International Conference on Intelligent Robots and Systems (IROS)_ , pages 4906–4913. IEEE, 2012.
  * Tassa et al. [2014] Yuval Tassa, Nicolas Mansard, and Emo Todorov.  Control-limited differential dynamic programming.  In _International Conference on Robotics and Automation (ICRA)_ , pages 1168–1175. IEEE, 2014.
  * Todorov and Li [2005] Emanuel Todorov and Weiwei Li.  A generalized iterative LQG method for locally-optimal feedback control of constrained nonlinear stochastic systems.  In _Proceedings of the American Control Conference_ , pages 300–306. IEEE, 2005.

###  -A Full $Q$ derivatives

The full $Q$ function derivatives are given by

$\begin{split}&Q^{0}_{t}=\mathcal{L}^{0}_{t}+V^{0}_{t+1},\\\
&Q^{x}_{t}=\mathcal{L}^{x}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{x}_{t+1},\\\
&Q^{u}_{t}=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{x}_{t+1},\\\
&Q^{\theta}_{t}=\mathcal{L}^{\theta}_{t}+V^{\theta}_{t+1}+(\mathbf{F}^{\theta}_{t})^{\top}V^{x}_{t+1},\\\
&\begin{aligned}
Q^{xx}_{t}&=\mathcal{L}^{xx}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{x}_{t}+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{xx}_{t}},\end{aligned}\\\
&\begin{aligned}
Q^{xu}_{t}&=\mathcal{L}^{xu}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{u}_{t}+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{xu}_{t}}=(Q^{ux}_{t})^{\top},\end{aligned}\\\
&\begin{aligned}
Q^{uu}_{t}&=\mathcal{L}^{uu}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{u}_{t}+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{uu}_{t}},\end{aligned}\\\
&\begin{aligned}
Q^{x\theta}_{t}={}&\mathcal{L}^{x\theta}_{t}+(\mathbf{F}^{x}_{t})^{\top}V^{x\theta}_{t+1}+(\mathbf{F}^{x}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}\\\
&+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{x\theta}_{t}}=(Q^{\theta
x}_{t})^{\top},\end{aligned}\\\ &\begin{aligned}
Q^{u\theta}_{t}={}&\mathcal{L}^{u\theta}_{t}+(\mathbf{F}^{u}_{t})^{\top}V^{x\theta}_{t+1}+(\mathbf{F}^{u}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}\\\
&+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{u\theta}_{t}}=(Q^{\theta
u}_{t})^{\top},\end{aligned}\\\ &\begin{aligned}
Q^{\theta\theta}_{t}={}&\mathcal{L}^{\theta\theta}_{t}+V^{\theta\theta}_{t+1}+V^{\theta
x}_{t+1}\mathbf{F}^{\theta}_{t}+(\mathbf{F}^{\theta}_{t})^{\top}V^{x\theta}_{t+1}\\\
&+(\mathbf{F}^{\theta}_{t})^{\top}V^{xx}_{t+1}\mathbf{F}^{\theta}_{t}+{\color[rgb]{1,0,0}\definecolor[named]{pgfstrokecolor}{rgb}{1,0,0}V^{x}_{t+1}\cdot\mathbf{F}^{\theta\theta}_{t}}.\end{aligned}\end{split}$
(36)

The terms highlighted in red denote contractions of the second-order
derivative dynamics tensors with the vector $V^{x}_{t+1}$. These terms are
dropped in the implementation of PDDP following iLQR [18].

###  -B Proof of Proposition 1

###### Proof.

From Eqs. 15 and 13, $\delta\boldsymbol{\theta}^{*}$ has the following form:

$\begin{split}\delta\boldsymbol{\theta}^{*}&\begin{aligned}
=\epsilon\mathbf{m}\end{aligned}\\\ &\begin{aligned}
={}&-\epsilon(\underbrace{Q^{\theta\theta}_{1}-Q^{\theta
u}_{1}(Q^{uu}_{1})^{-1}Q^{u\theta}_{1}}_{V^{\theta\theta}_{1}})^{-1}\\\
&\times(\underbrace{Q^{\theta}_{1}-Q^{\theta
u}_{1}(Q^{uu}_{1})^{-1}Q^{u}_{1}}_{V^{\theta}_{1}})\end{aligned}\\\
&\begin{aligned}
=-\epsilon(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}.\end{aligned}\end{split}$
(37)

$V^{\theta}_{1}$ and $V^{\theta\theta}_{1}$ are the gradient and Hessian of
the value function with respect to the parameters at the first timestep. The
step in Eq. 37 corresponds exactly to the direction given by an iteration of
Newton’s method for minimizing the value function at the initial time with
respect to the parameters. ∎

###  -C Proof of Lemma 1

###### Proof.

Using the expression for the cost function $\mathcal{J}$ given in Eq. 2,

$\begin{split}&\begin{aligned}
\nabla_{\mathbf{u}_{t}}\mathcal{J}&=\nabla_{\mathbf{u}_{t}}\left[\sum_{i=1}^{T}\mathcal{L}(\mathbf{x}_{i},\mathbf{u}_{i};\boldsymbol{\theta})+\phi(\mathbf{x}_{T+1};\boldsymbol{\theta})\right]\end{aligned}\\\
&\begin{aligned}
\quad={}&\mathcal{L}^{u}_{t}+\left((\mathcal{L}^{x}_{t+1})^{\top}\mathbf{F}^{u}_{t}\right)^{\top}+\ldots\\\
&+\left((\mathcal{L}^{x}_{T})^{\top}\mathbf{F}^{x}_{T-1}\ldots\mathbf{F}^{x}_{t+1}\mathbf{F}^{u}_{t}\right)^{\top}\\\
&+\left((\phi^{x}_{T+1})^{\top}\mathbf{F}^{x}_{T}\mathbf{F}^{x}_{T-1}\ldots\mathbf{F}^{u}_{t}\right)^{\top}\end{aligned}\\\
&\begin{aligned}
\quad=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}\Big{(}&\mathcal{L}^{x}_{t+1}+(\mathbf{F}^{x}_{t+1})^{\top}\mathcal{L}^{x}_{t+2}+\ldots\\\
&+(\mathbf{F}^{x}_{t+1})^{\top}\ldots(\mathbf{F}^{x}_{T-1})^{\top}\mathcal{L}^{x}_{T}\\\
&+(\mathbf{F}^{x}_{t+1})^{\top}\ldots(\mathbf{F}^{x}_{T})^{\top}\phi^{x}_{T+1}\Big{)}\end{aligned}\\\
&\begin{aligned} &\quad=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}\\\
&\times\left(\underbrace{\mathcal{L}^{x}_{t+1}+(\mathbf{F}^{x}_{t+1})^{\top}\left(\underbrace{\mathcal{L}^{x}_{t+2}+\ldots+(\mathbf{F}^{x}_{t+2})^{\top}\ldots(\mathbf{F}^{x}_{T})^{\top}\phi^{x}_{T+1}}_{\eta_{t+2}}\right)}_{\eta_{t+1}}\right)\end{aligned}\\\
&\quad=\mathcal{L}^{u}_{t}+(\mathbf{F}^{u}_{t})^{\top}\eta_{t+1}.\end{split}$

This shows Eq. 17 of Lemma 1 is true. ∎

###  -D Proof of Lemma 2

###### Proof.

From the definition of $\delta\boldsymbol{\theta}$ in Eq. 15,

$\displaystyle\delta\boldsymbol{\theta}$
$\displaystyle=\epsilon\mathbf{m}=O(\epsilon).$

Now, Eq. 19 is proven true by induction. Starting with $t=1$,

$\begin{split}\delta\mathbf{u}_{1}&=\epsilon\mathbf{k}_{1}+\mathbf{K}_{1}\underbrace{\delta\mathbf{x}_{1}}_{=0}+\underbrace{\mathbf{M}_{1}\delta\boldsymbol{\theta}}_{=O(\epsilon)}=O(\epsilon),\\\
\delta\mathbf{x}_{2}&=\mathbf{x}_{2}-\bar{\mathbf{x}}_{2}\\\
&=\mathbf{F}(\bar{\mathbf{x}}_{1},\bar{\mathbf{u}}_{1}+\delta\mathbf{u}_{1};\bar{\boldsymbol{\theta}}+\delta\boldsymbol{\theta})-\mathbf{F}(\bar{\mathbf{x}}_{1},\bar{\mathbf{u}}_{1};\bar{\boldsymbol{\theta}})\\\
&=\mathbf{F}^{u}_{1}\delta\mathbf{u}_{1}+\mathbf{F}^{\theta}_{1}\delta\boldsymbol{\theta}+\underbrace{O(\norm{\delta\mathbf{u}_{1}}_{2}^{2}+\norm{\delta\boldsymbol{\theta}}_{2}^{2})}_{=O(\epsilon^{2})}=O(\epsilon).\end{split}$

This shows Eq. 19 is true for $t=1$. Now, assume Eq. 19 holds for $t=i$,
namely $\delta\mathbf{u}_{i}=O(\epsilon)$ and
$\delta\mathbf{x}_{i+1}=O(\epsilon)$. Then, for $t=i+1$,

$\begin{split}&\delta\mathbf{u}_{i+1}=\epsilon\mathbf{k}_{i+1}+\mathbf{K}_{i+1}\delta\mathbf{x}_{i+1}+\mathbf{M}_{i+1}\delta\boldsymbol{\theta}=O(\epsilon),\\\
&\begin{aligned}
\delta\mathbf{x}_{i+2}={}&\mathbf{F}^{x}_{i+1}\delta\mathbf{x}_{i+1}+\mathbf{F}^{u}_{i+1}\delta\mathbf{u}_{i+1}+\mathbf{F}^{\theta}_{i+1}\delta\boldsymbol{\theta}\\\
&+O(\norm{\delta\mathbf{x}_{i+1}}_{2}^{2}+\norm{\delta\mathbf{u}_{i+1}}_{2}^{2}+\norm{\delta\boldsymbol{\theta}}_{2}^{2})=O(\epsilon)\end{aligned}.\end{split}$

Therefore, by induction, Eq. 19 is true for all $t=1,\ldots,T$. ∎

###  -E Proof of Proposition 2

###### Proof.

To show Eq. 20 is true, it is sufficient to show

$\displaystyle\sum_{t=1}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}$
$\displaystyle=-\epsilon\sum_{t=1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+O(\epsilon^{2}),$
(38)

with $\lambda_{t}=(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t}$ and
$\gamma_{t}=(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}$. To show Eq.
38, it is sufficient to prove using induction that

$\begin{split}\sum_{t=i}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}={}&-\epsilon\sum_{t=i}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{i}-\eta_{i})^{\top}\delta\mathbf{x}_{i}+O(\epsilon^{2})\end{split}.$
(39)

Starting at $i=T$,

$\begin{split}&(\nabla_{\mathbf{u}_{T}}\mathcal{J})^{\top}\delta\mathbf{u}_{T}\\\
&\quad=(\underbrace{\mathcal{L}^{u}_{T}+(\mathbf{F}^{u}_{T})^{\top}\phi^{x}_{T+1}}_{Q^{u}_{T}})^{\top}(\epsilon\mathbf{k}_{T}+\mathbf{K}_{T}\delta\mathbf{x}_{T}+\mathbf{M}_{T}\delta\boldsymbol{\theta})\\\
&\quad=\epsilon(Q^{u}_{T})^{\top}\mathbf{k}_{T}+(Q^{u}_{T})^{\top}\mathbf{K}_{T}\delta\mathbf{x}_{T}+(Q^{u}_{T})^{\top}\mathbf{M}_{T}\delta\boldsymbol{\theta}\\\
&\quad\begin{aligned}
={}&-\epsilon\underbrace{(Q^{u}_{T})^{\top}(Q^{uu}_{T})^{-1}Q^{u}_{T}}_{\lambda_{T}}-\epsilon\underbrace{(Q^{u}_{T})^{\top}(Q^{uu}_{T})^{-1}Q^{u\theta}_{T}}_{\gamma_{T}}\mathbf{m}\\\
&+\left[\underbrace{Q^{x}_{T}+(\mathbf{K}_{T})^{\top}Q^{u}_{T}}_{V^{x}_{T}}-Q^{x}_{T}\right]\delta\mathbf{x}_{T}\end{aligned}\\\
&\quad\begin{aligned} ={}&-\epsilon(\lambda_{T}+\gamma_{T}\mathbf{m})\\\
&+\left[V^{x}_{T}-(\underbrace{\mathcal{L}^{x}_{T}+(\mathbf{F}^{x}_{T})^{\top}\phi^{x}_{T+1}}_{\eta_{T}})\right]^{\top}\delta\mathbf{x}_{T}\end{aligned}\\\
&\quad=-\epsilon(\lambda_{T}+\gamma_{T}\mathbf{m})+(V^{x}_{T}-\eta_{T})^{\top}\delta\mathbf{x}_{T}.\end{split}$

Next, assume Eq. 39 holds for $i=k+1$, namely

$\displaystyle\sum_{t=k+1}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}={}$
$\displaystyle-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})$
$\displaystyle+(V^{x}_{k+1}-\eta_{k+1})^{\top}\delta\mathbf{x}_{k+1}+O(\epsilon^{2}).$

Then, for $i=k$,

$\displaystyle\begin{aligned}
\sum_{t=k}^{T}(\nabla\mathbf{u}_{t}\mathcal{J})^{\top}\delta\mathbf{u}_{t}=(\nabla\mathbf{u}_{k}\mathcal{J})^{\top}\delta\mathbf{u}_{k}+\sum_{t=k+1}^{T}(\nabla\mathbf{u}_{t}\mathcal{J})^{\top}\delta\mathbf{u}_{t}\end{aligned}$
$\displaystyle\begin{aligned}
={}&(\nabla\mathbf{u}_{k}\mathcal{J})^{\top}\delta\mathbf{u}_{k}-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\delta\mathbf{x}_{k+1}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&(\mathcal{L}^{u}_{k}+(\mathbf{F}^{u}_{k})^{\top}\eta_{k+1})^{\top}\delta\mathbf{u}_{k}-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\Big{[}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}+\mathbf{F}^{u}_{k}\delta\mathbf{u}_{k}\\\
&\quad+\underbrace{O(\norm{\delta\mathbf{x}_{k}}^{2}+\norm{\delta\mathbf{u}_{k}}^{2})}_{O(\epsilon^{2})}\Big{]}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&(\mathcal{L}^{u}_{k}+(\mathbf{F}^{u}_{k})^{\top}\eta_{k+1})^{\top}\delta\mathbf{u}_{k}-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{u}_{k}\delta\mathbf{u}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&(\mathcal{L}^{u}_{k}+(\mathbf{F}^{u}_{k})^{\top}\left[\cancel{\eta_{k+1}-\eta_{k+1}}+V^{x}_{k+1}\right])^{\top}\delta\mathbf{u}_{k}\\\
&-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&(\underbrace{\mathcal{L}^{u}_{k}+(\mathbf{F}^{u}_{k})^{\top}V^{x}_{k+1}}_{Q^{u}_{k}})^{\top}\delta\mathbf{u}_{k}-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&(Q^{u}_{k})^{\top}(\epsilon\mathbf{k}_{k}+\mathbf{K}_{k}\delta\mathbf{x}_{k}+\mathbf{M}_{k}\delta\boldsymbol{\theta})\\\
&-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&\epsilon(Q^{u}_{k})^{\top}\mathbf{k}_{k}+(Q^{u}_{k})^{\top}\mathbf{K}_{k}\delta\mathbf{x}_{k}+(Q^{u}_{k})^{\top}\mathbf{M}_{k}\delta\boldsymbol{\theta}\\\
&-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&-\epsilon\underbrace{(Q^{u}_{k})^{\top}(Q^{uu}_{k})^{-1}Q^{u}_{k}}_{\lambda_{k}}-\epsilon\underbrace{(Q^{u}_{k})^{\top}(Q^{uu}_{k})^{-1}Q^{u\theta}_{k}}_{\gamma_{k}}\mathbf{m}\\\
&-\epsilon\sum_{t=k+1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+(V^{x}_{k+1}-\eta_{k+1})^{\top}\mathbf{F}^{x}_{k}\delta\mathbf{x}_{k}\\\
&+(Q^{u}_{k})^{\top}\mathbf{K}_{k}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&-\epsilon\sum_{t=k}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+\Big{[}\mathcal{L}^{x}_{k}\underbrace{-\mathcal{L}^{x}_{k}-(\mathbf{F}^{x}_{k})^{\top}\eta_{k+1}}_{-\eta_{k}}\\\
&\qquad+(\mathbf{F}^{x}_{k})^{\top}V^{x}_{k+1}+\mathbf{K}_{k}^{\top}Q^{u}_{k}\Big{]}^{\top}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&-\epsilon\sum_{t=k}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+\Big{[}\underbrace{\mathcal{L}^{x}_{k}+(\mathbf{F}^{x}_{k})^{\top}V^{x}_{k+1}}_{Q^{x}_{k}}+\mathbf{K}_{k}^{\top}Q^{u}_{k}-\eta_{k}\Big{]}^{\top}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle\begin{aligned}
={}&-\epsilon\sum_{t=k}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})\\\
&+\Big{[}\underbrace{Q^{x}_{k}+\mathbf{K}_{k}^{\top}Q^{u}_{k}}_{V^{x}_{k}}-\eta_{k}\Big{]}^{\top}\delta\mathbf{x}_{k}+O(\epsilon^{2})\end{aligned}$
$\displaystyle=-\epsilon\sum_{t=k}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+(V^{x}_{k}-\eta_{k})^{\top}\delta\mathbf{x}_{k}+O(\epsilon^{2})$

This implies, by induction, that Eq. 39 holds for all $i=T,\ldots,1$. Taking
$i=1$ in Eq. 39 implies Eq. 38 is true, since $\delta\mathbf{x}_{1}=0$.

Finally, by Proposition 1, the form of Eq. 38 can be expressed as

$\begin{split}\mkern-18.0mu\sum_{t=1}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}&=-\epsilon\sum_{t=1}^{T}(\lambda_{t}+\gamma_{t}\mathbf{m})+O(\epsilon^{2})\\\
&=-\epsilon\sum_{t=1}^{T}\lambda_{t}+\epsilon\left(\sum_{t=1}^{T}\gamma_{t}\right)(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}+O(\epsilon^{2}).\end{split}$

This shows that there exists some $\epsilon$ sufficiently small such that Eq.
20 holds. ∎

###  -F Proof of Lemma 3

###### Proof.

Starting with Eq. 7 and substituting in the expressions for
$\delta\mathbf{u}_{t}^{*}$ from Eq. 14 and $\delta\boldsymbol{\theta}^{*}$
from Eq. 15 yields

$\begin{split}&\begin{aligned}
&Q(\mathbf{x}_{t},\mathbf{u}_{t}^{*};\boldsymbol{\theta}^{*})\\\
&\quad\approx\bigg{[}Q^{0}_{t}+\epsilon(Q^{u}_{t})^{\top}\mathbf{k}_{t}+\epsilon(Q^{u}_{t})^{\top}\mathbf{M}_{t}\mathbf{m}+\epsilon(Q^{\theta}_{t})^{\top}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{k}_{t}^{\top}Q^{uu}_{t}\mathbf{k}_{t}+\epsilon^{2}\mathbf{k}_{t}^{\top}Q^{uu}_{t}\mathbf{M}_{t}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}\mathbf{M}_{t}^{\top}Q^{uu}_{t}\mathbf{M}_{t}\mathbf{m}+\epsilon^{2}\mathbf{k}_{t}Q^{u\theta}_{t}\mathbf{m}\\\
&\qquad+\epsilon^{2}\mathbf{m}^{\top}\mathbf{M}_{t}^{\top}Q^{u\theta}_{t}\mathbf{m}+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}Q^{\theta\theta}_{t}\mathbf{m}\bigg{]}\\\
&\qquad+\bigg{[}(Q^{x}_{t})^{\top}+(Q^{u}_{t})^{\top}\mathbf{K}_{t}+\epsilon\mathbf{k}_{t}^{\top}Q^{ux}_{t}\\\
&\qquad+\epsilon\mathbf{m}^{\top}\mathbf{M}_{t}^{\top}Q^{ux}_{t}+\epsilon\mathbf{m}^{\top}Q^{\theta
x}_{t}+\epsilon\mathbf{k}_{t}^{\top}Q^{uu}_{t}\mathbf{K}_{t}\\\
&\qquad+\epsilon\mathbf{m}^{\top}\mathbf{M}^{\top}Q^{uu}_{t}\mathbf{K}_{t}+\epsilon\mathbf{m}^{\top}Q^{\theta
u}_{t}\mathbf{K}_{t}\bigg{]}\delta\mathbf{x}_{t}\\\
&\qquad+\frac{1}{2}\delta\mathbf{x}_{t}^{\top}\bigg{[}Q^{xx}_{t}+2Q^{xu}_{t}\mathbf{K}_{t}+\mathbf{K}_{t}^{\top}Q^{uu}_{t}\mathbf{K}_{t}\bigg{]}\delta\mathbf{x}_{t},\end{aligned}\end{split}$

where the zero-, first-, and second-order terms in $\delta\mathbf{x}_{t}$ have
been grouped together. Equating like powers in Eq. 6 gives

$\displaystyle\begin{aligned}
&V^{0}_{t}=Q^{0}_{t}+\epsilon(Q^{u}_{t})^{\top}\mathbf{k}_{t}+\epsilon(Q^{u}_{t})^{\top}\mathbf{M}_{t}\mathbf{m}+\epsilon(Q^{\theta}_{t})^{\top}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{k}_{t}^{\top}Q^{uu}_{t}\mathbf{k}_{t}+\epsilon^{2}\mathbf{k}_{t}^{\top}Q^{uu}_{t}\mathbf{M}_{t}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}\mathbf{M}_{t}^{\top}Q^{uu}_{t}\mathbf{M}_{t}\mathbf{m}+\epsilon^{2}\mathbf{k}_{t}Q^{u\theta}_{t}\mathbf{m}\\\
&\qquad+\epsilon^{2}\mathbf{m}^{\top}\mathbf{M}_{t}^{\top}Q^{u\theta}_{t}\mathbf{m}+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}Q^{\theta\theta}_{t}\mathbf{m}\end{aligned}$
$\displaystyle\begin{aligned}
&\quad=Q^{0}_{t}-\epsilon\underbrace{(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t}}_{\lambda_{t}}-\epsilon(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}\mathbf{m}\\\
&\qquad+\epsilon(Q^{\theta}_{t})^{\top}\mathbf{m}+\frac{1}{2}\epsilon^{2}\underbrace{(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t}}_{\lambda_{t}}\\\
&\qquad+\epsilon^{2}(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}\mathbf{m}+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}Q^{\theta
u}_{t}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}\mathbf{m}\\\
&\qquad-\epsilon^{2}(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}\mathbf{m}-\epsilon^{2}\mathbf{m}^{\top}Q^{\theta
u}_{t}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}Q^{\theta\theta}_{t}\mathbf{m}\end{aligned}$
$\displaystyle\begin{aligned}
&\quad=Q^{0}_{t}-\epsilon(1-\frac{1}{2}\epsilon)\lambda_{t}+\epsilon(\underbrace{Q^{\theta}_{t}-Q^{\theta
u}_{t}(Q^{uu}_{t})^{-1}Q^{u}_{t}}_{V^{\theta}_{t}})^{\top}\mathbf{m}\\\
&\qquad+\frac{1}{2}\epsilon^{2}\mathbf{m}^{\top}(\underbrace{Q^{\theta\theta}_{t}-Q^{\theta
u}_{t}(Q^{uu}_{t})^{-1}Q^{u\theta}_{t}}_{V^{\theta\theta}_{t}})\mathbf{m},\end{aligned}$

which shows the form of Eq. 21. Now let $t=1$ and substituting in the
expression for $\mathbf{m}$ given in Eq. 37 yields

$\begin{split}&\begin{aligned}
&V^{0}_{1}=Q^{0}_{1}-\epsilon(1-\frac{1}{2}\epsilon)\lambda_{1}\\\
&\qquad-\epsilon\underbrace{(V^{\theta}_{1})^{\top}(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}}_{\psi}+\frac{1}{2}\epsilon^{2}\underbrace{(V^{\theta}_{1})^{\top}(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}}_{\psi}\end{aligned}\\\
&\quad=Q^{0}_{1}-\epsilon(1-\frac{1}{2}\epsilon)(\lambda_{1}+\psi),\end{split}$

which shows Eq. 22. ∎

###  -G Proof of Proposition 3

###### Proof.

Let the cost of the nominal trajectory
$\bar{\mathbf{x}}_{t},\bar{\mathbf{u}}_{t},\bar{\boldsymbol{\theta}}$ after
iteration $k$ be $\mathcal{J}^{(k)}$. The cost $\mathcal{J}^{(k+1)}$ after
iteration $k+1$ when applying the optimal control and parameter update is
given by $V^{0}_{1}$ given in Eq. 22 plus higher-order terms, namely

$\begin{split}\mathcal{J}^{(k+1)}&=V^{0}_{1}+O(\epsilon^{3})\\\
&=Q^{0}_{1}-\epsilon(1-\frac{1}{2}\epsilon)(\lambda_{1}+\psi)+O(\epsilon^{3})\\\
&=\mathcal{L}^{0}_{1}+V^{0}_{2}-\epsilon(1-\frac{1}{2}\epsilon)(\lambda_{1}+\psi)+O(\epsilon^{3})\\\
&=\mathcal{L}^{0}_{1}+Q^{0}_{2}-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{2}\lambda_{t}+\psi\right)+O(\epsilon^{3})\\\
&=\sum_{t=1}^{2}\mathcal{L}_{t}^{0}+V_{3}^{0}-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{2}\lambda_{t}+\psi\right)+O(\epsilon^{3})\\\
&\quad\vdots\\\
&=\underbrace{\sum_{t=1}^{T}\mathcal{L}_{t}^{0}+\phi_{T+1}^{0}}_{\mathcal{J}^{(k)}}-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right)+O(\epsilon^{3})\\\
&=\mathcal{J}^{(k)}-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right)+O(\epsilon^{3}).\end{split}$

Thus,

$\begin{split}\Delta\mathcal{J}^{(k+1)}&=\mathcal{J}^{(k+1)}-\mathcal{J}^{(k)}\\\
&=-\epsilon(1-\frac{1}{2}\epsilon)\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right)+O(\epsilon^{3}).\end{split}$

∎

###  -H Proof of Theorem 1

###### Proof.

First, note that for $0<\epsilon\leq 1$,

$\begin{split}-(1-\frac{1}{2}\epsilon)\leq-\frac{1}{2}\implies-\epsilon(1-\frac{1}{2}\epsilon)\leq-\frac{1}{2}\epsilon.\end{split}$

Therefore, by Proposition 3, the cost reduction after the $k^{\text{th}}$
iteration is upper bounded by

$\displaystyle\Delta\mathcal{J}^{(k)}$
$\displaystyle\leq-\frac{1}{2}\epsilon\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right).$
(40)

Now, by Proposition 1 and Proposition 2, there exists some $0<\epsilon_{1}\leq
1$ such that for all $0<\epsilon\leq\epsilon_{1}$,

$\begin{split}\sum_{t=1}^{T}(\nabla_{\mathbf{u}_{t}}\mathcal{J})^{\top}\delta\mathbf{u}_{t}&\leq-\epsilon\sum_{t=1}^{T}\lambda_{t}+\epsilon\left(\sum_{t=1}^{T}\gamma_{t}\right)(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}\\\
\delta\boldsymbol{\theta}&=-\epsilon(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}.\end{split}$

Likewise, by Eq. 40, there exists some $0<\epsilon_{2}\leq\epsilon_{1}$ such
that for all $0<\epsilon\leq\epsilon_{2}$,

$\Delta\mathcal{J}^{(k)}\leq-\frac{1}{2}\epsilon\left(\sum_{t=1}^{T}\lambda_{t}+\psi\right),$

which implies the sequence of costs after successive iterations of PDDP is
monotonically decreasing.

To proceed, it is assumed that the space of controls and parameters is
compact. Thus, since $\mathcal{J}$ is a continuous function over a compact
space, it is bounded, so there exists some $\mathbf{U}^{*}$ and
$\boldsymbol{\theta}^{*}$ such that

$\displaystyle\lim_{k\to\infty}\mathcal{J}^{(k)}$
$\displaystyle=\mathcal{J}(\mathbf{U}^{*};\boldsymbol{\theta}^{*}),$ (41)

meaning the sequence of costs converge to a minimum. Finally, it can be shown
that $\mathbf{U}^{(k)}$ and $\boldsymbol{\theta}^{(k)}$ converge to the
minimizers.

Note that Eq. 41 implies that $\Delta\mathcal{J}^{(k)}\to 0$ as $k\to\infty$.
This further implies that for all $t=1,\ldots,T$, $\lambda_{t}\to 0$ and
$\psi\to 0$; so

$\begin{split}\lambda_{t}\to
0&\implies(Q^{u}_{t})^{\top}(Q^{uu}_{t})^{-1}Q^{u}_{t}\to 0\\\
&\implies(Q^{uu}_{t})^{-1}Q^{u}_{t}=\mathbf{k}_{t}\to\mathbf{0}\\\ \psi\to
0&\implies(V^{\theta}_{1})^{\top}(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}\to
0\\\
&\implies(V^{\theta\theta}_{1})^{-1}V^{\theta}_{1}=\mathbf{m}\to\mathbf{0}.\end{split}$

Recall $\delta\boldsymbol{\theta}=\epsilon\mathbf{m}$, so
$\delta\boldsymbol{\theta}\to\mathbf{0}$ and
$\boldsymbol{\theta}^{(k)}\to\boldsymbol{\theta}^{*}$ as $k\to\infty$. Using
induction, it is proven that for all $t=1,\ldots,T$,

$\begin{split}\delta\mathbf{u}_{t}&\to\mathbf{0}\\\
\delta\mathbf{x}_{t+1}&\to\mathbf{0},\end{split}$ (42)

as $k\to\infty$.

At time $t=1$, since the initial condition is fixed,
$\delta\mathbf{x}_{1}=\mathbf{0}$; thus

$\begin{split}\delta\mathbf{u}_{1}&=\epsilon\mathbf{k}_{1}+\mathbf{M}_{1}\delta\boldsymbol{\theta}\to\mathbf{0}\\\
\delta\mathbf{x}_{2}&=\mathbf{F}^{x}_{1}\delta\mathbf{x}_{1}+\mathbf{F}^{u}_{1}\delta\mathbf{u}_{1}+\mathbf{F}^{\theta}_{1}\delta\boldsymbol{\theta}\\\
&\quad+O(\norm{\delta\mathbf{x}_{1}}_{2}^{2}+\norm{\delta\mathbf{u}_{1}}_{2}^{2}+\norm{\delta\boldsymbol{\theta}}_{2}^{2})\to\mathbf{0}.\end{split}$

Now, assume Eq. 42 holds for $t=i$, namely

$\begin{split}\delta\mathbf{u}_{i}&\to\mathbf{0}\\\
\delta\mathbf{x}_{i+1}&\to\mathbf{0}.\end{split}$

Then, for time $t=i+1$,

$\begin{split}&\delta\mathbf{u}_{i+1}=\epsilon\mathbf{k}_{i+1}+\mathbf{K}_{i+1}\delta\mathbf{x}_{i+1}+\mathbf{M}_{i+1}\delta\boldsymbol{\theta}\to\mathbf{0}\\\
&\begin{aligned}
\delta\mathbf{x}_{i+2}={}&\mathbf{F}^{x}_{i+1}\delta\mathbf{x}_{i+1}+\mathbf{F}^{u}_{i+1}\delta\mathbf{u}_{i+1}+\mathbf{F}^{\theta}_{i+1}\delta\boldsymbol{\theta}\\\
&+O(\norm{\delta\mathbf{x}_{i+1}}_{2}^{2}+\norm{\delta\mathbf{u}_{i+1}}_{2}^{2}+\norm{\delta\boldsymbol{\theta}}_{2}^{2})\to\mathbf{0}.\end{aligned}\end{split}$

This proves Eq. 42 holds by induction, implying
$\mathbf{U}^{(k)}\to\mathbf{U}^{*}$. ∎

  *[UAM]: urban air mobility
  *[VTOL]: vertical takeoff and landing
  *[DDP]: Differential Dynamic Programming
  *[MPC]: model predictive control
  *[PDDP]: Parameterized Differential Dynamic Programming
  *[RVLT]: Revolutionary Vertical Lift Technology
  *[MHE]: moving horizon estimation
  *[iLQR]: iterative Linear-Quadratic Regulator
  *[STO-DDP]: Switching Time Optimization Differential Dynamic Programming
  *[MLE]: maximum likelihood estimation
  *[STO]: switching time optimization