Feedback Interconnected Mean-Field Density Estimation and Control

For a vector $x\in\mathbb{R}^{n}$, its Euclidean norm is denoted by $\|x\|$. Let $E\subset\mathbb{R}^{n}$ be a measurable set. For $f:E\to\mathbb{R}$, its $L^{2}$-norm is denoted by $\|f\|_{L^{2}(E)}:=(\int_{E}|f(x)|^{2}dx)^{1/2}$. Given $g(x)$ with $0<\inf_{x\in E}g(x)\leq\sup_{x\in E}g(x)<\infty$, the weighted norm $\|f\|_{L^{p}(E;g)}:=(\int_{E}|f(x)|^{p}g(x)dx)^{1/p}$ is equivalent to $\|f\|_{L^{p}(E)}$. We will omit $E$ in the notation when it is clear. The gradient and Laplacian of a scalar function $f$ are denoted by $\nabla f$ and $\Delta f$, respectively The divergence of a vector field $F$ is denoted by $\nabla\cdot F$.

We introduce ISS for infinite-dimensional systems [15]. Define the following classes of comparison functions:

Let $\left(X,\|\cdot\|_{X}\right)$ and $\left(U,\|\cdot\|_{U}\right)$ be the state and input space, endowed with norms $\|\cdot\|_{X}$ and $\|\cdot\|_{U}$, respectively. Denote $U_{c}=PC(\mathbb{R}_{+};U)$, the space of piecewise right-continuous functions from $\mathbb{R}_{+}$ to $U$, equipped with the sup-norm. Consider a control system $\Sigma=(X,U_{c},\phi)$ where $\phi:\mathbb{R}_{+}\times X\times U_{c}\to X$ is a transition map. Let $x(t)=\phi(t,x_{0},u)$.

ISS is useful for studying the stability of cascade systems. Consider two systems $\Sigma_{i}=(X_{i},U_{ci},\phi_{i}),i=1,2$, where $U_{ci}=PC(\mathbb{R}_{+};U_{i})$ and $X_{1}\subset U_{2}$. We say they form a cascade connection if $u_{2}(t)=\phi_{1}(t,t_{0},\phi_{01},u_{1})$.

We introduce the infinite-dimensional Kalman filters presented in [17]. Let $\mathcal{H},\mathcal{K}$ be real Hilbert spaces. Consider the following infinite-dimensional linear system:

where $\mathcal{A}(t)$ is a linear closed operator on $\mathcal{H}$, $\mathcal{B}(\cdot)\in L^{\infty}([0,T];\mathcal{L}(\mathcal{H}))$, $\mathcal{C}(\cdot)\in L^{\infty}([0,T];\mathcal{L}(\mathcal{H},\mathcal{K}))$, and $\mathcal{F}(\cdot),\mathcal{F}(\cdot)^{-1}\in L^{\infty}([0,T];\mathcal{L}(\mathcal{K}))$. $w(t)$ and $v(t)$ are independent Wiener processes on $\mathcal{H}$ and $\mathcal{K}$ with covariance operators $\mathcal{W}$ and $\mathcal{V}$, respectively. The infinite-dimensional Kalman filter is given by:

where $\mathcal{K}(t)=\mathcal{P}(t)\mathcal{C}^{*}(t)\left(\mathcal{F}(t)\mathcal{V}\mathcal{F}^{*}(t)\right)^{-1}$ is the Kalman gain, and $\mathcal{P}(t)$ is the solution of the Riccati equation:

Given a smooth target density $p_{*}(x)$, bounded from above and below by positive constants, we design:

where $\alpha\geq 0$ is a design parameter for individuals to adjust their velocity magnitude. In the collective level, (4) is called density feedback because of its explicit dependence on the mean-field density $p$. In the individual level, (4) is essentially nonlinear state feedback and the velocity input for the $i$-th robot is simply given by $v(X_{t}^{i},t)$.

Define $\Phi=p-p_{*}$ as the convergence error.

The mean-field density $p$ cannot be measured directly. We will study how to estimate $p$ in next section. We first establish some robustness results regardless of what estimation algorithm to use. It is useful to rewrite (4) as:

In general, we need to estimate $p$ and $\nabla p$ separately because $\nabla$ is an unbounded operator, i.e., any algorithm that produces accurate estimates of $p$ may have arbitrarily large estimation errors for $\nabla p$. Let $\hat{p}$ and $\widehat{\nabla p}$ be the estimates of $p$ and $\nabla p$. Based on (5), the control law using estimates is given by:

Define $\epsilon=\hat{p}-p$ and $\epsilon_{g}=\widehat{\nabla p}-\nabla p$ as the estimation errors. Substituting (6) into (3), we obtain the closed-loop system:

We have the following robustness result.

This theorem ensures that $\|\Phi(t)\|_{L^{2}}$ is always bounded by a function of $\|\epsilon(t)\|_{L^{2}}$ and $\|\epsilon_{g}(t)\|_{L^{2}}$, and will approach 0 exponentially if both $\hat{p}$ and $\widehat{\nabla p}$ are accurate.

Now we design algorithms to estimate $p$ and $\nabla p$ separately. The corresponding algorithms will be referred to as density filters and gradient filters, respectively. The former has been studied in [12, 13] and is included for completeness. In the estimation problem, the system (3) is assumed known (including $v$). We use kernel density estimation (KDE) to construct noisy measurements of $p$ and $\nabla p$ and utilize two important properties: the dynamics governing $p$ and $\nabla p$ are linear; their measurement noises are approximately Gaussian. Then we use infinite-dimensional Kalman filters to design two filters to estimate them separately.

First, the processes $\{X_{t}^{i}\}_{i=1}^{N}$ become asymptotically independent as $N\to\infty$ [3]. Hence, for a large $N$, $\{X_{t}^{i}\}_{i=1}^{N}$ can be approximately treated as independent samples of $p(x,t)$. We then use KDE to construct priori estimates which are used as noisy measurements of $p$ and $\nabla p$.

We need to derive an evolution equation for $\nabla p$. By applying the gradient operator on both sides of (3), we have:

where $\mathscr{I}$ is an integration operator defined as follows. For a given vector field $F:\Omega\to\mathbb{R}^{n}$, define $\mathscr{I}(F)=f$, where $f:\Omega\to\mathbb{R}$ is uniquely determined by the following relations:

For simplicity, denote $q=\nabla p$, i.e., $q:\Omega\times(0,T)\to\mathbb{R}^{n}$. We obtain a partial-integro-differential equation for $q$:

which is a linear equation. We will rewrite (3) and (9) as evolution equations in $\mathcal{H}_{1}=L^{2}(\Omega)$ and $\mathcal{H}_{2}=L^{2}(\Omega)^{n}:=\{(F_{1},\dots,F_{n}):\Omega\to\mathbb{R}^{n}\mid F_{i}\in L^{2}(\Omega)\}$, respectively. Specifically, define the following linear operators:

For any $t$, we use KDE and the samples $\{X_{t}^{i}\}_{i=1}^{N}$ to construct a priori estimate $p_{\text{KDE}}(x,t)$ [21]:

where $K(x)$ is a kernel function and $h$ is the bandwidth. We treat $p_{\text{KDE}}(x,t)$ as a noisy measurement of $p(x,t)$. We also treat $\nabla p_{\text{KDE}}(x,t)$, the gradient of $p_{\text{KDE}}(x,t)$, as a noisy measurement of $q(x,t)$. Define $w(x,t)=p_{\text{KDE}}(x,t)-p(x,t)$ and $w_{g}(x,t)=\nabla p_{\text{KDE}}(x,t)-q(x,t)$. Then, $w(x,t)$ and $w_{g}(x,t)$ are approximately infinite-dimensional Gaussian noises with diagonal covariance operators $\bar{\mathcal{R}}(t)=k\operatorname{diag}(p(t))$ and $\bar{\mathcal{R}}_{g}(t)=k_{g}\operatorname{diag}(q(t))$, respectively, where $k,k_{g}>0$ are constants depending on the kernels and $N$. (See Appendices in [13] for why $w$ and $w_{g}$ are approximately Gaussian.)

We obtain two infinite-dimensional linear systems:

We can design infinite-dimensional Kalman filters for (12) and (15). However, $\bar{\mathcal{R}}$ and $\bar{\mathcal{R}}_{g}$ are unknown because they depend on $p$ and $q$, the states to be estimated. Hence, we approximate $\bar{\mathcal{R}}$ and $\bar{\mathcal{R}}_{g}$ with $\mathcal{R}(t)=k\operatorname{diag}(p_{\text{KDE}}(t))$ and $\mathcal{R}_{g}(t)=k_{g}\operatorname{diag}(\nabla p_{\text{KDE}}(t))$. In this way, the “suboptimal” density filter is given by:

and the “suboptimal” gradient filter is given by:

where $\hat{p}$ and $\hat{q}$ are our estimates for $p$ and $q$ (i.e., $\nabla p$).

Now we study the stability and optimality of these two filters. Let $\bar{\mathcal{P}}$ and $\bar{\mathcal{Q}}$ be the corresponding solutions of (17) and (19) when $\mathcal{R}$ and $\mathcal{R}_{g}$ are respectively replaced by $\bar{\mathcal{R}}$ and $\bar{\mathcal{R}}_{g}$, their true but unknown values. Then, $\bar{\mathcal{P}}$ and $\bar{\mathcal{Q}}$ represent the optimal flows of estimation error covariance. We denote $\mathcal{L}=\mathcal{P}\mathcal{R}^{-1}$ and $\mathcal{L}_{g}=\mathcal{Q}\mathcal{R}_{g}^{-1}$ to represent the suboptimal Kalman gains, and denote $\bar{\mathcal{L}}=\bar{\mathcal{P}}\bar{\mathcal{R}}^{-1}$ and $\bar{\mathcal{L}}_{g}=\bar{\mathcal{Q}}\bar{\mathcal{R}}_{g}^{-1}$ to represent the optimal Kalman gains. Define $\epsilon=\hat{p}-p$ and $\epsilon_{g}=\hat{q}-q$ as the estimation errors of the filters. Then $\epsilon$ and $\epsilon_{g}$ satisfy the follow equations:

We can show that the suboptimal filters are stable and remain close to the optimal ones. The following theorem is for the density filter (16) and (17), which is proved in [12, 13].

Property (i) means that the suboptimal filter (16) is stable even if $\mathcal{R}$ is only an approximation for $\bar{\mathcal{R}}$. Property (ii) means that the solution of the suboptimal operator Riccati equation (17) remains close to the solution of the optimal one (when $\mathcal{R}$ is replaced by $\bar{\mathcal{R}}$). Property (iii) means that the suboptimal Kalman gain remains close to the optimal Kalman gain. We have similar results for the gradient filter (18) and (19). The proof is similar to the proof of Theorem 5 and thus omitted.

Now we discuss the stability of the feedback interconnection of density estimation and control. We collect equations (7), (20) and (21) in the following for clarity:

where we write $\mathcal{A}(t;\Phi,\epsilon,\epsilon_{g})$ and $\mathcal{A}_{g}(t;\Phi,\epsilon,\epsilon_{g})$ to emphasize their dependence on $\Phi$, $\epsilon$ and $\epsilon_{g}$ through $v$.

A critical observation is that the ISS results we have established for $\epsilon$ and $\epsilon_{g}$ are valid in spite of this dependence. This is because when designing estimation algorithms, $\mathcal{A}$ and $\mathcal{A}_{g}$ (and $v$) are treated as known system coefficients. In this way, the bilinear control system (3) becomes a linear system. The dependence on $\Phi$, $\epsilon$ and $\epsilon_{g}$ can be seen as part of the time-varying nature of $\mathcal{A}$ and $\mathcal{A}_{g}$. Hence, the stability results for our density and gradient filters will not be affected. (Nevertheless, since in the interconnection $v$ depends on $(\hat{p},\hat{q})$ and becomes stochastic, the presented filters downgrade to be the best “linear” filters instead of the minimum covariance filters.) In this regard, we can treat (24) as a cascade system. By Theorem 2, we have the following stability result for (24).

This theorem has two implications. First, the stability results for the filters are independent of the density controller. Hence, they can be used for any control design when density feedback is required. Second, the interconnected system is always ISS as long as the density feedback laws are designed such that the tracking error is ISS w.r.t. the $L^{2}$ norms of estimation errors. Considering that norms of infinite-dimensional vectors are not equivalent in general, obtaining ISS results w.r.t. $L^{2}$ norms is critical. This in turn highlights the advantage of (4), because it is difficult to obtain such an ISS result if $p$ presents in the denominator, as in [9, 10, 11].