Stochastic Dynamic Games in Belief Space

Wilko Schwarting, Alyssa Pierson, Sertac Karaman, Daniella Rus

LQR

iLQR

iterative LQ Games

iLQG

Belief Space iLQG

Belief Space Dynamic Games

\dot{x} = Ax + Bu

J=\int_{0}^{\infty}\left(x^{T} Q x+u^{T} R u+2 x^{T} N u\right) d t

\dot{x} = f(x,u)

\dot{x} = f(x,u,m)

y = h(x,u,n)

Preliminaries

\(N\) agents \(\{1,\dots,N\}\)

Joint State: \(x_k \in R^{n_{x}}\)

Joint Action: \(u_k \in R^{n_{u}}\)

Joint Measurement: \(z_k \in R^{n_{z}}\)

Belief Dynamics

b_{k+1} = \beta(b_k, u_k, z_{k+1})

b' = \tau(b,a,o)

analogous to

Belief Update

\mathbf{b}\left(\mathbf{x}_{k+1}\right)=\eta p\left(\mathbf{z}_{k+1} \mid \mathbf{x}_{k+1}\right) \int p\left(\mathbf{x}_{k+1} \mid \mathbf{x}_{k}, \mathbf{u}_{k}\right) \mathbf{b}\left(\mathbf{x}_{k}\right) \mathrm{d} \mathbf{x}_{k}

Analytical Bayes filter solution intractable

\begin{aligned}\mathbf{x}_{k+1} &=f\left(\mathbf{x}_{k}, \mathbf{u}_{k}, \mathbf{m}_{k}\right), & \mathbf{m}_{k} & \sim \mathcal{N}(0, I) \\\mathbf{z}_{k} &=h\left(\mathbf{x}_{k}, \mathbf{n}_{k}\right) & \mathbf{n}_{k} & \sim \mathcal{N}(0, I)\end{aligned}

Resort to EKF

\begin{aligned}\hat{\mathbf{x}}_{k+1} &=f\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right)+K_{k}\left(\mathbf{z}_{k+1}-h\left(f\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right), 0\right)\right) \\\Sigma_{k+1} &=\Gamma_{k+1}-K_{k} H_{k} \Gamma_{k+1}\end{aligned}

Belief Update

\begin{aligned}\Gamma_{k+1} &=A_{k} \Sigma_{k} A_{k}^{T}+M_{k} M_{k}^{T} \\K_{k} &=\Gamma_{k+1} H_{k}^{\top}\left(H_{k} \Gamma_{k+1} H_{k}^{\top}+N_{k} N_{k}^{\top}\right)^{-1} \\A_{k} &=\frac{\partial f}{\partial \mathbf{x}}\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right), \quad M_{k}=\frac{\partial f}{\partial \mathbf{m}}\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right) \\H_{k} &=\frac{\partial h}{\partial \mathbf{x}}\left(f\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right), 0\right), \quad N_{k}=\frac{\partial h}{\partial \mathbf{n}}\left(f\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right), 0\right)\end{aligned}

\begin{aligned} \Sigma_{k+1} &= P_k^+ \\ \Gamma_{k+1} &= P_k^- \\ A_k &= F_k \\ M_kM_k^T &= Q \\ N_kN_k^T &= R \\ \end{aligned}

Their Notation

Our Notation

Belief Update

g_{k}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right)=\left[\begin{array}{c}f\left(\hat{\mathbf{x}}_{k}, \mathbf{u}_{k}, 0\right) \\\operatorname{vec}\left(\Gamma_{k+1}-K_{k} H_{k} \Gamma_{k+1}\right)\end{array}\right]

\mathbf{b}_{k+1}=g\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right)+W\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right) \xi_{k}, \quad \xi_{k} \sim \mathcal{N}(0, I)

\(\xi_k\) accounts for both measurement and transition noise

W_{k}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right)=\left[\begin{array}{c}\sqrt{K_{k} H_{k} \Gamma_{k+1}} \\\mathbf{0}\end{array}\right]

Vectorize belief:

\mathbf{b}_k = [\hat{x}_k^T, \text{vec}(\Sigma_k)^T]^T

POMDP Best Response Game

Q^i(b_0, u) = \mathbb{E}_{z}\left[c_l^i(b_l) + \sum_{k=0}^{l-1}c_k^i(b_k,u_k)\right]

Expected Return for agent \(i\)

\(c_l\) - cost at final time-step (terminal cost)

\(c_k\) - cost for any intermediary time step

\pi^{i}=\underset{\mathbf{u}^{i}}{\arg \min } Q^{i}\left(\mathbf{b}_{0}, \mathbf{u}\right) \quad \forall i \in\{1, \ldots, N\} \\ \text { s.t. } \mathbf{b}_{k+1}=\beta\left(\mathbf{b}_{k}, \mathbf{u}_{k}, \mathbf{z}_{k+1}\right)

\begin{aligned}V_{l}^{i}\left(\mathbf{b}_{l}\right) &=c_{l}^{i}\left(\mathbf{b}_{l}\right) \\Q_{k}^{i}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right) &=c_{k}^{i}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right)+\underset{\mathbf{z}_{k+1}}{\mathbb{E}}\left[V_{k+1}^{i}\left(\beta\left(\mathbf{b}_{k}, \mathbf{u}_{k}, \mathbf{z}_{k+1}\right)\right)\right] \\V_{k}^{i}\left(\mathbf{b}_{k}\right) &=\min _{\mathbf{u}_{k}^{i}} Q_{k}^{i}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right) \\\pi_{k}^{i}\left(\mathbf{b}_{k}\right) &=\underset{\mathbf{u}_{k}^{i}}{\arg \min } Q_{k}^{i}\left(\mathbf{b}_{k}, \mathbf{u}_{k}\right)\end{aligned}

((\(\pi^i\) is a function of \(\pi^{\neg i}\)))

Iterative Dynamic Programming

Necessary condition of local Nash Equilibrium:

\frac{\partial Q_k^i(b_k,u_k)}{\partial u_k^i} = 0 \quad \forall i \in \{1,\dots, N\}

Optimize over perturbations

\begin{aligned} \bar{b} &= b - \delta b \\ \bar{u} &= u - \delta u \\ \bar{s} &= s - \delta s \quad (\bar{s} = \left[\bar{b}^T, \bar{u}^T\right]^T) \end{aligned}

V_{k}^{i}\left(\overline{\mathbf{b}}_{k}+\delta \mathbf{b}_{k}\right) \approx V_{k}^{i}+V_{\mathbf{b}, k}^{i, \top} \delta \mathbf{b}_{k}+\frac{1}{2} \delta \mathbf{b}_{k}^{\top} V_{\mathbf{b b}, k}^{i} \delta \mathbf{b}_{k}

Quadratic Value Approximation

\left( f(x) \approx f(a) + f'(a)(x-a) + \frac{1}{2}f''(a)(x-a)^2 \right)

Q_{k}^{i}\left(\overline{\mathbf{s}}_{k}+\delta \mathbf{s}_{k}\right) \approx Q_{k}^{i}+Q_{\mathrm{s}, k}^{i, \top} \delta \mathbf{s}_{k}+\frac{1}{2} \delta \mathbf{s}_{k}^{\top} Q_{\mathrm{ss}, k}^{i} \delta \mathbf{s}_{k}

\delta \mathbf{u}_{k}^{i, *}=\underset{\delta \mathbf{u}_{k}^{i}}{\arg \min }\left\{Q_{\mathbf{s}, k}^{i, \top} \delta \mathbf{s}_{k}+\frac{1}{2} \delta \mathbf{s}_{k}^{\top} Q_{\mathrm{ss}, k}^{i} \delta \mathbf{s}_{k}\right\}

\delta \mathbf{u}_{k}^{*}=-\hat{Q}_{\mathbf{u u}}^{-1}\left(\hat{Q}_{\mathbf{u}}+\hat{Q}_{\mathbf{u b}} \delta \mathbf{b}_{k}\right)

\hat{Q}_{\mathrm{uu}}=\left[\begin{array}{c}Q_{\mathrm{u}^{1}}^{1} \mathrm{u} \\Q_{\mathrm{u}^{2}}^{2} \mathrm{u} \\\vdots \\Q_{\mathrm{u}^{N}}^{N}\end{array}\right], \hat{Q}_{\mathrm{ub}}=\left[\begin{array}{c}Q_{\mathrm{u}^{1}}^{1} \mathrm{~b} \\Q_{\mathrm{u}^{2} \mathrm{~b}}^{2} \\\vdots \\Q_{\mathrm{u}^{N} \mathrm{~b}}^{N}\end{array}\right], \hat{Q}_{\mathrm{u}}=\left[\begin{array}{c}Q_{\mathrm{u}^{1}}^{1} \\Q_{\mathrm{u}^{2}}^{2} \\\vdots \\Q_{\mathrm{u}^{N}}^{N}\end{array}\right]

\pi_k = \bar{\mathbf{u}}_k + j_k + K_k\delta\mathbf{b}_k

j_k = \hat{Q}_{\mathbf{u}\mathbf{u}}^{-1}\hat{Q}_{\mathbf{u}}

K_k = -\hat{Q}_{\mathbf{u}\mathbf{u}}^{-1}\hat{Q}_{\mathbf{u}\mathbf{b}}

Nominal

Feed-forward

Feedback

Backward Pass

V_{l}^{i}=c_{l}^{i}\left(\overline{\mathbf{b}}_{l}\right), \quad V_{\mathrm{b}, l}^{i}=\left.\frac{\partial c_{l}^{i}(\mathbf{b})}{\partial \mathbf{b}}\right|_{\mathrm{b}=\overline{\mathbf{b}}_{l}}, \quad V_{\mathrm{bb}, l}^{i}=\left.\frac{\partial^{2} c_{l}^{i}(\mathbf{b})}{\partial \mathbf{b}^{2}}\right|_{\mathrm{b}=\overline{\mathbf{b}}_{l}}

K_k = -\hat{Q}_{\mathbf{u}\mathbf{u}}^{-1}\hat{Q}_{\mathbf{u}\mathbf{b}}, \quad j_k = \hat{Q}_{\mathbf{u}\mathbf{u}}^{-1}\hat{Q}_{\mathbf{u}}, \quad \pi_k = \bar{\mathbf{u}}_k + j_k + K_k\delta\mathbf{b}_k

\hat{Q}_{\mathbf{u u}}=\left[\begin{array}{c}Q_{\mathbf{u}^{1} \mathbf{u}}^{1} \\Q_{\mathbf{u}^{2} \mathbf{u}}^{2} \\\vdots \\Q_{\mathbf{u}^{N} \mathbf{u}}^{N}\end{array}\right], \hat{Q}_{\mathbf{u b}}=\left[\begin{array}{c}Q_{\mathbf{u}^{1} \mathbf{b}}^{1} \\Q_{\mathbf{u}^{2} \mathbf{b}}^{2} \\\vdots \\Q_{\mathbf{u}^{N} \mathbf{b}}^{N}\end{array}\right], \hat{Q}_{\mathbf{u}}=\left[\begin{array}{c}Q_{\mathbf{u}^{1}}^{1} \\Q_{\mathbf{u}^{2}}^{2} \\\vdots \\Q_{\mathbf{u}^{N}}^{N}\end{array}\right]

Forward Pass

\tilde{Q}_{\mathbf{u}\mathbf{u}}^i = \hat{Q}_{\mathbf{u}\mathbf{u}}^i + \mu_{\mathbf{u}}I

\begin{aligned}\tilde{Q}_{\mathrm{ss}, k}^{i}=& c_{\mathrm{ss}, k}^{i}+g_{\mathrm{s}, k}^{T}\left(V_{\mathrm{bb}, k+1}^{i}+\mu_{\mathrm{b}} I\right) g_{\mathrm{s}, k} \\&+\sum_{i=1}^{n} W_{\mathrm{s}, k}^{(j), T}\left(V_{\mathrm{bb}, k+1}^{i}+\mu_{\mathrm{b}} I\right) W_{\mathrm{s}, k}^{(j)}\end{aligned}

Control Regularization

Belief Regularization

Experimental Results

Active Surveillance

Agent 1 - observe agent 2

Agent 2 - maintain constant speed

Experimental Results

Guide Dog for Blind Agent

Agent 1 - Guide agent 2 to goal with low unceratainty

Agent 2 - No navigational control

Experimental Results

Competitive Racing

Agent 1 - Faster than agent 2 but starts behind agent 2

Agent 2 - Slower than agent 1 but starts ahead