BLT*: Informed Belief Localization Trees for Uncertainty-Aware Planning on Digital Twins

Preliminaries

We use bold lowercase letters to denote vectors, e.g., $\mathbf{x}\in\mathbb{R}^{d}$, and bold uppercase letters for matrices, e.g., $\mathbf{A}\in\mathbb{R}^{d\times d}$. The space of $d\times d$ positive semidefinite matrices is denoted $\mathbb{S}^{d}_{+}$.

We denote the configuration space by $X\subseteq\mathbb{R}^{d}$; the obstacle space by $X_{\text{obs}}\subset X$; and the free space by $X_{\text{free}}=\operatorname{cl}(X\setminus X_{\text{obs}})$, where $\operatorname{cl}(\cdot)$ denotes closure.

Problem Formulation

Consider the state space $X$, the belief space $\mathcal{B}^{d}$, the obstacle space $X_{\mathrm{obs}}$, and the goal region $X_{\mathrm{goal}}$ as defined in Section III . Let $P_{\mathrm{inter}}\in(0,1)$ denote a prescribed probability threshold for goal attainment and collision avoidance.

Methodology

In this section, we outline the evolution of belief states under our motion and observation models. We use the Kalman filter notation from [ 20 ] , where $\check{(\cdot)}$ and $\hat{(\cdot)}$ denote prior and posterior quantities, respectively.

The prediction step under the motion model ( 5 ) is Assuming the maximum-likelihood observation, the innovation is zero and the correction step is expressed in information form as where $\bm{\Omega}_{k}=\mathbf{R}_{k}^{-1}$ is the information matrix in ( 6 ).

Validation

For each map from the previous section, we conducted 50 trials using pseudorandom seeds, with the same seed used across all five planners in each trial. Each planner was run for $100\,\mathrm{s}$ under the $\ell_{2}$ and $W_{2}$ objectives separately. The objective value was sampled every $1\,\mathrm{ms}$ by a separate monitoring thread, and the time to the first solution was recorded with sub-millisecond resolution.

Sources and Demonstrations

Figures are reproduced from Preston-Krebs et al. See the full paper for experimental details and the project page, when the paper links one, for demonstrations and videos.