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 (W2) 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
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.
Fig. 2 : Digital twins used for evaluation: Campus and Rural (top), and Office (bottom).
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.
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
(Ours)
(Ours)
[ 3 ]
[ 3 ]
[ 1 ]
(Ours)
(Ours)
[ 3 ]
[ 3 ]
[ 1 ]
(Ours)
(Ours)
[ 3 ]
[ 3 ]
[ 1 ]
Simulation R2
Empty
Block
Narrow
W2
1st time [ 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 [ 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 s cost [ 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 s cost [ 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. W2 and ℓ2 denote the optimization objectives. Metrics are mean first-solution cost/time, mean cost at 10/100 s , and success at 10/100 s . Means include finite solutions only; success is omitted for R2 maps, where all planners converge.
Planner
Inf. BLT*
BLT*
B-RRT
B-SST
RRBT
(Ours)
(Ours)
[ 3 ]
[ 3 ]
[ 1 ]
Simulation R2
Random × 10
W2
1st time [ ms ]
3.30
3.27
6.56
4.98
45.32
1st cost [ m ]
99.97
99.97
124.89
120.65
93.02
10 s cost [ m ]
67.71
70.30
120.47
66.78
68.51
100 s cost [ 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). W2 and ℓ2 denote the optimization objectives. Metrics are mean first-solution cost/time and mean cost at 10/100 s .
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%.
Alex Rose, Christopher Jewison, Jonathan P. How
Laboratory for Information and Decision Systems, Massachusetts Institute of Technology, Cambridge MA 02139 · Draper, Cambridge MA 02139
Accurate indoor localization is essential for emerging applications in robotic navigation and search and rescue. While classical methods typically focus on single-point estimates, complex indoor environments with heavy blockage and multipath propagation often lead to multimodal likelihood surfaces where a single estimate is insufficient. This paper proposes LOCUS-DT (Localization via Observation-Conditioned Uncertainty Scoring with Digital Twins), a framework that treats snapshot localization as posterior inference over the transmitter location. By leveraging a ray-tracing-based digital twin (DT) of the known environment, LOCUS-DT generates synthetic multipath profiles for candidate locations and compares them against the measured channel profile. Central to our approach is a novel learned scoring function designed to compare a fixed number of dominant specular paths, providing robustness against errors in both the DT environment model and the physical channel estimation. Importantly, LOCUS-DT is trained over an ensemble of environments to ensure generalization to unseen layouts. We evaluate the system using a Sionna-based ray-tracing backend, demonstrating that LOCUS-DT captures the sharp, multimodal posterior structures inherent in indoor settings more accurately than standard Gaussian or Gaussian-mixture benchmarks.
Haozhe Lei, Roberto Bomfin, Marwa Chafii +1
NYU WIRELESS, Tandon School of Engineering, New York University, Brooklyn, NY 11201, USA · Engineering Division, New York University Abu Dhabi, 129188, UAE
We present Riemannian Informed Trees (RIT*), a planning framework that replaces Euclidean primitives in batch-informed search with their Riemannian counterparts. RIT* constructs a tighter, cost-consistent informed set, performs a nearest-neighbour search under an anisotropic distance metric, and evaluates edge costs efficiently via a cascading scheme. We further introduce a Collision-Adaptive Metric Refinement (CARM), which learns an obstacle-proximity cost field online from collision feedback, reducing the reliance on prior metric design in practical settings. Experiments across environments from 2-D to 14-D show that RIT* is competitive in low-dimensional and spatially constant-metric settings and produces substantially lower-cost solutions when the metric varies spatially in high-dimensional configuration spaces. Performance gains scale with anisotropy and dimension, reaching up to 13.0% improvement in median initial cost over BIT* in the 3-D anisotropic benchmark, up to 9.0% in median final cost over BIT* in 6-DOF manipulation, and 24.8-63.5% in a 14-DOF bimanual planning problem, where Euclidean-informed baselines degrade. Videos and code can be found here: https://muhayyuddin.github.io/ritstar/
Muhayy Ud Din, Ahmed Nadar, Jan Rosell +1
Center for Autonomous Robotic Systems, Khalifa University (KU-CARS) · Institute of Industrial and Control Engineering, Universitat Politècnica de Catalunya, Barcelona, Spain