# Adaptive Dual Covariance Steering with Active Parameter Estimation

Jacob W. Knaup  Panagiotis Tsiotras J. Knaup is with the School of Interactive
Computing, College of Computing and the Institute for Robotics and Intelligent
Machines, Georgia Institute of Technology, Atlanta, GA 30332–0250, USA
(e-mail: jacobk@gatech.edu)P. Tsiotras is with the School of Aerospace
Engineering and the Institute for Robotics and Intelligent Machines, Georgia
Institute of Technology, Atlanta, GA 30332–0150, USA (e-mail:
<EMAIL_ADDRESS>
###### Abstract

This work examines the optimal covariance steering problem for systems subject
to unknown parameters that enter multiplicatively with the state and control,
in addition to additive disturbances. In contrast to existing works, the
unknown parameters are modeled as random variables and are estimated online.
This work proposes the utilization of recursive least squares estimation for
efficient parameter identification. A dual control problem is formulated in
which the effect of the planned control policy on the parameter estimates is
modeled and optimized for. The parameter estimates are then used to modify the
pre-computed control policy online in an adaptive control fashion. Finally,
the proposed approach is demonstrated in a vehicle control example with
closed-loop parameter identification.

##  I Introduction

Optimal covariance steering deals with the problem of steering a stochastic
system from a given initial mean and covariance to a terminal mean and
covariance while minimizing a given performance criterion [1, 2, 3, 4].
Covariance steering has been successfully applied to a variety of practical
problems including path planning [5, 6]; differential games [7]; entry,
descent, and landing for space systems [8, 9]; and orbital operations [10, 11,
12]. While originally the problem was formulated for Gaussian distributions—in
which case the initial and terminal distributions were fully defined by the
initial and terminal means and covariances—recent results have moved towards
more general distributions with finite moments [13, 14, 15]. Moreover, in
addition to additive disturbances, recent works have also considered systems
subject to multiplicative disturbances [16, 17, 18] and unknown parameters
[19].

While control policies are typically designed for idealized models, real
physical systems, such as the vehicles and spacecraft considered in [8, 9, 10,
11, 12, 17, 19], depend on a variety of physical parameters that can be
difficult to determine exactly. Typically, the parameter identification is
performed offline and then the nominal identified parameter is taken as the
ground truth for the purposes of control design. Although adaptive control
extends this paradigm by performing the identification online, these methods,
nonetheless, still only use the nominal estimate for the purposes of control
design [20, 21, 22]. This, however, can lead to over-confidence in the control
policy and poor performance if the parameter estimate is inaccurate [23]. In
[19], we proposed an alternative paradigm in which the estimated parameter
_distribution_ is used for the purposes of control design. This method
accounts for lingering uncertainty in the true value of the parameter after
estimation, and designs a control policy that is robust to this additional
source of uncertainty. This method also extends to the case in which
parameters cannot be estimated a-priori but are known to lie in a given set.

An alternative method is to identify the parameters online in closed-loop.
This has the advantage of enabling continuous, lifelong learning of the
system’s parameters and is also applicable to systems for which generating
suitable test signals for parameter identification is difficult (e.g., because
the system must obey safety constraints while the parameters are being
identified) [23]. This gives rise to the question of dual control, which
addresses how to optimally control the system while performing identification
to balance the performance of the system’s operation and the accuracy of the
resulting parameter estimates [23, 21]. In general, there is a trade-off
between these objectives as, typically, introducing increased excitation leads
to better parameter estimates but at the expense of the performance and safety
of the system [24, 22].

This work builds on our recent results in [19] dealing with covariance
steering for systems subject to unknown parameters and additive disturbances.
The present work examines linear time-varying systems subject to additive
disturbances. Additionally, we consider that these systems are subject to
unknown parameters, modeled as random variables drawn from a known
distribution which may enter the system multiplicatively with the state and
control as well as additively. In contrast to [19], which designs a fixed
optimal control policy for the given _distribution_ of parameters, the present
work proposes to estimate the parameters online and design an optimal control
policy which _adapts_ to the parameter _estimates_.

Specifically, we formulate an optimal dual control problem which models the
effect of the control policy on the uncertainty of the parameter estimates,
and designs an optimal feedback policy which adapts to the parameter estimates
as they converge. Although the feedback policy is designed offline, the
adaptation takes place online as the data is collected; thus, feedback is
applied both with respect to the state measurement and the parameter estimate.
The parameter estimation is performed efficiently using recursive least
squares estimation which avoids the need for solving an optimization problem
online. Therefore, the resulting policy may be efficiently implemented on
embedded systems with extremely limited computational resources. This work
represents the first introduction of online learning and parameter estimation
into the covariance steering literature and, in particular, presents the first
dual control formulation of the covariance steering problem.

##  II Problem Formulation

Consider an affine time-and-parameter-varying system given by

$\displaystyle x_{k+1}$
$\displaystyle=A_{k}(p)x_{k}+B_{k}(p)u_{k}+D_{k}w_{k}+r_{k}(p),$ (1a) where,
$\displaystyle A_{k}(p)=\sum_{j=1}^{n_{p}}A_{j,k}p^{j},$ $\displaystyle\quad
B_{k}(p)=\sum_{j=1}^{n_{p}}B_{j,k}p^{j},$ (1b) $\displaystyle r_{k}(p)$
$\displaystyle=\sum_{j=1}^{n_{p}}r_{j,k}p^{j},$ (1c)

and where $p=[p^{1},\dots,p^{n_{p}}]^{\top}\in\mathbb{R}^{n_{p}}$,
$x_{k}\in\mathbb{R}^{n_{x}}$, and $u_{k}\in\mathbb{R}^{n_{u}}$. Let the
parameter $p\sim\mathcal{P}$, such that $\mathbb{E}[p]=\bar{p}$ and
$\mathbb{E}[(p-\bar{p})(p-\bar{p})^{\top}]=P\succeq\bm{0}_{n_{p}\times
n_{p}}$, and let $w_{k}\sim\mathcal{W}$ such that
$\mathbb{E}[w_{k}]=\mathbf{0}_{n_{w}}$,
$\mathbb{E}[w_{k}w_{k}^{\top}]=I_{n_{w}}$, and
$\mathbb{E}[w_{k_{1}}w_{k_{2}}^{\top}]=0_{n_{w}\times n_{w}}$ for all
$k_{1}\neq k_{2}$. It is assumed that $A_{j,k}$, $B_{j,k}$, $D_{k}$, and
$r_{j,k}$ are known for all $j=1,\dots,n_{p}$ and $k=0,\dots,N-1$. The system
dynamics are therefore affine in the parameters, and thus can also be written
as

$\displaystyle x_{k+1}$ $\displaystyle=\Gamma_{k}(x_{k},u_{k})p+D_{k}w_{k},$
(2a) where, $\displaystyle\Gamma_{k}(x,u)$
$\displaystyle=[A_{1,k}x_{k}+B_{1,k}u_{k}+r_{1,k},\dots,$ $\displaystyle\qquad
A_{n_{p},k}x_{k}+B_{n_{p},k}u_{k}+r_{n_{p},k}].$ (2b)

Let the initial conditions be given by $x_{0}\sim\mathcal{X}_{0}$, where
$\mathbb{E}[x_{0}]=\mu_{I}$ and
$\mathbb{E}[(x_{0}-\mu_{I})(x_{0}-\mu_{I})^{\top}]=\Sigma_{I}$, for
$\mu_{I}\in\mathbb{R}^{n_{x}}$ and $\Sigma_{I}\succeq\bm{0}_{n_{x}\times
n_{x}}$. We wish to steer (1a) to a given final mean
$\mu_{F}\in\mathbb{R}^{n_{x}}$ at time $N$ and regulate the terminal
covariance with respect to $\Sigma_{F}\succ\bm{0}_{n_{x}\times n_{x}}$, such
that

$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mathbb{E}[x_{N}])(x_{N}-\mathbb{E}[x_{N}])^{\top}]\preceq\Sigma_{F},$

while minimizing the cost function

$\displaystyle J(\mu_{I},\Sigma_{I};u_{0},\dots,u_{N-1})$
$\displaystyle=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(3a) where, $\displaystyle\ell_{k}(x_{k},u_{k})$
$\displaystyle=x_{k}^{\top}Q_{k}x_{k}+u_{k}^{\top}R_{k}u_{k},$ (3b) and
$Q_{k}\succeq\bm{0}_{n_{x}\times n_{x}}$, $R_{k}\succ\bm{0}_{n_{u}\times
n_{u}}$.

Thus, the problem may be summarized by designing an $N$-step control policy
$\bm{\pi}^{N}=\\{\pi_{0},\dots,\pi_{N-1}\\}$ that solves the problem

$\displaystyle\min_{\bm{\pi}^{N}}J_{N}(\bm{\pi}^{N})=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(4a) subject to $\displaystyle x_{0}\sim\mathcal{X}_{0},\quad
w_{k}\sim\mathcal{W},$ (4b) $\displaystyle
x_{k+1}=A_{k}(p_{\mathrm{gt}})x_{k}+B_{k}(p_{\mathrm{gt}})u_{k}+D_{k}w_{k}+r_{k}(p_{\mathrm{gt}}),$
(4c) $\displaystyle u_{k}=\pi_{k}(x_{k}),$ (4d)
$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mu_{F})(x_{N}-\mu_{F})^{\top}]\preceq\Sigma_{F},$
(4e)

for $k=0,1,\dots,N-1$, and where the dynamics (4c) evolve according to the
particular, unknown realization of $p_{\mathrm{gt}}\sim\mathcal{P}$.

##  III Covariance Steering Controller Design

###  III-A Existing Methods

While Problem (4) represents the ideal problem we would like to solve using
the true parameter value, it is not tractable as the realization of
$p_{gt}\sim\mathcal{P}$ is not known ahead of time, nor can it be observed
directly. One alternative is to solve the certainty equivalence problem given
by

$\displaystyle\min_{\bm{\pi}^{N}}J_{N}(\bm{\pi}^{N})=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(5a) subject to $\displaystyle x_{0}\sim\mathcal{X}_{0},\quad
w_{k}\sim\mathcal{W},$ (5b) $\displaystyle
x_{k+1}=A_{k}(\bar{p})x_{k}+B_{k}(\bar{p})u_{k}+D_{k}w_{k}+r_{k}(\bar{p}),$
(5c) $\displaystyle u_{k}=\pi_{k}(x_{k}),$ (5d)
$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mu_{F})(x_{N}-\mu_{F})^{\top}]\preceq\Sigma_{F},$
(5e)

for $k=0,1,\dots,N-1$, and where the expected value of $p$, given by
$\bar{p}$, is used in place of the true value, $p_{\mathrm{gt}}$. This is the
case, for example, when system identification is performed offline and then
the nominal estimated parameter values are taken as the true values for the
purpose of control design. However, this approach does not consider the
uncertainty in the parameters and will fail to meet the terminal constraints
in practice if $p_{\mathrm{gt}}\neq\bar{p}$.

Another alternative, similar to the approach used in [19], is to solve the
stochastic problem given by

$\displaystyle\min_{\bm{\pi}^{N}}J_{N}(\bm{\pi}^{N})=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(6a) subject to $\displaystyle x_{0}\sim\mathcal{X}_{0},\quad
w_{k}\sim\mathcal{W},\quad p\sim\mathcal{P},$ (6b) $\displaystyle
x_{k+1}=A_{k}({p})x_{k}+B_{k}({p})u_{k}+D_{k}w_{k}+r_{k}({p}),$ (6c)
$\displaystyle u_{k}=\pi_{k}(x_{k}),$ (6d)
$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mu_{F})(x_{N}-\mu_{F})^{\top}]\preceq\Sigma_{F},$
(6e)

for $k=0,1,\dots,N-1$, and where the known distribution of $p$, given by
$\mathcal{P}$, is used to propagate the uncertainty in the state dynamics. In
contrast to Problem (5), the expectations in Problem (6) are also taken over
$p\sim\mathcal{P}$ in order to account for the additional source of
uncertainty. This approach is robust to the uncertainty and will meet the
terminal constraints in practice. However, this approach is overly
conservative, as it must design a single state-feedback control policy which
statistically performs well for all possible parameter realizations.

A more optimal policy would _adapt_ to the parameter realization to
approximate the solution to (4). To this end, introduce the control policy

$\displaystyle u_{k}$ $\displaystyle=\rho_{k}(x_{k},p),$ (7)

where
$\rho_{k}(\cdot,\cdot):\mathbb{R}^{n_{x}}\times\mathbb{R}^{n_{p}}\rightarrow\mathbb{R}^{n_{u}}$
for all $k=0,1,\dots,N-1$. Such a control policy is referred to as _adaptive_
, which we define below.

######  Definition 1 (Adaptive Control [23, 25, 26])

An adaptive control system is one that automatically changes the parameters of
the control policy in order to improve performance by adjusting its behavior
to the properties of the system to be controlled.

However, policy (7) is inadmissible because the control policy depends on the
unobservable parameter realization $p$. Instead, we propose a control policy
which adapts to the _estimated_ parameter.

###  III-B Proposed Approach

We propose to estimate the unknown parameters using the method of weighted
least squares given by

$\displaystyle\min_{{p}\,\in\,\mathbb{R}^{n_{p}}}~{}\gamma^{k}({p}-\bar{p})^{\top}P^{-1}({p}-\bar{p})$
(8)
$\displaystyle+\sum_{t=0}^{k-1}\gamma^{k-t-1}(x_{t+1}-\Gamma_{k}(x_{t},u_{t}){p})^{\top}(x_{t+1}-\Gamma_{k}(x_{t},u_{t}){p}),$

for $k=0,1,\dots,N-1$, and where $P$ is the covariance of $\mathcal{P}$ and
$\gamma\in(0,1]$. If $\gamma<1$, then $\gamma$ acts as an exponentially
decaying weight and reduces the influence of the prior and older measurements
over time, ensuring the estimated parameter fits the most recent data. The
case of $\gamma=1$ is also permitted, and in this case, all observations and
the prior are weighted equally. The inclusion of the prior, which is
constructed using the first and second moments of the parameter distribution,
ensures that Problem (8) has a unique solution regardless of the available
data $x_{0},\dots,x_{k}$, and $u_{0},\dots,u_{k-1}$. The optimal parameter
estimate which solves (8) at time $k$ is given by
$\hat{p}_{k}\in\mathbb{R}^{n_{p}}$ and depends on the observations available
up to and including time $k$ as well as on the first and second moments of the
parameter distribution.

We then employ the adaptive control policy (7), and use the estimated
parameter $\hat{p}_{k}$ in place of the true parameter $p$. Thus, the control
is given by

$\displaystyle u_{k}=\rho_{k}(x_{k},\hat{p}_{k}),$ (9)

which clearly possesses the adaptive property given in Definition 1.
Additionally, we wish to design the policy (9) in such a way that it also
possesses the _dual_ control properties given in Definition 2 below,

######  Definition 2 (Adaptive Dual Control [23, 21, 24, 22])

A dual control system is one that operates under model uncertainty and
incorporates this uncertainty into the control strategy such that the control
signal has the dual properties:

  1. (i)

The system tracks the desired reference value and obeys the constraints in the
presence of the model uncertainty.

  2. (ii)

The system is excited in order to improve the estimation (by reducing the
higher order moments of the parameter error) so that the quality of the
adaptive control policy using the parameter estimate can be improved
considerably in future time intervals.

We are now ready to introduce the adaptive dual covariance steering problem,
which is given as

$\displaystyle\min_{\bm{\rho}^{N}}J_{N}(\bm{\rho}^{N})=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(10a) subject to $\displaystyle x_{0}\sim\mathcal{X}_{0},\quad
w_{k}\sim\mathcal{W},\quad p\sim\mathcal{P},$ (10b) $\displaystyle
x_{k+1}=A_{k}({p})x_{k}+B_{k}({p})u_{k}+D_{k}w_{k}+r_{k}(p),$ (10c)
$\displaystyle u_{k}=\rho_{k}(x_{k},\hat{p}_{k}),$ (10d)
$\displaystyle\hat{p}_{k}=\mathrm{argmin}_{{p}}~{}\gamma^{k}({p}-\bar{p})^{\top}P^{-1}({p}-\bar{p})$
(10e)
$\displaystyle+\sum_{t=0}^{k-1}\gamma^{k-t-1}(x_{t+1}-\Gamma_{k}(x_{t},u_{t}){p})^{\top}(x_{t+1}-\Gamma_{k}(x_{t},u_{t}){p}),$
$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mu_{F})(x_{N}-\mu_{F})^{\top}]\preceq\Sigma_{F},$
(10f)

for $k=0,1,\dots,N-1$, and where
$\bm{\rho}^{N}=\\{\rho_{0}(\cdot,\cdot),\dots,\rho_{N-1}(\cdot,\cdot)\\}$,
${p}\in\mathbb{R}^{n_{p}}$, and $\gamma\in(0,1]$.

Most adaptive control policies are based on the separation of parameter
estimation and controller design and only identify a model passively as a
byproduct. In such cases, the control law is designed using the estimated
parameters as an exact representation for the system in a certainty
equivalence fashion, without accounting for the uncertainty of estimation [20,
21]. Problem (10), on the other hand, ensures the constraints are satisfied
statistically for any parameter realization $p\sim\mathcal{P}$, regardless of
the estimate. In contrast to Problem (6) though, Problem (10) also accounts
for the online estimation of the parameter and adapts the control policy to
the parameter estimate $\hat{p}_{k}$. Thus, the solution is an adaptive _dual_
control policy, as per Definition 2.

###  III-C Tractable Formulation

The solution to Problem (10) depends on optimizing over the space of $N$
arbitrary (infinite-dimensional) control policies which each depend on the
estimated parameters at that time step. The estimated parameters, in-turn,
depend on the solution of $N$ quadratic programs. We address these
computational issues by first introducing an analytical, recursive solution to
the quadratic program (8). Then, we introduce a finite-dimensional
approximation of $\rho_{k}(\cdot,\cdot)$.

The solution to Problem (8) requires storing the entire history of the
observations, which grows with $k$, making the approach impractical for large
time horizons. To resolve this issue, we utilize recursive least squares,
given by the following lemma.

######  Lemma 1 (Recursive Least Squares [27])

The solution to Problem (8) is given by

$\displaystyle\hat{p}_{k+1}=\hat{p}_{k}+P_{k+1}\Gamma_{k}(x_{k},u_{k})^{\top}(x_{k+1}-\Gamma_{k}(x_{k},u_{k})\hat{p}_{k}),$
(11a) $\displaystyle
P_{k+1}=\frac{1}{\gamma}P_{k}-\frac{1}{\gamma}P_{k}\Gamma_{k}(x_{k},u_{k})^{\top}$
$\displaystyle\;\;(\gamma
I+\Gamma_{k}(x_{k},u_{k})P_{k}\Gamma_{k}(x_{k},u_{k})^{\top})^{-1}\Gamma_{k}(x_{k},u_{k})P_{k},$
(11b)

for $k=0,1,\dots,N-1$, where $\hat{p}_{0}=\bar{p}$ and $P_{0}=P$.

###### Proof:

The result follows immediately from Theorem 1 of [27]. ∎

Lemma 1 allows for the parameter to be updated using only the two most recent
data points. Thus, the sizes of the matrices and vectors in (11) are fixed and
do not depend on $k$. For more details on recursive least squares, see, for
example, [28, 29].

Having rendered the parameter estimation tractable, we turn to the control
policy parameterization. Since optimizing over arbitrary feedback policies
amounts to an intractable infinite dimensional optimization problem, in
practice, prior works, such as [1, 30, 19], introduce to Problems (4), (5),
(6) the affine state-feedback policy parameterization given by

$\displaystyle\pi_{k}(x_{k})$ $\displaystyle=v_{k}+L_{k}x_{k},$ (12)

where $v_{k}\in\mathbb{R}^{n_{u}}$ and $L_{k}\in\mathbb{R}^{n_{u}\times
n_{x}}$ for $k=0,1,\dots,N-1$. The affine state feedback parameterization
given by (12) is reasonable due to the following lemma.

######  Lemma 2 ([16])

The optimal solution for the exact covariance steering (equality constrained)
version of Problem (4) is given by

$\displaystyle\pi^{\ast}(x_{k})=\rho^{\ast}_{k}(x_{k},p_{\mathrm{gt}})=\tilde{v}^{\ast}_{k}(p_{\mathrm{gt}})+\tilde{L}^{\ast}_{k}(p_{\mathrm{gt}})x_{k},$
(13)

where
$\tilde{v}^{\ast}_{k}(\cdot):\mathbb{R}^{n_{p}}\rightarrow\mathbb{R}^{n_{u}}$
and
$\tilde{L}^{\ast}_{k}(\cdot):\mathbb{R}^{n_{p}}\rightarrow\mathbb{R}^{n_{u}\times
n_{x}}$ for $k=0,1,\dots,N-1$, and where $\tilde{v}^{\ast}_{k}(\cdot)$ and
$\tilde{L}^{\ast}_{k}(\cdot)$ are given in Theorem 3 of [16]. Furthermore, the
result also holds for Problem (5), using $\bar{p}$ in place of
$p_{\mathrm{gt}}$.

###### Proof:

The result follows from Theorem 3 of [16], with the following considerations.
While [16] presents the optimal policy as $u_{k}=K_{k}(x_{k}-\mu_{k})+z_{k}$,
one may observe that the mean sequence is deterministic and thus it may be
incorporated into the feed-forward gain, to arrive at
$\tilde{L}^{\ast}_{k}(\cdot)=K_{k}$ and
$\tilde{v}^{\ast}_{k}(\cdot)=z_{k}-K_{k}\mu_{k}$. Additionally, observe the
expressions for the optimal values of $z_{k}$ and $K_{k}$ given in Theorem 3
of [16] depend on the initial moments as well as the system matrices, which
in-turn, depend on the system parameter realization, hence the mappings
$\tilde{v}^{\ast}_{k}(p_{\mathrm{gt}})$ and
$\tilde{L}^{\ast}_{k}(p_{\mathrm{gt}})$. ∎

Thus, as Lemma 2 illustrates, the optimal control policy depends on the system
parameter realization which is unknown a-priori at the time the control policy
is designed in the cases of Problems (6) and (10). Therefore, the
parameterization proposed in (7) and (9) is reasonable. As optimizing over
functions is intractable, we replace (9) with a first-order approximation of
the optimal policy presented in Lemma 2, given by

$\displaystyle\rho_{k}(x_{k},\hat{p}_{k})$
$\displaystyle=v_{k}^{0}+L_{k}^{0}x_{k}+\sum_{j=1}^{n_{p}}v_{k}^{j}\hat{p}_{k}^{j}+L_{k}^{j}\hat{p}_{k}^{j}x_{k},$
(14)

where $v_{k}^{j}\in\mathbb{R}^{n_{u}}$ and
$L_{k}^{j}\in\mathbb{R}^{n_{u}\times n_{x}}$ for $k=0,1,\dots,N-1$ and
$j=0,1,\dots,n_{p}$.

Thus, we have an alternative to Problem (10), given by

$\displaystyle\min_{\bm{v}^{N},\bm{L}^{N}}J_{N}(\bm{v}^{N},\bm{L}^{N})=\mathbb{E}\left[\sum_{k=0}^{N-1}\ell_{k}(x_{k},u_{k})\right],$
(15a) subject to $\displaystyle
x_{0}\sim\mathcal{X}_{0},\;\;w_{k}\sim\mathcal{W},\;\;p\sim\mathcal{P},\;\;\hat{p}_{0}=\bar{p},\;\;P_{0}=P,$
(15b) $\displaystyle
x_{k+1}=A_{k}({p})x_{k}+B_{k}({p})u_{k}+D_{k}w_{k}+r_{k}({p}),$ (15c)
$\displaystyle
u_{k}=v_{k}^{0}+L_{k}^{0}x_{k}+\sum_{j=1}^{n_{p}}v_{k}^{j}\hat{p}_{k}^{j}+L_{k}^{j}\hat{p}_{k}^{j}x_{k},$
(15d)
$\displaystyle\hat{p}_{k+1}=\hat{p}_{k}+P_{k+1}\Gamma_{k}(x_{k},u_{k})^{\top}(x_{k+1}-\Gamma_{k}(x_{k},u_{k})\hat{p}_{k}),$
(15e) $\displaystyle
P_{k+1}=\frac{1}{\gamma}P_{k}-\frac{1}{\gamma}P_{k}\Gamma_{k}(x_{k},u_{k})^{\top}$
$\displaystyle\;\;(\gamma
I+\Gamma_{k}(x_{k},u_{k})P_{k}\Gamma_{k}(x_{k},u_{k})^{\top})^{-1}\Gamma_{k}(x_{k},u_{k})P_{k},$
(15f)
$\displaystyle\mathbb{E}[x_{N}]=\mu_{F},\quad\mathbb{E}[(x_{N}-\mu_{F})(x_{N}-\mu_{F})^{\top}]\preceq\Sigma_{F},$
(15g)

for $k=0,1,\dots,N-1$, and where
$\bm{v}^{N}=\\{v^{j}_{k}\\}_{k=0,j=0}^{N-1,n_{p}}$ and
$\bm{L}^{N}=\\{L_{k}^{j}\\}_{k=0,j=0}^{N-1,n_{p}}$. Problem (15) replaces the
batch least squares estimation defined in (10e) with the recursive least
squares estimation (15e)-(15f), and Problem (15) parameterizes the arbitrary
feedback policies $\bm{\rho}^{N}$ in terms of $\bm{v}^{N}$ and $\bm{L}^{N}$.

###  III-D Sample Average Approximation

As discussed in [19], analytically propagating the state uncertainty through
(15c) is challenging due to the dependence between $x_{k}$ and $p$ for all
$k>0$. In [19], we derived analytical expressions for the propagation of the
moments for a specific class of problems subject to constant parametric
uncertainty, similar to Problem (6). In our current work, the moment dynamics
are further complicated by the online estimation of the parameters and the
feedback in the control policy on the parameter estimates. As a result, in
this work, we take an alternative approach and utilize a Monte Carlo sampling-
based approximation to propagate the uncertainty.

The approximation of Problem (15) is given by

$\displaystyle\min_{\bm{v}^{N},\bm{L}^{N},V}J_{N}^{M}(\bm{v}^{N},\bm{L}^{N})=\sum_{i=1}^{M}\sum_{k=0}^{N-1}\ell_{k}(x_{k}^{i},u_{k}^{i}),$
(16a) subject to $\displaystyle
x_{0}^{i}\sim\mathcal{X}_{0},\;\;w_{k}^{i}\sim\mathcal{W},\;\;p^{i}\sim\mathcal{P},\;\;\hat{p}_{0}^{i}=\bar{p},\;\;P_{0}^{i}=P,$
(16b) $\displaystyle
x_{k+1}^{i}=A_{k}(p^{i})x_{k}^{i}+B_{k}({p}^{i})u_{k}^{i}+D_{k}w_{k}^{i}+r_{k}(p^{i}),$
(16c) $\displaystyle
u_{k}^{i}=v_{k}^{0}+L_{k}^{0}x_{k}^{i}+\sum_{j=1}^{n_{p}}v_{k}^{j}\hat{p}_{k}^{i,j}+L_{k}^{j}\hat{p}_{k}^{i,j}x_{k}^{i},$
(16d)
$\displaystyle\hat{p}_{k+1}^{i}=\hat{p}_{k}^{i}+P_{k+1}^{i}\Gamma_{k}(x_{k}^{i},u_{k}^{i})^{\top}(x_{k+1}^{i}-\Gamma_{k}(x_{k}^{i},u_{k}^{i})\hat{p}_{k}^{i}),$
(16e) $\displaystyle
P_{k+1}^{i}=P_{k}^{i}-P_{k}^{i}\Gamma_{k}(x_{k}^{i},u_{k}^{i})^{\top}$
$\displaystyle\quad(I+\Gamma_{k}(x_{k}^{i},u_{k}^{i})P_{k}^{i}\Gamma_{k}(x_{k}^{i},u_{k}^{i})^{\top})^{-1}\Gamma_{k}(x_{k}^{i},u_{k}^{i})P_{k}^{i},$
(16f) $\displaystyle|\sum_{i=1}^{M}x_{N}^{i}/M-\mu_{F}|\leq\Delta_{\mu},$
(16g)
$\displaystyle|\mathtt{vec}(\sum_{i=1}^{M}(x_{N}^{i}-\mu_{F})(\star)^{\top}/M+VV^{T}-\Sigma_{F})|\leq\Delta_{\Sigma},$
(16h)

for $i=1,\dots,M$, $k=0,1,\dots,N-1$, and where $(\star)$ represents repeated
terms,
$\hat{p}_{k}^{i}=[\hat{p}_{k}^{i,1},\dots,\hat{p}_{k}^{i,n_{p}}]^{\top}$,
$\hat{p}_{k}^{i,j}\in\mathbb{R}$ for $j=1,\dots,n_{p}$, $\mathtt{vec}(\cdot)$
is the vector (flattening) operator, $V\in\mathbb{R}^{n_{x}\times n_{x}}$, and
$\Delta_{\mu}\in\mathbb{R}^{n_{x}}$ and
$\Delta_{\Sigma}\in\mathbb{R}^{n_{x}n_{x}}$ are small quantities. For details
on $V$, $\Delta_{\mu}$, and $\Delta_{\Sigma}$, refer to Remark 1. Problem (16)
approximates Problem (15) using $M$ Monte Carlo trials. The use of Monte Carlo
sampling in this context is referred to as the sample average approximation
(SAA), a well-established technique in the field of stochastic optimization
[31]. For results on the convergence and feasibility of SAA, see, for example,
[31, 32].

###### Remark 1

As most nonlinear programming (NLP) solvers do not support semidefinite
constraints, we add the slack variable $V$ to convert the semidefinite-
constrained NLP (15) to an equality-constrained NLP, which is then relaxed to
the inequality-constrained NLP given by (16). Note that
$VV^{\top}\succeq\bm{0}_{n_{x}\times n_{x}}$ for all
$V\in\mathbb{R}^{n_{x}\times n_{x}}$, thus the relaxation using the slack
variable $V$ is lossless. As recommended in [32], we relax the equality
constraints to inequalities allowing for a small margin of error, given by
$\Delta_{\mu}$ and $\Delta_{\Sigma}$, dependent on the sample size.

We are now ready to introduce the following theorem concerning the proposed
adaptive dual covariance steering method.

###### Theorem 1

Problem (16) preserves the dual control effect given in Definition 2. That is,
the role of the control actions in affecting the parameter estimate is
accounted for in the problem formulation, and the control policy adapts to the
estimated parameters so that the following dual control properties are
present.

  1. (i)

The control policy minimizes the performance criteria and satisfies the
terminal constraints for $p\sim\mathcal{P}$.

  2. (ii)

The control policy affects the parameter uncertainty which is exploited to
adapt the control policy using the improved parameter estimate at later time
steps.

###### Proof:

Satisfaction of property (i) may be seen from the dynamics (16c), in which,
the future empirical state distributions $\\{x_{k}^{i}\\}_{k=0,i=1}^{N,M}$
depend on the parameter realizations $\\{p^{i}\\}_{i=1}^{M}$ sampled from the
distribution $\mathcal{P}$. The cost and constraint equations (16a), (16g),
(16h), then enforce the objective function and terminal constraints on the
empirical state distributions $\\{x_{k}^{i}\\}_{k=0,i=1}^{N,M}$. Note that the
parameter estimate only enters into the problem as a source of feedback
through the control policy. This is in contrast to methods that enforce the
constraints using the estimated parameter and which may not provide the
desired performance in practice.

Satisfaction of property (ii) may be seen from the effect of the control on
the parameter estimate in (16e)-(16f). Specifically, note that, as per (16b),
$p_{0}^{i}=\bar{p}$ and $P_{0}^{i}=P$, for all $i=1,\dots,M$, but for $k>0$,
$p_{k}^{i}$ and $P_{k}^{i}$ depend on the previous controls $u_{t}^{i}$ where
$0\leq t<k$. Therefore, the control policy may improve the parameter estimate
and reduce the corresponding uncertainty. The improvement in the parameter
estimate at later time steps is then exploited through the control policy
(16d), which allows for more useful adaptation as $\hat{p}_{k}^{i}$ approaches
the true realization of $p^{i}$. ∎

##  IV Numerical Example

We compare the proposed approach (16) against the non-adaptive method given in
[19] in an example of controlling a vehicle with uncertainty in the steering
column dynamics. The equations of motion for the nonlinear kinematic bicycle
model, shown in Fig. 1, are given by the following

$\displaystyle\dot{e}_{\psi}$
$\displaystyle=\dot{\psi}-\dot{\psi}_{\mathrm{ref}}+\dot{\nu}_{y}/\nu_{x},$
(17a) $\displaystyle\dot{e}_{y}$
$\displaystyle=\nu_{y}\cos{e_{\psi}}+\nu_{x}\sin{e_{\psi}},$ (17b)
$\displaystyle\dot{s}$
$\displaystyle=(\nu_{x}\cos{e_{\psi}}-\nu_{y}\sin{e_{\psi}})/(1-e_{y}\sigma),$
(17c)

where $\nu_{y}=\nu_{x}\delta\frac{\ell_{r}}{\ell_{f}+\ell_{r}}$,
$\dot{\psi}=\tan{\delta}\frac{\nu_{x}}{\ell_{f}+\ell_{r}}$, and where
$e_{\psi}$ is the heading error with respect to the reference path, $\psi$ is
the vehicle’s absolute heading, $\psi_{\mathrm{ref}}$ is the heading of the
reference path, $\nu_{y}$ is the vehicle’s lateral velocity, $\nu_{x}$ is the
vehicle’s longitudinal velocity, $e_{y}$ is the vehicle’s lateral error with
respect to the reference path, $s$ is the vehicle’s longitudinal position with
respect to the reference path, $\sigma$ is the curvature of the reference
path, $\delta$ is the front steering angle, and $\ell_{f}$ and $\ell_{r}$ are
the locations of the vehicle’s center of mass.

Figure 1: Kinematic bicycle model in curvilinear coordinates.

Given a constant velocity $\nu_{x}$ and curvature $\sigma$, and assuming
$\delta$ and $e_{\psi}$ remain small, a linear approximation for the lateral
motion is given by

$\displaystyle\dot{e}_{\psi}$
$\displaystyle=\frac{\nu_{x}}{\ell_{f}+\ell_{r}}\delta-\nu_{x}\sigma+\dot{\delta}\frac{\ell_{r}}{\ell_{f}+\ell_{r}},$
(18a) $\displaystyle\dot{e}_{y}$
$\displaystyle=\frac{\ell_{r}}{\ell_{f}+\ell_{r}}\nu_{x}\delta+\nu_{x}e_{\psi}.$
(18b)

We define the state $x=[\delta,e_{\psi},e_{y}]^{\top}$ and the control
$u=\dot{\delta}/p_{\dot{\delta}}$, and use Euler integration with a time-step
of $\Delta t=0.2$ sec to obtain the affine system

$\displaystyle x_{k+1}$
$\displaystyle=Ax_{k}+B(p_{\dot{\delta}})u_{k}+Dw_{k}+r,$ (19a) where,
$\displaystyle A$ $\displaystyle=\begin{bmatrix}1&0&0\\\
\frac{\nu_{x}}{\ell_{f}+\ell_{r}}\Delta t&1&0\\\
\frac{\ell_{r}}{\ell_{f}+\ell_{r}}\nu_{x}\Delta t&\nu_{x}\Delta
t&1\end{bmatrix},~{}~{}B(p_{\dot{\delta}})=\begin{bmatrix}p_{\dot{\delta}}\Delta
t\\\ p_{\dot{\delta}}\frac{\ell_{r}}{\ell_{f}+\ell_{r}}\Delta t\\\
0\end{bmatrix},$ $\displaystyle D$
$\displaystyle=\begin{bmatrix}\theta_{\delta}\Delta t&0&0\\\
0&\theta_{\psi}\Delta t&0\\\ 0&0&\theta_{y}\Delta t\end{bmatrix},\quad
r=\begin{bmatrix}0\\\ -\sigma\nu_{x}\Delta t\\\ 0\\\ \end{bmatrix},$ (19b)

where we have added the additive noise term to account for errors owing to the
linear approximation.

We compared the proposed approach (16) with the method presented in [19],
which solves Problem (6) using policy parameterization (12), as a baseline. We
used CasADi [33] and IPOPT [34] to formulate and solve the NLP given by
Problem (16). The two methods are evaluated with additive Gaussian
disturbances and with parameters drawn from the following distributions shown
in Fig. 2: Gaussian, uniform, Beta, and a mixture of two Gaussians. Table I
presents a comparison of the proposed approach (Adaptive Dual) vs. the
baseline method [19] (Static Robust) for different parameter distributions and
terminal covariance constraints. The performance is measured as the average
cost over 1,000 Monte Carlo trials and normalized using the average cost of
the proposed method for the given problem data. The terminal covariance is
given by
$\Sigma_{F}=\mathtt{diag}(0.01,\Sigma_{F}^{\theta},\Sigma_{F}^{\theta})$.

Figure 2: Parameter distributions. TABLE I: Average Cost Comparison for Various Parameter Distributions and Terminal Constraints Control Formulation | Static Robust | Adaptive Dual
---|---|---
Gaussian |  |
$\Sigma_{F}^{\theta}=10^{-4}$ | $1.03$ | $1.0$
Uniform |  |
$\Sigma_{F}^{\theta}=10^{-3}$ | $1.41$ | $1.0$
Uniform |  |
$\Sigma_{F}^{\theta}=10^{-4}$ | $\infty$ | $1.0$
Beta |  |
$\Sigma_{F}^{\theta}=10^{-3}$ | $1.62$ | $1.0$
Beta |  |
$\Sigma_{F}^{\theta}=10^{-4}$ | $\infty$ | $1.0$
Gaussian Mixture |  |
$\Sigma_{F}^{\theta}=10^{-4}$ | $1.09$ | $1.0$

It may be seen in Table I that the proposed adaptive dual covariance steering
method is able to steer the state distribution to the prescribed terminal
constraints while incurring a significantly lower average cost than the
baseline static robust covariance steering method for the uniform and Beta
distributions when $\Sigma_{F}^{\theta}=10^{-3}$. Moreover, when
$\Sigma_{F}^{\theta}=10^{-4}$, the proposed method is still able to meet the
constraint using adaptive control, whereas the baseline static method is not
able to find a solution that will satisfy the terminal constraint for the
uniform and Beta distributions. For the Gaussian distribution and mixture of
Gaussians, the proposed adaptive dual covariance steering method still incurs
a lower cost, but the benefits are less significant, and both the proposed and
baseline methods are able to meet the terminal constraint when
$\Sigma_{F}^{\theta}=10^{-4}$. These results may be interpreted through the
entropy of the distributions shown in Fig. 2. The Gaussian and bimodal mixture
of Gaussians have less entropy and the parameter value is likely to lie around
one or two values, respectively. Thus, the static robust method is able to use
the statistical information about these distributions to design a static
control policy which statistically performs well for most parameter
realizations. However, the uniform and beta distributions have higher entropy,
causing the advantages of an adaptive control policy to become more apparent.

Fig. 3 shows sampled realized trajectories when a control policy generated by
solving Problem (16) using the proposed method (blue) is compared with a
baseline static control policy (red). The baseline static control policy is
designed to robustly minimize the cost and satisfy the terminal constraints
for parameters sampled from the known prior distribution. The proposed method,
on the other hand, also guarantees robust satisfaction of the terminal
constraints, but additionally adapts the pre-computed control policy online in
order to adjust to the changing parameter estimate and achieve a lower cost.

Figure 3: Sampled vehicle trajectories. Figure 4: Terminal constraint
visualization.

Although the trajectories for both vehicles in Fig. 3 appear similar, as seen
in Fig. 4, the proposed adaptive dual covariance steering is able to
successfully achieve smaller terminal covariance targets than the baseline
static robust covariance steering by adapting the control policy to the
parameter estimate online and, thus, achieves a lower average cost.

##  V Conclusion

In this paper, we formulated a dual control variation of the covariance
steering problem for systems subject to unknown parameters. The adaptive dual
covariance steering problem includes online parameter identification and a
feedback policy parameterization which enables online adaptation to the
estimated parameters while still ensuring constraint satisfaction in practice.
We showed that the proposed method preserves the dual control property of
encoding the control policy’s effect on reducing model uncertainty through the
parameter estimation. We applied the proposed adaptive dual covariance
steering approach to a vehicle control example and demonstrated it is able to
outperform a baseline covariance steering method for systems subject to
parametric uncertainty but which does not include adaptive dual control.

## References

  * [1] M. Goldshtein and P. Tsiotras, “Finite-horizon covariance control of linear time-varying systems,” in _IEEE 56th Annual Conference on Decision and Control (CDC)_ , Melbourne, Australia, Dec. 12–15, 2017, pp. 3606–3611.
  * [2] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution, part I,” _IEEE Transactions on Automatic Control_ , vol. 61, no. 5, pp. 1158–1169, 2015.
  * [3] ——, “Optimal steering of a linear stochastic system to a final probability distribution, part II,” _IEEE Transactions on Automatic Control_ , vol. 61, no. 5, pp. 1170–1180, 2015.
  * [4] ——, “Optimal steering of a linear stochastic system to a final probability distribution—part III,” _IEEE Transactions on Automatic Control_ , vol. 63, no. 9, pp. 3112–3118, 2018.
  * [5] K. Okamoto and P. Tsiotras, “Optimal stochastic vehicle path planning using covariance steering,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 3, pp. 2276–2281, 2019.
  * [6] D. Zheng, J. Ridderhof, P. Tsiotras, and A. Agha-mohammadi, “Belief space planning: A covariance steering approach,” in _International Conference on Robotics and Automation_ , Philadelphia, PA, May 23–27, 2022, pp. 11 051–11 057.
  * [7] Y. Chen, T. T. Georgiou, and M. Pavon, “Covariance steering in zero-sum linear-quadratic two-player differential games,” in _IEEE 58th Conference on Decision and Control (CDC)_ , Nice, France, Dec. 11–13, 2019, pp. 8204–8209.
  * [8] J. Ridderhof and P. Tsiotras, “Uncertainty quantification and control during mars powered descent and landing using covariance steering,” in _AIAA Guidance, Navigation, and Control Conference_ , Kissimmee, FL, Jan. 8–12, 2018, p. 0611.
  * [9] R. Goyal, M. Majji, and R. E. Skelton, “Optimal actuator/sensor precision for covariance steering with soft convex constraints on state and control,” in _IEEE American Control Conference_ , New Orleans, LA, May 25–28, 2021, pp. 5015–5022.
  * [10] J. Ridderhof and P. Tsiotras, “Chance-constrained covariance steering in a Gaussian random field via successive convex programming,” _Journal of Guidance, Control, and Dynamics_ , vol. 45, no. 4, pp. 599–610, 2022.
  * [11] B. Benedikter, A. Zavoli, Z. Wang, S. Pizzurro, and E. Cavallini, “Convex approach to covariance control with application to stochastic low-thrust trajectory optimization,” _Journal of Guidance, Control, and Dynamics_ , vol. 45, no. 11, pp. 2061–2075, 2022.
  * [12] ——, “Covariance control for stochastic low-thrust trajectory optimization,” in _AIAA SCITECH 2022 Forum_ , San Diego, CA, Jan. 3–7, 2022, p. 2474.
  * [13] F. Liu and P. Tsiotras, “Optimal covariance steering for continuous-time linear stochastic systems with additive generic noise,” _arXiv preprint arXiv:2206.11201_ , 2022.
  * [14] J. Ridderhof, K. Okamoto, and P. Tsiotras, “Nonlinear uncertainty control with iterative covariance steering,” in _IEEE 58th Conference on Decision and Control (CDC)_ , Nice, France, Dec. 11–13, 2019, pp. 3484–3490.
  * [15] Y. Chen, “Covariance steering for nonlinear control-affine systems,” _arXiv preprint arXiv:2108.09530_ , 2021.
  * [16] F. Liu and P. Tsiotras, “Optimal covariance steering for continuous-time linear stochastic systems with multiplicative noise,” _arXiv preprint arXiv:2206.11735_ , 2022.
  * [17] J. W. Knaup and P. Tsiotras, “Computationally efficient covariance steering for systems subject to parametric disturbances and chance constraints,” in _IEEE 62nd Conference on Decision and Control (CDC)_ , Singapore, Dec. 13–15, 2023, pp. 1796–1801.
  * [18] I. M. Balci and E. Bakolas, “Covariance steering of discrete-time linear systems with mixed multiplicative and additive noise,” in _IEEE American Control Conference (ACC)_ , San Diego, CA, May 31–June 2, 2023, pp. 2586–2591.
  * [19] J. W. Knaup and P. Tsiotras, “Covariance steering for systems subject to unknown parameters,” in _IEEE 62nd Conference on Decision and Control (CDC)_ , Singapore, Dec. 13–15, 2023, pp. 1790–1795.
  * [20] R. Soloperto, J. Köhler, M. A. Müller, and F. Allgöwer, “Dual adaptive mpc for output tracking of linear systems,” in _IEEE 58th Conference on Decision and Control (CDC)_ , Nice, France, Dec. 11–13, 2019, pp. 1377–1382.
  * [21] N. M. Filatov and H. Unbehauen, “Survey of adaptive dual control methods,” _IEE Proceedings-Control Theory and Applications_ , vol. 147, no. 1, pp. 118–128, 2000.
  * [22] A. Mesbah, “Stochastic model predictive control with active uncertainty learning: A survey on dual control,” _Annual Reviews in Control_ , vol. 45, pp. 107–117, 2018.
  * [23] H. Unbehauen, “Adaptive dual control systems: a survey,” in _Proceedings of the IEEE Adaptive Systems for Signal Processing, Communications, and Control Symposium_ , Lake Louise, AB, Canada, Oct. 4, 2000, pp. 171–180.
  * [24] Y. Bar-Shalom and E. Tse, “Dual effect, certainty equivalence, and separation in stochastic control,” _IEEE Transactions on Automatic Control_ , vol. 19, no. 5, pp. 494–500, 1974.
  * [25] D. Seborg, T. F. Edgar, and S. Shah, “Adaptive control strategies for process control: a survey,” _AIChE Journal_ , vol. 32, no. 6, pp. 881–913, 1986.
  * [26] R. Isermann, “Parameter adaptive control algorithms—a tutorial,” _Automatica_ , vol. 18, no. 5, pp. 513–528, 1982.
  * [27] S. A. U. Islam and D. S. Bernstein, “Recursive least squares for real-time implementation [lecture notes],” _IEEE Control Systems Magazine_ , vol. 39, no. 3, pp. 82–85, 2019.
  * [28] Y. Liu and F. Ding, “Convergence properties of the least squares estimation algorithm for multivariable systems,” _Applied Mathematical Modelling_ , vol. 37, no. 1-2, pp. 476–483, 2013.
  * [29] R. Isermann and M. Münchhof, _Identification of Dynamic Systems: An Introduction with Applications_. Springer, 2011, vol. 85.
  * [30] G. Rapakoulias and P. Tsiotras, “Discrete-time optimal covariance steering via semidefinite programming,” in _IEEE 62nd Conference on Decision and Control (CDC)_ , Singapore, Dec. 13–15, 2023, pp. 1802–1807.
  * [31] A. Shapiro, D. Dentcheva, and A. Ruszczynski, _Lectures on Stochastic Programming: Modeling and Theory_. SIAM, 2021.
  * [32] T. Lew, R. Bonalli, and M. Pavone, “Sample average approximation for stochastic programming with equality constraints,” _arXiv preprint arXiv:2206.09963_ , 2022.
  * [33] J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” _Mathematical Programming Computation_ , vol. 11, no. 1, pp. 1–36, 2019.
  * [34] A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” _Mathematical programming_ , vol. 106, pp. 25–57, 2006.