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

Elliot Preston-Krebs; Abhishek Goudar; Timothy D. Barfoot

cs.ROOct 1, 2026

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

Authors: Elliot Preston-Krebs, Abhishek Goudar, Timothy D. Barfoot

Organizations: University of Toronto Institute for Aerospace Studies (UTIAS), 4925 Dufferin St, Ontario, Canada.

Abstract

We present Informed Belief Localization Trees* (Informed BLT*), a sampling-based belief space planning (BSP) algorithm that scales to large outdoor digital twins with point-cloud observations. We adapt RRT* and Informed RRT* to belief space using the $2$ -Wasserstein ( $W_2$ ) metric. Assuming isotropic Gaussian beliefs, sampled belief states can be connected efficiently while accounting for available information and probabilistic collision constraints. This enables steering and rewiring without repeatedly propagating observations, and allows previously computed measurement information to be reused. We present a framework to generate semantically labelled digital twins for planning in real-world environments with point-cloud-based localization. Experiments in simulated environments and digital twins show faster initial solution discovery in most maps with competitive cost convergence.

Figures & tables

Panel of Figure 1 — Fig. 1 : (Top) Configuration-space view of an Informed BLT* solution projected onto the Office map. The trajectory is coloured from green to red, indicating progression; point-cloud observations are colour-matched to their corresponding positions, and the ring denotes the lidar range. (Bottom) Belief-space planning view with uncertainty on the vertical axis, scaled for visualization. Grey surfaces are obstacles lifted into belief space under the uncertainty-based collision criterion in Section V-F . Blue edges and cyan nodes show the planning tree and the final path in magenta. The robot first detours within lidar range of a building to reduce uncertainty, allowing passage through a narrower corridor to a shorter path.

Panel of Figure 2 — Fig. 2 : Digital twins used for evaluation: Campus and Rural (top), and Office (bottom).

Panel of Figure 3 — Fig. 3 : Benchmark results for Campus and one representative Random map over 50 independently seeded trials. Top: percentage of trials solved versus computation time. Bottom: median solution cost versus computation time, with non-parametric $95\%$ confidence bands. Dots denote the median initial solution, with $95\%$ confidence intervals on initial-solution time and cost.

Panel of Figure 4 — Fig. 4 : Simulated environments used for evaluation: from left to right, Empty , Block , Narrow , and Random . The start node and its covariance are denoted by a green dot and an orange circle, respectively, and the goal region by a red circle.

	Planner	Inf. BLT*	BLT*	B-RRT	B-SST	RRBT	Inf. BLT*	BLT*	B-RRT	B-SST	RRBT	Inf. BLT*	BLT*	B-RRT	B-SST	RRBT
	Planner	(Ours)	(Ours)	[ 3 ]	[ 3 ]	[ 1 ]	(Ours)	(Ours)	[ 3 ]	[ 3 ]	[ 1 ]	(Ours)	(Ours)	[ 3 ]	[ 3 ]	[ 1 ]
	Simulation $\mathbb{R}^{2}$	Empty					Block					Narrow
$W_{2}$	1st time [ $\mathrm{ms}$ ]	0.14	0.12	4.11	2.51	3.72	8.77	8.35	12.37	26.93	14.09	19.30	19.19	9.72	6.15	249.23
	1st cost [ $\mathrm{m}$ ]	140.45	140.45	186.36	185.03	140.40	198.90	198.90	304.11	262.47	187.04	212.50	212.50	325.11	292.49	215.40
	10 $\mathrm{s}$ cost [ $\mathrm{m}$ ]	110.85	112.41	181.76	113.25	111.68	126.74	128.54	254.13	153.05	136.15	185.76	185.69	307.12	180.13	182.93
	100 $\mathrm{s}$ cost [ $\mathrm{m}$ ]	110.41	111.89	180.64	111.35	110.79	124.80	126.40	217.48	132.02	134.40	183.15	183.34	280.73	177.34	178.67

TABLE I : Planner performance across simulated maps and digital twins over 50 runs per map.

W_{2}

and

\ell_{2}

denote the optimization objectives. Metrics are mean first-solution cost/time, mean cost at 10/100

\mathrm{s}

, and success at 10/100

\mathrm{s}

. Means include finite solutions only; success is omitted for

\mathbb{R}^{2}

maps, where all planners converge.

	Planner	Inf. BLT*	BLT*	B-RRT	B-SST	RRBT
	Planner	(Ours)	(Ours)	[ 3 ]	[ 3 ]	[ 1 ]
	Simulation $\mathbb{R}^{2}$	Random $\times$ 10
$W_{2}$	1st time [ $\mathrm{ms}$ ]	3.30	3.27	6.56	4.98	45.32
	1st cost [ $\mathrm{m}$ ]	99.97	99.97	124.89	120.65	93.02
	10 $\mathrm{s}$ cost [ $\mathrm{m}$ ]	67.71	70.30	120.47	66.78	68.51
	100 $\mathrm{s}$ cost [ $\mathrm{m}$ ]	66.40	68.81	119.12	65.52	66.50

TABLE II : Planner performance on the Random benchmark over 10 maps and 50 runs per map (500 runs per planner).

W_{2}

and

\ell_{2}

denote the optimization objectives. Metrics are mean first-solution cost/time and mean cost at 10/100

\mathrm{s}

Explore similar work

PRISM: Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees
Jun 24, 2026Alex Rose, Christopher Jewison, Jonathan P. HowMotion Planning Reachability
Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge MA 02139 · Draper, Cambridge MA 02139
LOCUS-DT: Localization via Observation-Conditioned Uncertainty Scoring with Digital Twins
Aug 1, 2026Haozhe Lei, Roberto Bomfin, Marwa Chafii +1Indoor Localization Digital Twins
NYU WIRELESS, Tandon School of Engineering, New York University, Brooklyn, NY 11201, USA · Engineering Division, New York University Abu Dhabi, 129188, UAE
RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning
Aug 1, 2026Muhayy Ud Din, Ahmed Nadar, Jan Rosell +1Motion Planning Obstacle Avoidance
Center for Autonomous Robotic Systems, Khalifa University (KU-CARS) · Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya, Barcelona, Spain

Jun 24, 2026, eess.SYJ/K move · Enter open · S save

PRISM: Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees

Alex Rose, Christopher Jewison, Jonathan P. How

Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge MA 02139 · Draper, Cambridge MA 02139

Motion Planning Reachability Planning Prism

Open paper AskPDF arXiv

Belief-space planning under motion uncertainty and state and control constraints remains a fundamental challenge, largely due to the difficulty of establishing reachability guarantees in constrained belief spaces. Existing constrained belief-space planners rely on sampling to construct multi-query belief roadmaps and explicitly find feasible trajectories between sampled nodes to establish reachability. These methods often struggle to cover the belief space or use robust control techniques that improve coverage at the cost of indirect, high-cost trajectories; they also lack finite-time or finite-memory completeness guarantees. We propose PRISM, a multi-query motion planning algorithm for belief spaces with state and control constraints that targets both high coverage and low cost. We present a new result on controllability of the state covariance under constraints, which is used by PRISM to decompose belief-space planning into deterministic mean planning and covariance shrinking. PRISM further includes an online local optimization method that reduces the cost of feasible belief-space trajectories. Under mild assumptions on the start and goal distributions, we prove that PRISM guarantees full coverage (i.e. completeness) despite actuator and obstacle constraints. In challenging simulated scenarios, PRISM achieves substantially higher roadmap coverage than state-of-the-art belief-space planning methods while producing trajectories with lower mean cost and cost variance. For example, PRISM achieves 100% coverage in easy and medium-difficulty scenarios, and, in the hardest scenario, which violates PRISM's coverage assumptions, it still achieves 97-100% coverage, while all other methods achieve less than 45%.

Abstract

Explore similar work

PRISM: Efficient and Locally Optimal Probabilistic Planning with Reachability Guarantees

LOCUS-DT: Localization via Observation-Conditioned Uncertainty Scoring with Digital Twins

RIT*: Riemannian Informed Trees for Cost-Adaptive Optimal Motion Planning