Literature
Display
← All papers

Control-Ready Uncertainty for Trajectory Diffusion

Zhiwei Xue · Jia Yue Kam · Jinhang Qiu · Yifeng Cheng · Ege Gursoy · Jiaming Wang · Vincent Bonnet · Harold Soh

Preprint posted · CoRL 2026 (accepted) ↗

Robotics22 min text · CondensedOriginal paper ↗
Contents

Overview

This is the complete 39-page arXiv v1 reading edition of SCOPE, including its mathematical and experimental appendices. The PDF is the figure-numbering reference: its omitted Maze2D Figure 8 is restored from the original PDF, and subsequent captions are relabeled accordingly. The original equations, tables, algorithm, references and project demonstrations remain available alongside the section notes.

At a glance

SCOPE turns a diffusion trajectory predictor into a Gaussian-mixture uncertainty interface for control. A lightweight head learns local score curvature during training, then predicts trajectory precision without repeatedly sampling the diffusion model at inference. The tubes describe local predictive variability, rather than epistemic confidence or a certified safe set. Experiments use simulated pedestrian forecasting/navigation, Maze2D planning and a Franka shelf task; the strongest hardware setting succeeds in 21 of 30 dynamic trials.

1 Introduction

A nominal trajectory omits how plausible nearby alternatives are, while estimating their spread with many diffusion samples can delay control. SCOPE adds structured local precision around explicitly indexed modes. It uses the resulting covariance either to predict moving obstacles or to shape sampling around a robot’s own plan. Mode weights represent distinct futures; the tube around each mode represents its local spread. These two kinds of uncertainty remain separate.

Complete original Figure 1: ( Top ) SCOPE leverages score curvature to augment diffusion predictions with precision tubes that are suitable for control. It provides uncertainty estimates at low computation cost, enabling real-time control. ( Bottom ) We apply SCOPE as a robot policy prior in MPPI and also to predict the trajectories of other agents.
Figure 1: (Top) SCOPE leverages score curvature to augment diffusion predictions with precision tubes that are suitable for control. It provides uncertainty estimates at low computation cost, enabling real-time control. (Bottom) We apply SCOPE as a robot policy prior in MPPI and also to predict the trajectories of other agents.

2 Preliminaries

The preliminaries connect diffusion denoising to the score of a trajectory density and introduce MPPI’s weighted rollout updates. MPPI evaluates sampled control sequences through dynamics and a running cost. Its exploration distribution determines which corrections it can discover with a limited rollout budget. SCOPE changes that interface rather than replacing collision checking with an unverified generated path.

Complete original Figure 2: SCOPE overview. Our method extracts control-ready Gaussian tubes from a diffusion predictor. Here, we use a multimodal diffusion backbone (left) that produces K K nominal trajectories with explicit mode probabilities. The SCOPE Precision Head (right) wraps each trajectory in a structured precision matrix 𝚲 k \boldsymbol{\Lambda}_{k} , learned by distilling the score model’s terminal curvature via Hessian-vector products at training time. At inference, Gaussian tubes are produced with low overhead after diffusion decoding.
Figure 2: SCOPE overview. Our method extracts control-ready Gaussian tubes from a diffusion predictor. Here, we use a multimodal diffusion backbone (left) that produces KK nominal trajectories with explicit mode probabilities. The SCOPE Precision Head (right) wraps each trajectory in a structured precision matrix 𝚲k\boldsymbol{\Lambda}_{k}, learned by distilling the score model’s terminal curvature via Hessian-vector products at training time. At inference, Gaussian tubes are produced with low overhead after diffusion decoding.

3 Score-Curvature for Online Precision Estimation (SCOPE)

The method packages nominal trajectories, mode probabilities and positive-definite precision matrices into a mixture of Gaussian trajectory tubes. Its discrete multimodal backbone and precision head are trained in stages. Curvature supplies the head’s teacher signal; post-hoc likelihood scaling adjusts covariance magnitude. The approximation is local and depends on the backbone learning useful modes.

3.1 Control Interface Representation: From Local Tubes to Mixtures

Each mode supplies a mean trajectory and joint precision over the complete horizon. Inverting precision gives covariance blocks for individual future times. The mixture therefore separates between-mode alternatives from within-mode temporal dependence. It can wrap an ordinary diffusion prediction as well as explicit categorical modes, but fitting one Gaussian around an arbitrary sampled trajectory does not guarantee a globally faithful density.

Equation 1
p(𝐲1:T∣𝐜)≈∑k=1Kπk(𝐜)𝒩(𝐲1:T;𝝁k(𝐜),𝚲k(𝐜)−1),p(\mathbf{y}_{1:T}\mid\mathbf{c})\;\approx\;\sum\nolimits_{k=1}^{K}\pi_{k}(\mathbf{c})\,\mathcal{N}\!\big(\mathbf{y}_{1:T};\boldsymbol{\mu}_{k}(\mathbf{c}),\boldsymbol{\Lambda}_{k}(\mathbf{c})^{-1}\big), (1)

3.2 Multimodal Diffusion Backbone

The backbone follows a categorical latent formulation with a context-conditioned mode prior and mode-conditioned diffusion decoder [13]. Its encoder posterior enables variational training, while the prior assigns mode probabilities at inference. A deterministic DDIM decode supplies one nominal trajectory per retained mode. The appendix adds mutual-information and usage regularizers to the displayed weighted ELBO; those additions are important to avoiding mode collapse.

Equations 2–3
pθ(𝐲1:T∣𝐜)=∑k=1Kpθ(z=k∣𝐜)pψ(𝐲1:T∣𝐡𝐜,z=k),\displaystyle p_{\theta}(\mathbf{y}_{1:T}\mid\mathbf{c})=\sum\nolimits_{k=1}^{K}p_{\theta}(z{=}k\mid\mathbf{c})\,p_{\psi}(\mathbf{y}_{1:T}\mid\mathbf{h}_{\mathbf{c}},z{=}k), (2)
ℒELBO=𝔼qη​(z|𝐜,𝐲)[ℒdiff(𝐜,𝐲;z)]−βKL(qη(z|𝐜,𝐲)∥pθ(z|𝐜)),\displaystyle\mathcal{L}_{\text{ELBO}}=\mathbb{E}_{q_{\eta}(z|\mathbf{c},\mathbf{y})}\!\big[\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y};z)\big]-\beta\,\mathrm{KL}\!\big(q_{\eta}(z|\mathbf{c},\mathbf{y})\,\|\,p_{\theta}(z|\mathbf{c})\big), (3)

3.3 Score-Curvature Precision Head

The head predicts a block-banded Cholesky factor plus a low-rank correction, producing positive-definite precision with local and long-range temporal couplings. Training matches its action on random probes to derivatives of a frozen diffusion score. Second derivatives are used during training, not repeatedly in the control loop. The teacher uses a small nonzero noise level in implementation; the terminal clean-density curvature is an approximation.

Equations 4–7
𝚲∗​(𝐲^):=−∇𝐲2​log⁡p⁡(𝐲)|𝐲=𝐲^,\boldsymbol{\Lambda}^{*}(\hat{\mathbf{y}})\;:=\;-\nabla_{\mathbf{y}}^{2}\log p(\mathbf{y})\big|_{\mathbf{y}=\hat{\mathbf{y}}}, (4)
ℒideal​(ϕ)=𝔼𝐲^​[‖𝚲ϕ​(𝐲^,𝐜,z)−𝚲∗​(𝐲^)‖F2].\mathcal{L}_{\text{ideal}}(\phi)\;=\;\mathbb{E}_{\hat{\mathbf{y}}}\!\Big[\big\|\boldsymbol{\Lambda}_{\phi}(\hat{\mathbf{y}},\mathbf{c},z)-\boldsymbol{\Lambda}^{*}(\hat{\mathbf{y}})\big\|_{F}^{2}\Big]. (5)
𝚲ϕ​(𝐲)=𝐀ϕ​(𝐲)+𝐑ϕ​(𝐲)​𝐑ϕ​(𝐲)⊤,𝐀ϕ​(𝐲)=𝐋ϕ​(𝐲)​𝐋ϕ​(𝐲)⊤,\boldsymbol{\Lambda}_{\phi}(\mathbf{y})=\mathbf{A}_{\phi}(\mathbf{y})+\mathbf{R}_{\phi}(\mathbf{y})\mathbf{R}_{\phi}(\mathbf{y})^{\top},\quad\mathbf{A}_{\phi}(\mathbf{y})=\mathbf{L}_{\phi}(\mathbf{y})\mathbf{L}_{\phi}(\mathbf{y})^{\top}, (6)
ℒprec​(ϕ)=𝔼𝐲​𝔼𝐮​[‖𝚲ϕ​(𝐲)​𝐮+∇𝐲(sθ​(𝐲,0)⊤​𝐮)‖22],\mathcal{L}_{\text{prec}}(\phi)\;=\;\mathbb{E}_{\mathbf{y}}\,\mathbb{E}_{\mathbf{u}}\Big[\,\big\|\boldsymbol{\Lambda}_{\phi}(\mathbf{y})\mathbf{u}\;+\nabla_{\mathbf{y}}\big(s_{\theta}(\mathbf{y},0)^{\top}\mathbf{u}\big)\big\|_{2}^{2}\,\Big], (7)

3.4 Using SCOPE in Downstream Control

As a forecaster, the tube becomes a Gaussian-mixture moving-obstacle model for MPPI’s collision-risk penalty. As a policy prior, a decoded path supplies warm starts/subgoals and its local covariance shapes exploration. For the single-integrator case, the next-waypoint state covariance is scaled by γ²/Δt², with γ below one. This is the paper’s chosen proposal design, rather than a general identity for arbitrary state/action dynamics.

Complete original Figure 3: ( Left ) Y-corridor environment. ( Right ) Uncertainty quality (NLL).
Figure 3: (Left) Y-corridor environment. (Right) Uncertainty quality (NLL).

4 Experiments

The experiments separately test uncertainty quality, policy-side rollout efficiency and forecasting inside closed-loop control. All quantitative results and implementation tables remain. Reported gains combine model quality with control behavior; they do not establish calibrated coverage, collision freedom or general transfer to raw visual observations.

4.1 SCOPE Precision Head Properties

ORCA-generated junction data vary crowd size from three to twelve pedestrians. SCOPE’s calibrated banded-plus-low-rank head has the best reported trajectory NLL across all four densities, with −61.4, −52.2, −56.4 and −60.6. The shared backbone keeps its mean-trajectory accuracy unchanged across precision ablations. At three pedestrians, total inference is 12.87 ms on an A5000: 12.19 ms decoding, 0.60 ms precision and 0.08 ms calibration. Batched 256-sample ensembles take 91.779 ms, about 7.1 times as long.

Complete original Table 1: Inference time breakdown of SCOPE (Banded+LR).
Table 1: Inference time breakdown of SCOPE (Banded+LR).
Accessible table and original citation links
Component Time [ms]
Diffusion backbone 12.19
SCOPE Precision Head 0.60
Calibration Scaling 0.08
Total 12.87

4.2 Case Study 1: Maze2D Navigation

Maze2D contains unseen non-convex wall layouts that can trap short-horizon planners. Diff-MPPI and SCOPE share a diffusion path prior and subgoal tracking; SCOPE additionally changes the proposal covariance. Across the rollout sweep it reports better success and shorter paths. At 1,024 rollouts, success is 99.25% versus 96.35% for Diff-MPPI over 2,000 problems. The covariance is learned from feasible trajectories, while current geometric collision checks still evaluate rollouts.

Complete original Figure 4: ( Left ) Maze2D. ( Right ) SR and average path length vs. MPPI rollouts.
Figure 4: (Left) Maze2D. (Right) SR and average path length vs. MPPI rollouts.

4.3 Case Study 2: Y-Corridor Pedestrian Navigation

The junction controller consumes forecasts of online ORCA pedestrians. At three/six/nine/twelve pedestrians, calibrated SCOPE reaches 100/96/93/92% success in 100 episodes per density, versus 100/94/86/88% without calibration. Travel time and distance are reported only for successes, so their averages exclude failed episodes. The world model in this experiment is specifically a pedestrian-trajectory predictor, with latency-aware forecast reuse when computation cannot keep up.

Complete original Table 2: Y-corridor navigation results across crowd densities N p ∈ { 3 , 6 , 9 , 12 } N_{p}\in\{3,6,9,12\} .
Table 2: Y-corridor navigation results across crowd densities Np∈{3,6,9,12}N_{p}\in\{3,6,9,12\}.
Accessible table and original citation links
SR [%] ↑\uparrow T¯\overline{T} [s] d¯\overline{d} [m]
Method 3 6 9 12 3 6 9 12 3 6 9 12
NF 80.0 72.0 70.0 65.0 14.44±\pm3.95 10.05±\pm6.91 8.91±\pm7.53 8.82±\pm7.63 10.14±\pm2.75 6.88±\pm4.87 5.97±\pm5.20 6.02±\pm5.37
LKF 96.0 91.0 90.0 85.0 14.64±\pm3.65 11.49±\pm6.92 10.28±\pm7.50 10.68±\pm8.87 10.26±\pm2.51 7.95±\pm4.92 6.99±\pm5.21 7.29±\pm6.06
MID-UM* 100.0 89.0 85.0 86.0 14.72±\pm3.98 11.95±\pm7.68 11.19±\pm9.59 11.84±\pm10.13 10.31±\pm2.77 8.24±\pm5.40 7.64±\pm6.64 8.09±\pm6.83
MID-MM* 100.0 91.0 85.0 88.0 14.74±\pm3.98 12.08±\pm7.47 10.56±\pm8.18 11.25±\pm9.84 10.30±\pm2.75 8.29±\pm5.22 7.21±\pm5.76 7.71±\pm6.68
MID-Ens-64 98.0 87.0 82.0 89.0 14.55±\pm3.91 11.43±\pm7.22 9.89±\pm7.72 10.59±\pm8.64 10.19±\pm2.71 7.89±\pm5.13 6.73±\pm5.41 7.24±\pm5.97
MID-Ens-128 99.0 90.0 82.0 86.0 14.60±\pm3.97 11.68±\pm7.25 9.57±\pm7.42 10.95±\pm8.66 10.21±\pm2.76 8.06±\pm5.14 6.52±\pm5.19 7.50±\pm5.95
MID-Ens-256 99.0 90.0 83.0 87.0 14.57±\pm3.91 11.53±\pm6.96 10.04±\pm7.85 10.71±\pm8.91 10.19±\pm2.74 7.95±\pm4.94 6.86±\pm5.47 7.30±\pm6.07
Ours-noCal 100.0 94.0 86.0 88.0 14.50±\pm3.86 11.89±\pm7.34 10.27±\pm7.75 11.35±\pm9.34 10.18±\pm2.69 8.21±\pm5.18 7.02±\pm5.45 7.75±\pm6.40
Ours 100.0 96.0 93.0 92.0 14.65±\pm3.97 11.96±\pm7.08 11.71±\pm9.34 11.99±\pm10.25 10.30±\pm2.76 8.32±\pm5.04 7.99±\pm6.49 8.19±\pm6.96
  • *

    Without uncertainty tube (SCOPE). SR: success rate, T¯\overline{T}: avg. navigation time (successful episodes only), d¯\overline{d}: avg. path length (successful episodes only). Bold: best, underline: second best.

4.4 Case Study 3: Real-Robot Manipulation

A Franka reaches into a shelf to move a tomato, with 30 trials per method and condition. Static SCOPE succeeds in 25/30 trials, versus 15/30 for Diff-MPPI, using 500 rollouts. Adding hand forecasting in the dynamic condition gives 21/30 successes, versus 11/30 without a hand world model and 16/30 with the 64-sample ensemble. Environment and hand collisions can occur in the same trial; their reported rates must not be summed as disjoint outcomes. Remaining failures include inaccurate occupancy and becoming trapped near the shelf.

Complete original Table 3: Static setting.
Table 3: Static setting.
Accessible table and original citation links
Method SR↑\uparrow CR↓\downarrow TO↓\downarrow
Diffuser 0.47 0.53 0.0
Vanilla MPPI 0.17 0.80 0.03
Diff-MPPI 0.50 0.23 0.27
Ours 0.83 0.13 0.03
Complete original Table 4: Dynamic setting.
Table 4: Dynamic setting.
Accessible table and original citation links
Method SR↑\uparrow CRenv↓\downarrow CRhuman↓\downarrow TO↓\downarrow
Ours (no WM) 0.37 0.27 0.40 0.0
MID-Ens-64 0.53 0.27 0.27 0.00
MID-Ens-128 0.47 0.30 0.33 0.00
Ours 0.70 0.13 0.10 0.07

A single episode may incur both collision types.

5 Conclusion

The contribution is a compact bridge from diffusion predictions to controller queries: explicit modes, structured local uncertainty and inexpensive marginal covariance. The evidence supports useful low-dimensional forecasting and proposal design in the reported settings. It leaves broader observation modalities, out-of-distribution behavior and certified safety unresolved.

6 Limitations

The head inherits gaps in the learned diffusion distribution. Missing behaviors or poorly separated modes can make local tubes misleading. All experiments use relatively low-dimensional state and geometric context; applying the approach directly to images or point clouds is proposed future work. The controller retains nonzero collision rates.

Acknowledgments

The original acknowledgment names Singapore Ministry of Education support and French/Région Occitanie research funding. The first three authors contributed equally; Xue and Soh are corresponding authors.

Original project demonstrations

The primary SCOPE project page supplies 16 unchanged original videos with their original posters: an overview, four Maze2D comparisons, two crowd-navigation comparisons and nine shelf examples. Labels retain method and outcome, including collisions and a stuck baseline. They are selected demonstrations, rather than extra trials to add to reported success rates. The comparison scripts synchronize these existing videos and do not reveal additional video files.

SCOPE original overview demonstration
Vanilla MPPI Maze2D navigation
Log-MPPI Maze2D navigation
Diff-MPPI Maze2D navigation
SCOPE Maze2D navigation
MID-Ensemble-64 with latency-aware execution, including pedestrian P1 and P2 predictions
SCOPE with synchronous execution, including pedestrian P1 and P2 predictions
Diffusion: Box collision
Diff-MPPI: Box collision
SCOPE: Obstacle avoidance
Diffusion: Shelf collision
Diff-MPPI: Stuck side grasp
SCOPE: Tighter-layout success
No world model: Top-shelf collision
No world model: Hand collision
SCOPE: Early yielding

Appendix A Related Works

The related-work appendix places SCOPE among trajectory diffusion, generative uncertainty and sampling-based robot control. Its distinction is the explicit precision interface distilled from local score curvature. The source’s full 47-entry bibliography remains linked.

A.1 Diffusion models for trajectory prediction.

Trajectory diffusion predicts pedestrian futures and also generates robot motion conditioned on goals or geometry. Prior systems use guidance, interaction conditioning and learned planning potentials [1] [2] [16]. Those capabilities alone do not supply per-mode precision or enforce downstream physical constraints. SCOPE adds an uncertainty representation around the learned futures.

A.2 Uncertainty quantification from generative trajectory models.

The target is aleatoric uncertainty over plausible futures. This differs from epistemic uncertainty about model parameters or unfamiliar inputs. Sampling-based covariance is expensive; reverse-process variances also need not equal clean-trajectory variability. Conformal prediction addresses a different question through calibrated coverage sets. SCOPE’s NLL fitting is not a conformal coverage guarantee.

A.3 Structured uncertainty for sampling-based control.

MPPI and related controllers combine sampled trajectories with geometric or probabilistic costs. Gaussian-process and structured prediction methods motivate temporal sparsity. SCOPE uses a banded-plus-low-rank precision to expose efficient covariance queries, while retaining explicit geometric evaluation and forecast-based risk penalties in the controller.

Appendix B Method Details

The supplement gives the local Gaussian argument, precision structure, theoretical query complexity, categorical diffusion training, calibration and probe/HVP identities. The complete mathematical displays are retained, including unnumbered definitions that explain the numbered equations.

B.1 Laplace’s Method

Laplace’s method approximates each mode-conditioned density locally with a Gaussian. The argument requires a smooth log density near a nondegenerate maximum, with positive-definite negative Hessian. A deterministic decoded trajectory is treated as a near-mode anchor; the appendix tests that assumption empirically rather than proving every anchor is a mode.

Equations 8–10
p⁡(𝐲∣𝐜)\displaystyle p(\mathbf{y}\mid\mathbf{c}) =∑k=1Kp⁡(𝐲,z=k∣𝐜)\displaystyle=\sum_{k=1}^{K}p(\mathbf{y},z{=}k\mid\mathbf{c}) (8)
=∑k=1Kp⁡(z=k∣𝐜)​p​(𝐲∣𝐜,z=k)\displaystyle=\sum_{k=1}^{K}p(z{=}k\mid\mathbf{c})\,p(\mathbf{y}\mid\mathbf{c},z{=}k) (9)
≈∑k=1Kπk⋅𝒩⁡(𝝁k,𝚺k).\displaystyle\approx\sum_{k=1}^{K}\pi_{k}\cdot\mathcal{N}(\boldsymbol{\mu}_{k},\boldsymbol{\Sigma}_{k}). (10)

Local Taylor Expansion of the Log-Density.

At a genuine mode the score vanishes, removing the first-order Taylor term. The quadratic term then describes the local log-density shape. Away from a stationary point a linear term remains, so the nominal decode and the local quadratic center can differ. Keeping the expansion local matters when the complete mode-conditioned distribution is not exactly Gaussian.

Equations 11–13
ℓ⁡(𝐲):=log⁡p⁡(𝐲∣𝐜,z=k).\ell(\mathbf{y}):=\log p(\mathbf{y}\mid\mathbf{c},z{=}k).
𝝁k:=arg⁡max𝐲⁡ℓ⁡(𝐲)\boldsymbol{\mu}_{k}:=\arg\max_{\mathbf{y}}\ell(\mathbf{y})
∇𝐲ℓ​(𝐲)|𝐲=𝝁k=𝟎.\nabla_{\mathbf{y}}\ell(\mathbf{y})\big|_{\mathbf{y}=\boldsymbol{\mu}_{k}}=\mathbf{0}. (11)
𝜹=𝐲−𝝁k,\boldsymbol{\delta}=\mathbf{y}-\boldsymbol{\mu}_{k},
ℓ⁡(𝝁k+𝜹)\displaystyle\ell(\boldsymbol{\mu}_{k}+\boldsymbol{\delta}) =ℓ⁡(𝝁k)+(∇𝐲ℓ​(𝝁k)⏟=𝟎)⊤​𝜹+12​𝜹⊤​∇𝐲2ℓ​(𝝁k)​𝜹+o⁡(‖𝜹‖2)\displaystyle=\ell(\boldsymbol{\mu}_{k})+(\underbrace{\nabla_{\mathbf{y}}\ell(\boldsymbol{\mu}_{k})}_{=\mathbf{0}})^{\top}\boldsymbol{\delta}+\frac{1}{2}\boldsymbol{\delta}^{\top}\nabla_{\mathbf{y}}^{2}\ell(\boldsymbol{\mu}_{k})\boldsymbol{\delta}+{o}(\|\boldsymbol{\delta}\|^{2}) (12)
≈ℓ⁡(𝝁k)+12​𝜹⊤​∇𝐲2ℓ​(𝝁k)​𝜹.\displaystyle\approx\ell(\boldsymbol{\mu}_{k})+\frac{1}{2}\boldsymbol{\delta}^{\top}\nabla_{\mathbf{y}}^{2}\ell(\boldsymbol{\mu}_{k})\boldsymbol{\delta}. (13)

Recovering the Gaussian Approximation.

Negating the mode Hessian gives precision, and its inverse gives covariance. Exponentiating the second-order expansion recovers an approximate Gaussian after normalization. Positive semidefiniteness at a maximum is insufficient for an invertible covariance; the stated nondegeneracy assumption supplies strict positivity.

Equations 14–17
𝚲∗:=−∇𝐲2​ℓ​(𝝁k)=−∇𝐲2​log⁡p⁡(𝐲∣𝐜,z=k)|𝐲=𝝁k.\boldsymbol{\Lambda}^{*}:=-\nabla_{\mathbf{y}}^{2}\ell(\boldsymbol{\mu}_{k})=-\nabla_{\mathbf{y}}^{2}\log p(\mathbf{y}\mid\mathbf{c},z{=}k)\big|_{\mathbf{y}=\boldsymbol{\mu}_{k}}. (14)
∇𝐲2​ℓ​(𝝁k)=−𝚲∗\nabla_{\mathbf{y}}^{2}\ell(\boldsymbol{\mu}_{k})=-\boldsymbol{\Lambda}^{*}
log⁡p⁡(𝐲∣𝐜,z=k)≈ℓ⁡(𝝁k)−12​(𝐲−𝝁k)⊤​𝚲∗​(𝐲−𝝁k).\log p(\mathbf{y}\mid\mathbf{c},z{=}k)\approx\ell(\boldsymbol{\mu}_{k})-\frac{1}{2}(\mathbf{y}-\boldsymbol{\mu}_{k})^{\top}\boldsymbol{\Lambda}^{*}(\mathbf{y}-\boldsymbol{\mu}_{k}). (15)
p⁡(𝐲∣𝐜,z=k)≈exp⁡(ℓ⁡(𝝁k))​exp⁡(−12​(𝐲−𝝁k)⊤​𝚲∗​(𝐲−𝝁k)).p(\mathbf{y}\mid\mathbf{c},z{=}k)\approx\exp\!\big(\ell(\boldsymbol{\mu}_{k})\big)\exp\!\left(-\frac{1}{2}(\mathbf{y}-\boldsymbol{\mu}_{k})^{\top}\boldsymbol{\Lambda}^{*}(\mathbf{y}-\boldsymbol{\mu}_{k})\right). (16)
𝚺k=(𝚲∗)−1.\boldsymbol{\Sigma}_{k}=(\boldsymbol{\Lambda}^{*})^{-1}.
p⁡(𝐲∣𝐜,z=k)≈𝒩⁡(𝐲,𝝁k,(𝚲∗)−1).p(\mathbf{y}\mid\mathbf{c},z{=}k)\approx\mathcal{N}\!\big(\mathbf{y};\,\boldsymbol{\mu}_{k},\,(\boldsymbol{\Lambda}^{*})^{-1}\big). (17)

Anchor and curvature diagnostics.

The diagnostic uses 1,000 focal predictions from 20 held-out three-pedestrian episodes and the top three of six modes, giving 3,000 anchors. Raw symmetrized negative score Jacobians are positive definite at 2,843 anchors (94.8%). On that positive-definite subset, the weighted median quadratic-center shift is 1.33 cm. Querying learned covariance at the corrected center changes mixture NLL by 0.027. These observations support the approximation locally; they do not characterize away the remaining indefinite anchors.

B.2 Markov Sparsity in Trajectory Precision

For a Gaussian trajectory model, zero precision blocks encode conditional independence between times. A finite-order Markov assumption therefore motivates a block band. Learned diffusion trajectories can have longer-range dependence, so the low-rank term permits dense corrections beyond that band. This is a structured approximation rather than an assertion that all diffusion trajectories have exact Markov sparsity.

Equations 18–20
p(𝐲1:T)=p(𝐲1)∏t=2Tp(𝐲t∣𝐲t−1,…,𝐲max⁡(1,t−b)),p(\mathbf{y}_{1:T})=p(\mathbf{y}_{1})\prod_{t=2}^{T}p\!\left(\mathbf{y}_{t}\mid\mathbf{y}_{t-1},\ldots,\mathbf{y}_{\max(1,t-b)}\right), (18)
𝐲∼𝒩⁡(𝝁,𝚺),\mathbf{y}\sim\mathcal{N}(\boldsymbol{\mu},\boldsymbol{\Sigma}),
𝚲=𝚺−1,\boldsymbol{\Lambda}=\boldsymbol{\Sigma}^{-1},
𝚲i​j=0⟺𝐲i⟂𝐲j∣𝐲∖{i,j}.\boldsymbol{\Lambda}_{ij}=0\quad\Longleftrightarrow\quad\mathbf{y}_{i}\perp\mathbf{y}_{j}\mid\mathbf{y}_{\setminus\{i,j\}}. (19)
|i−j|>b.|i-j|>b.
𝚲i​j=0for|i−j|>b.\boldsymbol{\Lambda}_{ij}=0\quad\text{for}\quad|i-j|>b. (20)
Complete original Figure 5: Empirical structure of teacher precision matrices.
The left two plots and the right two plots correspond to two representative trajectories, respectively.
For each trajectory, we visualize the same precision matrix with a linear scale and a symmetric log scale.
The linear-scale views show strong diagonal dominance, while the log-scale views reveal banded local structure and off-band global patterns.
Figure 5: Empirical structure of teacher precision matrices. The left two plots and the right two plots correspond to two representative trajectories, respectively. For each trajectory, we visualize the same precision matrix with a linear scale and a symmetric log scale. The linear-scale views show strong diagonal dominance, while the log-scale views reveal banded local structure and off-band global patterns.

Empirical structure of teacher precision.

Figure 5 shows teacher precision for two representative trajectories at linear and symmetric-log scales. The plots exhibit diagonal dominance, near-diagonal structure and weaker off-band patterns. They motivate the chosen band-plus-low-rank parameterization; two examples alone do not establish that every teacher Hessian has that form or the chosen rank is sufficient.

B.3 Banded Cholesky Structure

A positive-definite block-banded matrix has a block-banded Cholesky factor with the same block bandwidth. Predicting that factor directly enforces the banded part’s positivity. For fixed block dimension and bandwidth, factorization scales linearly in horizon rather than cubically in the full flattened dimension. The low-rank addition handles remaining global coupling.

Equations
𝚲i​j=0whenever|i−j|>b,\boldsymbol{\Lambda}_{ij}=0\quad\text{whenever}\quad|i-j|>b,
𝚲=𝐋𝐋⊤,\boldsymbol{\Lambda}=\mathbf{L}\mathbf{L}^{\top},
𝐋i​j=0wheneveri−j>b.\mathbf{L}_{ij}=0\quad\text{whenever}\quad i-j>b.
𝐀ϕ=𝐋ϕ​𝐋ϕ⊤\mathbf{A}_{\phi}=\mathbf{L}_{\phi}\mathbf{L}_{\phi}^{\top}
O⁡(T​b2​d3),O(Tb^{2}d^{3}),
O⁡(D3)=O⁡(T3​d3).O(D^{3})=O(T^{3}d^{3}).
O⁡(T​b2​d3).O(Tb^{2}d^{3}).

B.4 Inference Complexity Analysis

The derivation combines banded triangular solves, a Woodbury correction and selected inverse blocks. Its total theoretical cost is O(Tb²d³ + Tbd²r + Tdr² + r³), linear in horizon T when bandwidth b, state dimension d and rank r stay fixed. Crucially, the reported experiments use dense Cholesky and dense solves because pedestrian trajectories have d=2, T=12 and flattened dimension 24; the appendix reports this operation below 0.5 ms. The linear-time structured solver is a scalable implementation path, not the implementation benchmarked in those experiments.

Equations 21
𝚲ϕ=𝐀ϕ+𝐑𝐑⊤,\boldsymbol{\Lambda}_{\phi}=\mathbf{A}_{\phi}+\mathbf{R}\mathbf{R}^{\top},
𝚺t​t∈ℝd×d\boldsymbol{\Sigma}_{tt}\in\mathbb{R}^{d\times d}
𝚺=𝚲ϕ−1.\boldsymbol{\Sigma}=\boldsymbol{\Lambda}_{\phi}^{-1}.
𝐀ϕ=𝐋𝐋⊤.\mathbf{A}_{\phi}=\mathbf{L}\mathbf{L}^{\top}.
𝚲ϕ=𝐀ϕ+𝐑𝐑⊤.\boldsymbol{\Lambda}_{\phi}=\mathbf{A}_{\phi}+\mathbf{R}\mathbf{R}^{\top}.
𝚲ϕ−1=𝐀ϕ−1−𝐀ϕ−1​𝐑​(𝐈r+𝐑⊤​𝐀ϕ−1​𝐑)−1​𝐑⊤​𝐀ϕ−1.\boldsymbol{\Lambda}_{\phi}^{-1}=\mathbf{A}_{\phi}^{-1}-\mathbf{A}_{\phi}^{-1}\mathbf{R}\left(\mathbf{I}_{r}+\mathbf{R}^{\top}\mathbf{A}_{\phi}^{-1}\mathbf{R}\right)^{-1}\mathbf{R}^{\top}\mathbf{A}_{\phi}^{-1}. (21)
O⁡(T​b​d2​r).O(Tbd^{2}r).
𝐌=𝐈r+𝐑⊤​𝐀ϕ−1​𝐑\mathbf{M}=\mathbf{I}_{r}+\mathbf{R}^{\top}\mathbf{A}_{\phi}^{-1}\mathbf{R}
O⁡(T​d​r2+r3),O(Tdr^{2}+r^{3}),
𝚺=𝚲ϕ−1,\boldsymbol{\Sigma}=\boldsymbol{\Lambda}_{\phi}^{-1},
O⁡(T​b2​d3).O(Tb^{2}d^{3}).
𝐁=𝐀ϕ−1​𝐑.\mathbf{B}=\mathbf{A}_{\phi}^{-1}\mathbf{R}.
𝐁t​𝐌−1​𝐁t⊤.\mathbf{B}_{t}\mathbf{M}^{-1}\mathbf{B}_{t}^{\top}.
O⁡(T​d​r2+T​d2​r).O(Tdr^{2}+Td^{2}r).
O⁡(T​b2​d3+T​b​d2​r+T​d​r2+r3).O\!\left(Tb^{2}d^{3}+Tbd^{2}r+Tdr^{2}+r^{3}\right).

B.5 Mixture-of-Diffusion Training Details

A discrete mode z and continuous noisy trajectory form the latent-variable model. Training introduces a posterior over modes, computes the diffusion variational term and enumerates the small mode set. The displayed weighted ELBO is augmented in implementation to encourage identifiable use of all modes.

Generative model.

The mode prior depends on context. Given a mode, reverse diffusion generates the clean trajectory from a Gaussian terminal noisy state. Conditioning combines context with the discrete mode, making global alternatives explicit before local uncertainty is attached.

Equation 22
p(𝐲0,𝐲1:𝖲,z∣𝐜)=pθ(z∣𝐜)p(𝐲𝖲)∏s=1𝖲pψ(𝐲𝗌−1∣𝐲𝗌,𝐡𝐜,z).p(\mathbf{y}_{0},\mathbf{y}_{1:\mathsf{S}},z\mid\mathbf{c})=p_{\theta}(z\mid\mathbf{c})\,p(\mathbf{y}_{\mathsf{S}})\prod_{s=1}^{\mathsf{S}}p_{\psi}(\mathbf{y}_{\mathsf{s}-1}\mid\mathbf{y}_{\mathsf{s}},\mathbf{h}_{\mathbf{c}},z). (22)

Variational distribution.

The variational distribution factors into a learned posterior over modes and the fixed forward noising process given the recorded clean trajectory. Future ground truth enters the training posterior but is unavailable to the inference prior. Matching the two is part of the KL term.

Equation 23
q(z,𝐲1:𝖲∣𝐜,𝐲0)=qη(z∣𝐜,𝐲0)q(𝐲1:𝖲∣𝐲0),q(z,\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{c},\mathbf{y}_{0})=q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})\,q(\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{y}_{0}), (23)

ELBO derivation.

Applying the variational lower bound yields expected mode-conditioned diffusion likelihood minus the categorical posterior/prior KL. It is a lower-bound derivation before changing the KL weight and adding regularizers. The original multi-line equations retain the complete factorization and expectation structure.

Equations 24–27
logp(𝐲0∣𝐜)=log∑z∫pθ(z∣𝐜)pψ(𝐲0:𝖲∣𝐡𝐜,z)d𝐲1:𝖲.\log p(\mathbf{y}_{0}\mid\mathbf{c})=\log\sum_{z}\int p_{\theta}(z\mid\mathbf{c})\,p_{\psi}(\mathbf{y}_{0:\mathsf{S}}\mid\mathbf{h}_{\mathbf{c}},z)\,d\mathbf{y}_{1:\mathsf{S}}. (24)
log⁡p⁡(𝐲0∣𝐜)\displaystyle\log p(\mathbf{y}_{0}\mid\mathbf{c}) ≥𝔼qη(z∣𝐜,𝐲0)q(𝐲1:𝖲∣𝐲0)[logpθ(z∣𝐜)+logpψ(𝐲0:𝖲∣𝐡𝐜,z)\displaystyle\geq\mathbb{E}_{q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})\,q(\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{y}_{0})}[\log p_{\theta}(z\mid\mathbf{c})+\log p_{\psi}(\mathbf{y}_{0:\mathsf{S}}\mid\mathbf{h}_{\mathbf{c}},z) (25)
−logqη(z∣𝐜,𝐲0)−logq(𝐲1:𝖲∣𝐲0)]\displaystyle\qquad\qquad\qquad\qquad\quad-\log q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})-\log q(\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{y}_{0})]
=−KL(qη(z∣𝐜,𝐲0)∥pθ(z∣𝐜))+𝔼qη​(z∣𝐜,𝐲0)[ℒdiff(𝐜,𝐲0;z)],\displaystyle=-\mathrm{KL}\!\big(q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})\,\|\,p_{\theta}(z\mid\mathbf{c})\big)+\mathbb{E}_{q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})}\!\big[\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y}_{0};z)\big], (26)
ℒdiff(𝐜,𝐲0;z):=𝔼q(𝐲1:𝖲∣𝐲0)[logpψ(𝐲0:𝖲∣𝐡𝐜,z)−logq(𝐲1:𝖲∣𝐲0)].\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y}_{0};z):=\mathbb{E}_{q(\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{y}_{0})}\!\left[\log p_{\psi}(\mathbf{y}_{0:\mathsf{S}}\mid\mathbf{h}_{\mathbf{c}},z)-\log q(\mathbf{y}_{1:\mathsf{S}}\mid\mathbf{y}_{0})\right]. (27)

Diffusion VLB expansion.

The diffusion variational term expands into prior matching, reverse-step KL terms and reconstruction. Every term is conditioned on context and mode. The source notes the standard expansion without replacing it with a new uncertainty objective for the backbone.

Discrete enumeration.

Because there are few categorical modes, the posterior expectation is summed exactly over z. This avoids a sampled categorical approximation for that part of training. It does not eliminate diffusion noise/time sampling in the conditional likelihood training.

Equation 28
𝔼qη​(z∣𝐜,𝐲0)​[ℒdiff​(𝐜,𝐲0,z)]=∑k=1Kqη​(z=k∣𝐜,𝐲0)​ℒdiff​(𝐜,𝐲0,z=k).\mathbb{E}_{q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})}\!\big[\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y}_{0};z)\big]=\sum_{k=1}^{K}q_{\eta}(z{=}k\mid\mathbf{c},\mathbf{y}_{0})\,\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y}_{0};z{=}k). (28)

Training loss.

The minimized base objective is posterior-weighted negative diffusion likelihood plus β times the mode KL. The final recipe adds a posterior mutual-information reward with coefficient 0.12 and a batch-marginal usage penalty decaying from 0.20 to 0.02. The latter penalizes deviation from uniform batch usage. These terms encourage diverse mode use, without proving semantic independence or perfect mode separation.

Equation 29
𝒥=−∑k=1Kqη(z=k∣𝐜,𝐲0)ℒdiff(𝐜,𝐲0;z=k)+βKL(qη(z∣𝐜,𝐲0)∥pθ(z∣𝐜)),\mathcal{J}=-\sum_{k=1}^{K}q_{\eta}(z{=}k\mid\mathbf{c},\mathbf{y}_{0})\,\mathcal{L}_{\text{diff}}(\mathbf{c},\mathbf{y}_{0};z{=}k)+\beta\,\mathrm{KL}\!\big(q_{\eta}(z\mid\mathbf{c},\mathbf{y}_{0})\,\|\,p_{\theta}(z\mid\mathbf{c})\big), (29)

B.6 Multimodal Backbone Diagnostics

Backbone diagnostics examine mode usage, sensitivity to mode count and the single-basin assumption within each mode. They use held-out junction contexts and distribution-shape tests. The results support the representation empirically within this setting; the diagnostic scope is narrower than a general unimodality theorem.

Complete original Figure 6: Multimodal backbone diagnostics. (a) Effective mode count during training. Vanilla omits MI and usage regularization, MI only adds the MI reward, and Final includes both. (b) Best-of-20 displacement errors for different mode counts. (c) Three highest-probability modes in one held-out context. Thin lines show eight samples per mode, and thick lines show means over 128 samples. (d) Endpoint density contours estimated from all 128 samples per mode.
Figure 6: Multimodal backbone diagnostics. (a) Effective mode count during training. Vanilla omits MI and usage regularization, MI only adds the MI reward, and Final includes both. (b) Best-of-20 displacement errors for different mode counts. (c) Three highest-probability modes in one held-out context. Thin lines show eight samples per mode, and thick lines show means over 128 samples. (d) Endpoint density contours estimated from all 128 samples per mode.

Mode usage.

Without mutual-information or usage regularization, only two of six modes are active and effective count is 1.84 on 1,000 held-out contexts. Mutual-information training activates all six with effective count 5.79; the final recipe reaches 5.90. Activity means highest posterior probability in at least 1% of contexts. Effective count is derived from aggregate posterior perplexity, not a per-context count of six equally likely futures.

Sensitivity to mode count.

Matched K=3/6/9 training uses 1,017 held-out contexts and best-of-20 displacement error. ADE/FDE improve from 0.057/0.085 m at K=3 to 0.048/0.074 m at K=9. These oracle-style sample metrics do not measure whether an online controller can select the best sample or whether its probability calibration improves equally.

Mode-conditioned unimodality.

For 200 held-out contexts, retained modes cover at least 95% prior mass and supply 128 trajectories each. Endpoint and trajectory-PCA KDE checks search for secondary basins across bandwidth changes and bootstrap resampling. Passing conditionals account for 93.34% of evaluated probability mass, with a context-bootstrap 95% interval of 89.94–96.40%. The residual mass and density-test limitations remain; the method does not assume a universally exact single Gaussian.

B.7 Trajectory-Level NLL Calibration

A disjoint 10% calibration split fits per-horizon covariance scales by full-trajectory mixture NLL; 70% trains the models and 20% is test data. Mode weights, means and raw covariance predictions are frozen. The objective improves likelihood scale without explicitly optimizing a coverage target or correcting mode probabilities.

Per-horizon inflation factors.

Each learned scale is exp(sₜ), ensuring positivity. Although called an inflation factor, it can exceed one or fall below one, widening or shrinking the covariance. Scale one leaves that horizon unchanged. There is no τ≥1 restriction in the displayed parameterization.

Equation 30
τt=exp⁡(st),t∈[T],\tau_{t}\;=\;\exp(s_{t}),\qquad t\in[T], (30)

Symmetric scaling of the full covariance.

The complete covariance is scaled symmetrically as DΣD, with sqrt(τₜ) repeated across spatial coordinates. This preserves symmetry, positive definiteness and correlation coefficients. Cross-time blocks scale by sqrt(τₛτₜ), retaining a coherent joint covariance instead of editing only marginal diagonal blocks.

Equations 31–32
𝐃=diag⁡(τ1,τ1,τ2,τ2,…,τT,τT)∈ℝ2​T×2​T,\mathbf{D}\;=\;\mathrm{diag}\!\left(\sqrt{\tau_{1}},\,\sqrt{\tau_{1}},\;\sqrt{\tau_{2}},\,\sqrt{\tau_{2}},\;\dots,\;\sqrt{\tau_{T}},\,\sqrt{\tau_{T}}\right)\in\mathbb{R}^{2T\times 2T}, (31)
𝚺kcal=𝐃​𝚺k​𝐃.\boldsymbol{\Sigma}_{k}^{\text{cal}}=\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}. (32)

Trajectory-level GMM log-likelihood.

The Gaussian likelihood is evaluated over the flattened complete trajectory. Log-sum-exp over modes occurs once for the whole horizon. Summing separate per-time mixture likelihoods would implicitly allow modes to switch between timesteps and discard the modeled joint dependence. The determinant and quadratic-form identities exploit D while retaining a Cholesky factor of the original covariance.

Equations 33–36
log⁡𝒩⁡(𝐲,𝝁k,𝐃​𝚺k​𝐃)=−12​[2​T​log⁡(2​π)+log⁡|𝐃​𝚺k​𝐃|+Δk⊤​(𝐃​𝚺k​𝐃)−1​Δk],\log\mathcal{N}\!\bigl(\mathbf{y}\,;\,\boldsymbol{\mu}_{k},\,\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr)\;=\;-\tfrac{1}{2}\Bigl[2T\log(2\pi)+\log\bigl|\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr|+\Delta_{k}^{\!\top}\bigl(\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr)^{-1}\Delta_{k}\Bigr], (33)
log⁡|𝐃​𝚺k​𝐃|\displaystyle\log\bigl|\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr| = 2​∑t=1Tlog⁡τt+log⁡|𝚺k|,\displaystyle\;=\;2\sum_{t=1}^{T}\log\tau_{t}\;+\;\log\bigl|\boldsymbol{\Sigma}_{k}\bigr|, (34)
Δk⊤​(𝐃​𝚺k​𝐃)−1​Δk\displaystyle\Delta_{k}^{\!\top}\bigl(\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr)^{-1}\Delta_{k} =(𝐃−1​Δk)⊤​𝚺k−1​(𝐃−1​Δk).\displaystyle\;=\;\bigl(\mathbf{D}^{-1}\Delta_{k}\bigr)^{\!\top}\boldsymbol{\Sigma}_{k}^{-1}\bigl(\mathbf{D}^{-1}\Delta_{k}\bigr). (35)
log⁡p⁡(𝐲)=logsumexpk∈[K][log⁡πk+log⁡𝒩⁡(𝐲|𝝁k,𝐃​𝚺k​𝐃)].\log p(\mathbf{y})\;=\;\operatorname*{logsumexp}_{k\in[K]}\!\Bigl[\,\log\pi_{k}\;+\;\log\mathcal{N}\!\bigl(\mathbf{y}\,\big|\,\boldsymbol{\mu}_{k},\,\mathbf{D}\boldsymbol{\Sigma}_{k}\mathbf{D}\bigr)\,\Bigr]. (36)

Calibration objective.

The calibration loss is average joint trajectory NLL plus an ℓ₂ penalty on log scales. Regularization shrinks scales toward one, limiting extreme covariance adjustments on the calibration split. Only the T scalar log scales receive gradients; the backbone, precision head and mixture probabilities remain fixed.

Equation 37
ℒ(𝐬)=−1B∑b=1Blogp(𝐲(b);𝐬)+λreg∑t=1Tst2.\mathcal{L}(\mathbf{s})\;=\;-\,\frac{1}{B}\sum_{b=1}^{B}\log p\!\left(\mathbf{y}^{(b)};\,\mathbf{s}\right)\;+\;\lambda_{\text{reg}}\sum_{t=1}^{T}s_{t}^{2}. (37)

Optimisation and data split.

Adam fits the log scales for a fixed iteration count on the separate calibration predictions. All reported metrics use the disjoint test partition. The resulting rescaling is inexpensive at inference, but held-out fitting alone does not provide distribution-free guarantees under future shift.

B.8 Hutchinson Trace Estimator: From Matrix Matching to Probe Matching

Direct dense Hessian supervision would be costly. Hutchinson probe matching replaces the squared matrix error with an expected squared error in how the two matrices act on isotropic random vectors. The equality holds in expectation over probes, not as an exact equality for each finite sampled training batch.

Equation 38
minϕ⁡‖𝚲ϕ​(𝐲)−𝚲∗​(𝐲)‖F2.\min_{\phi}\;\|\boldsymbol{\Lambda}_{\phi}(\mathbf{y})-\boldsymbol{\Lambda}^{*}(\mathbf{y})\|_{F}^{2}. (38)

Hutchinson identity.

For error matrix M and E[uuᵀ]=I, E‖Mu‖² equals tr(MᵀM)=‖M‖²F. Gaussian probes satisfy the isotropy condition. This explains the matrix-matching target without materializing the teacher Hessian; finite-probe variance and approximation of the teacher score remain separate issues.

Equations 39–41
‖𝐌‖F2=tr⁡(𝐌⊤​𝐌).\|\mathbf{M}\|_{F}^{2}=\mathrm{tr}(\mathbf{M}^{\top}\mathbf{M}). (39)
𝔼𝐮​[‖𝐌𝐮‖22]\displaystyle\mathbb{E}_{\mathbf{u}}\!\left[\|\mathbf{M}\mathbf{u}\|_{2}^{2}\right] =𝔼𝐮​[𝐮⊤​𝐌⊤​𝐌​𝐮]\displaystyle=\mathbb{E}_{\mathbf{u}}\!\left[\mathbf{u}^{\top}\mathbf{M}^{\top}\mathbf{M}\,\mathbf{u}\right]
=tr⁡(𝐌⊤​𝐌​𝔼​[𝐮𝐮⊤])\displaystyle=\mathrm{tr}\!\left(\mathbf{M}^{\top}\mathbf{M}\;\mathbb{E}[\mathbf{u}\mathbf{u}^{\top}]\right)
=tr⁡(𝐌⊤​𝐌)\displaystyle=\mathrm{tr}(\mathbf{M}^{\top}\mathbf{M})
=‖𝐌‖F2.\displaystyle=\|\mathbf{M}\|_{F}^{2}. (40)
‖𝚲ϕ​(𝐲)−𝚲∗​(𝐲)‖F2=𝔼𝐮​[‖(𝚲ϕ​(𝐲)−𝚲∗​(𝐲))​𝐮‖22].\|\boldsymbol{\Lambda}_{\phi}(\mathbf{y})-\boldsymbol{\Lambda}^{*}(\mathbf{y})\|_{F}^{2}\;=\;\mathbb{E}_{\mathbf{u}}\!\left[\|(\boldsymbol{\Lambda}_{\phi}(\mathbf{y})-\boldsymbol{\Lambda}^{*}(\mathbf{y}))\mathbf{u}\|_{2}^{2}\right]. (41)

B.9 HVP Identity: Efficient Teacher Signal

The score derivative supplies the teacher’s response to a probe. Automatic differentiation of a scalar score/probe inner product returns a vector without constructing the full Hessian. The score is learned and evaluated at a small nonzero terminal noise level, so the practical target approximates clean-density curvature.

Score–curvature relationship.

For an exact conservative score, its Jacobian is the log-density Hessian. The negative Hessian is the local precision teacher. The implementation substitutes a small nonzero diffusion noise level for ideal time zero. A learned score is not guaranteed exactly conservative or positive definite at every queried anchor.

Equations 42–43
𝚲∗​(𝐲)=−∇𝐲2​log⁡p⁡(𝐲),\boldsymbol{\Lambda}^{*}(\mathbf{y})=-\nabla_{\mathbf{y}}^{2}\log p(\mathbf{y}), (42)
sθ​(𝐲,0)≈∇𝐲​log​p​(𝐲).s_{\theta}(\mathbf{y},0)\approx\nabla_{\mathbf{y}}\log p(\mathbf{y}). (43)

HVP derivation.

Differentiating s(y)ᵀu with respect to y yields the transposed score-Jacobian response, equal to the Hessian response for a true gradient score. One backward pass produces the desired vector under that assumption. The source’s curvature diagnostics symmetrize raw Jacobians; this distinction matters when interpreting the learned teacher as a mathematical precision.

Equation 44
𝚲∗​(𝐲)​𝐮\displaystyle\boldsymbol{\Lambda}^{*}(\mathbf{y})\,\mathbf{u} =[−∇𝐲2​log⁡p⁡(𝐲)]​𝐮\displaystyle=\left[-\nabla_{\mathbf{y}}^{2}\log p(\mathbf{y})\right]\mathbf{u}
=−∇𝐲[∇𝐲​log​p​(𝐲)⊤​𝐮]\displaystyle=-\nabla_{\mathbf{y}}\!\left[\nabla_{\mathbf{y}}\log p(\mathbf{y})^{\top}\mathbf{u}\right]
≈−∇𝐲(sθ​(𝐲,0)⊤​𝐮).\displaystyle\approx-\nabla_{\mathbf{y}}\!\left(s_{\theta}(\mathbf{y},0)^{\top}\mathbf{u}\right). (44)

Final training loss.

The head’s precision/probe product is matched to the negative gradient of the frozen score/probe scalar. The backbone stays frozen while the head learns. Hutchinson supplies the expected Frobenius interpretation and autodiff supplies the teacher response. The positive-definite student structure can approximate the signal without making every raw teacher matrix positive definite.

Equation 45
ℒprec​(ϕ)=𝔼𝐲​𝔼𝐮​[‖𝚲ϕ​(𝐲)​𝐮+∇𝐲(sθ​(𝐲,0)⊤​𝐮)‖22].\mathcal{L}_{\text{prec}}(\phi)\;=\;\mathbb{E}_{\mathbf{y}}\,\mathbb{E}_{\mathbf{u}}\!\left[\left\|\boldsymbol{\Lambda}_{\phi}(\mathbf{y})\mathbf{u}+\nabla_{\mathbf{y}}\!\left(s_{\theta}(\mathbf{y},0)^{\top}\mathbf{u}\right)\right\|_{2}^{2}\right]. (45)

Appendix C MPPI Control Details

The control appendix retains all costs, the collision-probability derivation and the five-step tube-to-proposal algorithm. World-model risk and policy-side exploration use different parts of the Gaussian representation. Neither generated trajectories nor likelihood calibration replace closed-loop geometric checks.

C.1 MPPI Cost Function Design

Junction MPPI sums weighted goal distance, soft wall avoidance and Gaussian-mixture risk costs across the planning horizon. It rolls deterministic robot dynamics for candidate action sequences. A smooth penalty encourages acceptable predicted risk; feasibility is not enforced as a hard optimization constraint.

Equation 46
S(k)=wgoal​Jgoal(k)+wwall​Jwall(k)+wgmm​Jgmm(k),S^{(k)}\;=\;w_{\text{goal}}\,J_{\text{goal}}^{(k)}\;+\;w_{\text{wall}}\,J_{\text{wall}}^{(k)}\;+\;w_{\text{gmm}}\,J_{\text{gmm}}^{(k)}, (46)

C.1.1 Goal-Reaching Cost

The goal term sums Euclidean robot-to-goal distance at every horizon step. It guides intermediate states rather than evaluating only the terminal point. This junction cost differs from Maze2D’s squared-distance and large binary wall penalty, whose separate settings remain in the experimental appendix.

Equation 47
Jgoal=∑t=1H∥𝐩t−𝐩goal∥2,J_{\text{goal}}=\sum_{t=1}^{H}\bigl\lVert\mathbf{p}_{t}-\mathbf{p}_{\text{goal}}\bigr\rVert_{2}, (47)

C.1.2 Wall-Avoidance Cost

A geometry module returns wall clearance. Softplus penalizes falling below a comfortable clearance margin, with a width parameter smoothing the transition. Its tail decays outside the margin but remains positive; it is a soft barrier rather than an impenetrable geometric constraint.

Equation 48
Jwall=∑t=1Hsoftplus⁡(dwall−ctϵ),J_{\text{wall}}=\sum_{t=1}^{H}\operatorname{softplus}\!\left(\frac{d_{\text{wall}}-c_{t}}{\epsilon}\right), (48)

C.1.3 GMM Chance-Constrained Collision Cost

For each time, a union bound sums collision probabilities across agents, and each mode’s disk-collision event is bounded by a separating half-space Gaussian probability. The weighted sum can exceed one and is used as a conservative risk surrogate. Squared softplus penalizes exceeding budget δ. If the modeled bound is below δ then the model’s per-time collision probability is bounded, but MPPI’s finite-weight soft objective need not satisfy that condition. The statement is neither a whole-horizon probability guarantee nor a calibrated real-world safety guarantee.

Equations 49–55
Cn​(t):={ω∈Ω:‖𝐩t(n)​(ω)−𝐩t‖≤r},C_{n}(t):=\{\omega\in\Omega:\|\mathbf{p}_{t}^{(n)}(\omega)-\mathbf{p}_{t}\|\leq r\}, (49)
C⁡(t):=⋃n=1NpCn​(t).C(t):=\bigcup_{n=1}^{N_{p}}C_{n}(t). (50)
Pr⁡(C⁡(t))=Pr⁡(⋃n=1NpCn​(t))≤∑n=1NpPr⁡(Cn​(t)).\Pr(C(t))=\Pr\left(\bigcup_{n=1}^{N_{p}}C_{n}(t)\right)\leq\sum_{n=1}^{N_{p}}\Pr(C_{n}(t)). (51)
Pr⁡(Cn​(t))≤∑k=1Kπk(n)⋅Φ⁡(r−dk,t(n)σk,t(n)).\Pr(C_{n}(t))\leq\sum_{k=1}^{K}{\pi_{k}^{(n)}\cdot\Phi\left(\frac{r-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\right)}. (52)
Prisk​(t):=∑n=1Np∑k=1Kπk(n)⋅Φ⁡(r−dk,t(n)σk,t(n)).P_{\text{risk}}(t):=\sum_{n=1}^{N_{p}}\sum_{k=1}^{K}\pi_{k}^{(n)}\cdot\Phi\left(\frac{r-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\right). (53)
Pr⁡(C⁡(t))≤Prisk​(t)≤δ.\Pr(C(t))\leq P_{\text{risk}}(t)\leq\delta. (54)
Jgmm=∑t=1H[softplus⁡(Prisk​(t)−δτ)]2,J_{\text{gmm}}=\sum_{t=1}^{H}\left[\operatorname{softplus}\!\left(\frac{P_{\text{risk}}(t)-\delta}{\tau}\right)\right]^{2}, (55)

C.2 Derivation of the Collision Probability Upper Bound (one agent)

The collision disk lies inside the half-space perpendicular to the mean separation direction. Projecting each Gaussian onto that direction gives a scalar normal CDF bound; mode probabilities then combine the bounds. The derivation is valid for the assumed Gaussian-mixture marginal and disk geometry, including correlated agents through the union bound without requiring their independence. The displayed normalized direction is undefined at zero mean separation unless an implementation adds a convention; the source does not specify one here.

Equations 56–70
𝐩t(n)∼∑k=1Kπk(n)​𝒩​(𝝁k,t(n),𝚺k,t(n)).\mathbf{p}_{t}^{(n)}\sim\sum_{k=1}^{K}\pi_{k}^{(n)}\mathcal{N}(\boldsymbol{\mu}_{k,t}^{(n)},\boldsymbol{\Sigma}_{k,t}^{(n)}). (56)
C:={𝐱∈ℝ2:‖𝐱−𝐩t‖≤r},C:=\{\mathbf{x}\in\mathbb{R}^{2}:\|\mathbf{x}-\mathbf{p}_{t}\|\leq r\}, (57)
𝝂k,t(n):=𝝁k,t(n)−𝐩t,\boldsymbol{\nu}_{k,t}^{(n)}:=\boldsymbol{\mu}_{k,t}^{(n)}-\mathbf{p}_{t}, (58)
ℋ:={𝐱∈ℝ2:𝝂^⊤​(𝐱−𝐩t)≤r}\mathcal{H}:=\{\mathbf{x}\in\mathbb{R}^{2}:\hat{\boldsymbol{\nu}}^{\top}(\mathbf{x}-\mathbf{p}_{t})\leq r\} (59)
Pr⁡(𝐱∈C)≤Pr⁡(𝐱∈ℋ).\Pr(\mathbf{x}\in C)\leq\Pr(\mathbf{x}\in\mathcal{H}). (60)
𝐱k−𝐩t∼𝒩⁡(𝝂k,t(n),𝚺k,t(n)).\mathbf{x}_{k}-\mathbf{p}_{t}\sim\mathcal{N}(\boldsymbol{\nu}_{k,t}^{(n)},\boldsymbol{\Sigma}_{k,t}^{(n)}). (61)
𝝂^⊤​(𝐱k−𝐩t)∼𝒩⁡(dk,t(n),(σk,t(n))2),\hat{\boldsymbol{\nu}}^{\top}(\mathbf{x}_{k}-\mathbf{p}_{t})\sim\mathcal{N}(d_{k,t}^{(n)},(\sigma_{k,t}^{(n)})^{2}), (62)
Pr⁡(𝐱k∈ℋ)\displaystyle\Pr(\mathbf{x}_{k}\in\mathcal{H}) =Pr⁡(𝝂^⊤​(𝐱k−𝐩t)≤r)\displaystyle=\Pr(\hat{\boldsymbol{\nu}}^{\top}(\mathbf{x}_{k}-\mathbf{p}_{t})\leq r) (63)
=Pr⁡(𝝂^⊤​(𝐱k−𝐩t)−dk,t(n)σk,t(n)≤r−dk,t(n)σk,t(n))\displaystyle=\Pr\left(\frac{\hat{\boldsymbol{\nu}}^{\top}(\mathbf{x}_{k}-\mathbf{p}_{t})-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\leq\frac{r-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\right) (64)
=Φ⁡(r−dk,t(n)σk,t(n))\displaystyle=\Phi\left(\frac{r-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\right) (65)
Pr⁡(Cn​(t))\displaystyle\Pr(C_{n}(t)) =Pr⁡(𝐩t(n)∈C)\displaystyle=\Pr(\mathbf{p}_{t}^{(n)}\in C) (66)
=∑k=1KPr⁡(z=k)​Pr⁡(𝐩t(n)∈C∣z=k)\displaystyle=\sum_{k=1}^{K}\Pr(z=k)\Pr(\mathbf{p}_{t}^{(n)}\in C\mid z=k) (67)
=∑k=1Kπk(n)​Pr⁡(𝐱k∈C)\displaystyle=\sum_{k=1}^{K}\pi_{k}^{(n)}\Pr(\mathbf{x}_{k}\in C) (68)
≤∑k=1Kπk(n)​Pr⁡(𝐱k∈ℋ)\displaystyle\leq\sum_{k=1}^{K}\pi_{k}^{(n)}\Pr(\mathbf{x}_{k}\in\mathcal{H}) (69)
=∑k=1Kπk(n)⋅Φ⁡(r−dk,t(n)σk,t(n)).\displaystyle=\sum_{k=1}^{K}\pi_{k}^{(n)}\cdot\Phi\left(\frac{r-d_{k,t}^{(n)}}{\sigma_{k,t}^{(n)}}\right). (70)

C.3 SCOPE Algorithm Details

Policy-side control starts from a single global path, follows it with forward-progress subgoals and caches a time-varying proposal covariance. These details are important: SCOPE’s benefit is not evaluated as covariance attached to a controller with no path initialization or tracking logic.

C.3.1 Notation and overview

The path is resampled and cumulative arclength computed once per episode. Single-integrator dynamics relate 2D velocity controls to successive states. The precision head is queried once on the selected path, rather than at every local rollout state.

C.3.2 Nominal MPPI control sequence

The initial MPPI nominal sequence follows velocities along the diffusion path. Later control steps shift the current nominal forward and refine it through weighted sampled perturbations. This preserves global guidance while allowing local deviation from the generated reference.

Warm-start from diffusion path.

Initial actions are adjacent waypoint differences divided by Δt, with the final action copied from its predecessor. The first rollout batch is therefore centered on a learned path rather than zero/random controls. Diff-MPPI shares the global-path reference in the comparison.

Equation 71
U(0)t=pt+1−ptΔ​t,t=0,…,H−2,U^{(0)}_{t}\;=\;\frac{p_{t+1}-p_{t}}{\Delta t},\qquad t=0,\ldots,H-2, (71)

Receding-horizon refinement.

At each receding-horizon step, Gaussian perturbations use the cached tube-shaped covariance and the updated nominal. MPPI evaluates goal/subgoal tracking and wall penalties, then applies the weighted action correction. The generated reference remains a soft guide rather than a hard executed trajectory.

C.3.3 Subgoal selection

Subgoals use a monotone nearest-path index followed by arclength lookahead. This prevents jumping backward when a path crosses itself and gives the local controller a forward target. It also constrains recovery: a short-horizon controller can still become stuck when reaching safety requires backing out.

Monotone nearest index.

The nearest waypoint search is limited to a forward window of 20 waypoints from the previous progress index. This enforces nondecreasing path progress. It is an execution heuristic rather than a proof of global progress or successful escape from every concave obstacle configuration.

Equation 72
it=arg⁡minj∈[it−1,min⁡(it−1+W,L−1)]⁡‖pj−xt‖2,i0=0.i_{t}\;=\;\arg\min_{j\in[\,i_{t-1},\ \min(i_{t-1}+W,\ L-1)\,]}\;\|p_{j}-x_{t}\|_{2},\qquad i_{0}=0. (72)

Arclength-based lookahead.

The next target is the first waypoint at least the chosen arclength ahead. Near the path end, or when less path remains than that lookahead, the target switches to the true goal. Maze2D uses a 0.4-unit lookahead.

Equation 73
itsg=min⁡{j≥it:sj≥sit+Ldist}.i^{\text{sg}}_{t}\;=\;\min\!\left\{\,j\geq i_{t}\;:\;s_{j}\;\geq\;s_{i_{t}}+L_{\text{dist}}\,\right\}. (73)

C.3.4 Precision tubes for exploration covariance

State marginals are rescaled back to configuration space and used to shape action exploration. Covariance windows and sampling factors are cached once per episode. Although the model learns full temporal precision, the displayed proposal mapping uses next-waypoint marginal covariance; the controller does not directly sample the complete learned joint trajectory Gaussian.

State-to-action mapping.

The rule Σᵘᵢ=(γ²/Δt²)Σˣᵢ₊₁ narrows the tube with γ below one and converts units for single-integrator velocity controls. Algorithm 1 separates episode-level covariance/factor preparation from per-step window retrieval and sampling. The original PDF’s five numbered steps and unnumbered headings are preserved in the native layout.

Equations 74–75
{Σix}i=0L−1=SCOPE(p0:L−1,zwall),\{\Sigma^{x}_{i}\}_{i=0}^{L-1}\;=\;\mathrm{SCOPE}\!\big(p_{0:L-1},\ z_{\text{wall}}\big), (74)
Σiu=γ2Δ​t2​Σi+1x,\Sigma^{u}_{i}\;=\;\frac{\gamma^{2}}{\Delta t^{2}}\;\Sigma^{x}_{i+1}, (75)
Original five-step Algorithm 1, including unnumbered per-episode and per-control-step headings.
Algorithm 1 Precision tube to MPPI exploration covariance.
Original selectable algorithm
1: Per episode (after the diffusion path p0:L−1p_{0:L-1} is selected):
2: {Σix}←SCOPE(p0:L−1,zwall)\{\Sigma^{x}_{i}\}\leftarrow\mathrm{SCOPE}(p_{0:L-1},\,z_{\text{wall}}); rescale to configuration space
3: Σiu←(γ2/Δ​t2)​Σi+1x\Sigma^{u}_{i}\leftarrow(\gamma^{2}/\Delta t^{2})\,\Sigma^{x}_{i+1} for i=0,…,L−2i=0,\ldots,L-2 ⊳\triangleright Eq. (75)
4: Precompute Cholesky factors for sliding windows of length HH
5: Per control step tt:
6: Retrieve the cached covariance window at path index iti_{t}.
7: Sample δ​ut(k)∼𝒩⁡(0,Σtu)\delta u^{(k)}_{t}\sim\mathcal{N}(0,\,\Sigma^{u}_{t}) via cached Cholesky factor

Appendix D Experiment Details

The experiment appendix supplies the complete accuracy, configuration, timing and navigation tables. Junction forecasting and closed-loop control reuse the same checkpoints; Maze2D trains an independent prior/head. The hardware system adapts the policy-side model to eight-dimensional joint/gripper trajectories and adds hand prediction for the dynamic condition.

Complete original Table 5: Trajectory accuracy (ADE / FDE). All SCOPE variants share the same backbone and achieve identical ADE/FDE. Bold : best.
Table 5: Trajectory accuracy (ADE / FDE). All SCOPE variants share the same backbone and achieve identical ADE/FDE. Bold: best.
Accessible table and original citation links
Method Np=3N_{p}{=}3 Np=6N_{p}{=}6 Np=9N_{p}{=}9 Np=12N_{p}{=}12
LKF 0.55/1.22 0.48/1.03 0.41/0.92 0.30/0.67
MID-Ens 0.07/0.12 0.12/0.23 0.10/0.21 0.12/0.24
Ours 0.07/0.11 0.10/0.18 0.10/0.20 0.12/0.24

D.1 Shared Y-Corridor Setup

Trajectron++ history/social encoders provide a 256-dimensional context. Six discrete modes condition a 100-step diffusion model, decoded in five DDIM steps. The frozen backbone supplies context/mode features to the precision head, followed by trajectory-level calibration. Forecasting and navigation share the dedicated 70/20/10 train/test/calibration split.

D.2 Forecaster Quality on Y-Corridor

Four ORCA datasets use Δt=0.4 s, eight observed steps and twelve predicted steps. Evaluation caps scene contributions and collects up to 1,000 test pairs per density; sequence means are then summarized across scenes. MID ensembles fit three full-covariance components to 64/128/256 samples. SCOPE retains the three most probable of six modes, renormalizes their weights, decodes once per retained mode and predicts covariance directly. Mode weights themselves are not calibrated. Best-of-mean ADE and FDE select minima separately.

Complete original Table 6: Hyperparameters for Y-corridor experiments (Q1 and Q3). Controller and cost settings apply to Q3 only; all other settings are shared.
Table 6: Hyperparameters for Y-corridor experiments (Q1 and Q3). Controller and cost settings apply to Q3 only; all other settings are shared.
Accessible table and original citation links
Category Setting Value
Data & model Timestep Δ​t\Delta t 0.40.4 s
Observation horizon HobsH_{\text{obs}} 88 steps (3.23.2 s)
Prediction horizon TT 1212 steps (4.84.8 s)
Discrete modes KzK_{z} / retained KK 66 / 33
Diffusion steps (train / infer) 100100 / 55 (DDIM)
Baselines MID-Ens trajectory samples MM 64,128,25664,128,256
MID-Ens GMM components KK 33
Calibration Data split (train / test / cal) 70%70\% / 20%20\% / 10%10\%
Controller MPPI rollouts NN 10241024
Planning horizon HH 44 steps (1.61.6 s)
MPPI temperature λ\lambda 1.51.5
Noise correlation (AR(1)) ρ\rho 0.80.8
Per-axis velocity limit ±0.6\pm 0.6 m/s
Cost weights Goal weight wgoalw_{\text{goal}} 5050
Wall weight wwallw_{\text{wall}} 10001000
GMM weight wgmmw_{\text{gmm}} 3030
Chance constraint Risk budget δ\delta 0.10.1
Softplus temperature τ\tau 0.10.1
Geometry Robot / pedestrian collision radius rr/rhr_{r}/r_{h} 0.30.3 m / 0.30.3 m
Wall clearance margin dwalld_{\text{wall}} 0.250.25 m
Softplus width ϵ\epsilon 0.050.05 m

D.3 Inference Time Comparison

All ensemble diffusion calls are fully batched on an A5000. Total times are 26.581/47.527/91.779 ms for 64/128/256 samples, versus 12.87 ms for SCOPE; increased crowd size adds latency, with a reported roughly 200 ms at nine pedestrians and 256 samples. One decode per retained mode is batched, rather than one new Monte Carlo ensemble. During delayed updates pedestrians keep moving and the controller interpolates the previous forecast. Figure 7’s marginal confidence-ellipse coverage improves after NLL scaling, but the source explicitly calls it a diagnostic without formal coverage guarantees.

Complete original Table 7: Inference time breakdown ( N p = 3 N_{p}{=}3 ): MID-Ensemble at M ∈ { 64,128,256 } M\in\{64,128,256\} samples versus Banded+LR (Ours). All diffusion passes are fully batched on a single GPU. Ours shares the same backbone but avoids repeated sampling, yielding consistently lower total latency.
Table 7: Inference time breakdown (Np=3N_{p}{=}3): MID-Ensemble at M∈{64,128,256}M\in\{64,128,256\} samples versus Banded+LR (Ours). All diffusion passes are fully batched on a single GPU. Ours shares the same backbone but avoids repeated sampling, yielding consistently lower total latency.
Accessible table and original citation links
Method Diffusion [ms] Head/GMM [ms] Calibration [ms] Total [ms]
MID-Ens (M=64M{=}64) 26.246 0.336 – 26.581
MID-Ens (M=128M{=}128) 47.083 0.444 – 47.527
MID-Ens (M=256M{=}256) 91.222 0.558 – 91.779
Banded+LR (Ours) 12.19 0.60 0.08 12.87
Complete original Figure 7: Coverage diagnostic on the 3-pedestrian Y-junction. NLL calibration improves empirical coverage relative to the uncalibrated tubes, but is not coverage-targeted and does not provide formal coverage guarantees.
Figure 7: Coverage diagnostic on the 3-pedestrian Y-junction. NLL calibration improves empirical coverage relative to the uncalibrated tubes, but is not coverage-targeted and does not provide formal coverage guarantees.

D.4 Maze2D MPPI Sample-Efficiency

Maze2D uses a 5×5 workspace, seven rectangular walls, single-integrator dynamics at 0.1 s and a 0.01 robot radius. A shared prior trains on about 2.34 million trajectories from 3,000 mazes. Evaluation has 100 held-out mazes with 20 fixed start/goal pairs each, repeated across nine rollout budgets from four to 1,024. Each method uses one decoded style/path, with no best-of-candidate selection. Success requires reaching within 0.3 of the goal by 200 steps and remaining collision-free at waypoints plus 0.05-spaced interpolants. The head’s trace-variation regularizer prevents a reported nearly isotropic collapse in preliminary training.

Complete original Table 8: Hyperparameters for the Maze2D experiment (Q2). The diffusion prior is shared by Diff-MPPI and Ours;
the Precision Tube Head is exclusive to Ours; the MPPI block applies to
all four methods.
Table 8: Hyperparameters for the Maze2D experiment (Q2). The diffusion prior is shared by Diff-MPPI and Ours; the Precision Tube Head is exclusive to Ours; the MPPI block applies to all four methods.
Accessible table and original citation links
Diffusion trajectory prior (Diff-MPPI and Ours)
Backbone 1D temporal U-Net dim 6464, mults (1,4,8)(1,4,8)
Wall encoder 1D vision transformer embed 6464, depth 33
Horizon HH 4848
Diffusion steps (train / infer) 100100 / 55 (DDIM)
CFG dropout / sampling weight 0.20.2 / 2.02.0
Training steps ×1062\!\times\!10^{6}
EMA decay 0.9950.995
Precision Tube Head (Ours only)
Low-rank rank rr 2020
Markov order 44
Trace-CV regulariser (λ,floor)(\lambda,\text{floor}) (1.0,0.15)(1.0,0.15)
MPPI controller (all four methods)
Planning horizon TmppiT_{\text{mppi}} 1515
Temperature λ\lambda 0.1690.169
Timestep Δ​t\Delta t 0.10.1
Base noise σbase\sigma_{\text{base}} (vanilla / diff) 1.01.0
Base noise σbase\sigma_{\text{base}} (log) 0.80.8
Sample-count sweep NN 4,8,…,10244,8,\ldots,1024
Arclength lookahead LdistL_{\text{dist}} 0.40.4

D.4.1 Maze2D Navigation Results

At 1,024 rollouts SCOPE reports 99.25% success, 0.65% collision, 99.90% goal reaching and path length 4.996±0.763 m. Diff-MPPI reports 96.35%, 2.60%, 98.75% and 5.328±1.601 m. Goal reaching alone includes colliding executions; success excludes them. The 0.65% collision rate ties Log-MPPI rather than uniquely winning that metric. Restored Figure 8 shows selected challenging cases at each rollout budget; they illustrate behavior rather than provide a second unbiased test set.

Complete original Table 9: Maze2D navigation results. MPPI-based planners use N = 1024 N=1024 rollouts per planning step.
Table 9: Maze2D navigation results. MPPI-based planners use N=1024N=1024 rollouts per planning step.
Accessible table and original citation links
Planner SR↑\uparrow CR↓\downarrow GR↑\uparrow L¯\overline{L} [m]↓\downarrow
Diffuser 0.874 0.126 1.0 6.134
PB-Diff 0.887 0.113 1.0 6.042
Vanilla MPPI 0.9375 0.0125 0.9500 5.573 ±\pm 3.354
Log-MPPI 0.9455 0.0065 0.9510 5.994 ±\pm 4.506
Diff-MPPI 0.9635 0.0260 0.9875 5.328 ±\pm 1.601
SCOPE (Ours) 0.9925 0.0065 0.9990 4.996 ±\pm 0.763
  • •

    SR: success rate, CR: collision rate, GR: goal rate, L¯\overline{L}: avg. path length. Bold: best.

Complete original Figure 8: Qualitative comparison of MPPI variants on selected challenging Maze2D instances at different rollout budgets N ∈{4, 8, 16, 32, 64, 128, 256, 512, 1024} (rows). Within each row, all methods are evaluated on the same maze and start–goal pair. Green trajectories denote collision-free executions, while red trajectories denote collisions (×). The blue-shaded region around the SCOPE trajectory visualizes the predicted uncertainty tube. The blue dot and star denote the start and goal, respectively. For SCOPE and Diff-MPPI, the dashed gray line denotes the shared diffusion-prior trajectory. Across these selected cases, SCOPE reaches the goal without collision at all shown rollout budgets, whereas the baselines require larger rollout budgets in several cases and may still collide or become trapped in concave obstacle regions.
Figure 8: Qualitative comparison of MPPI variants on selected challenging Maze2D instances at different rollout budgets N ∈{4, 8, 16, 32, 64, 128, 256, 512, 1024} (rows). Within each row, all methods are evaluated on the same maze and start–goal pair. Green trajectories denote collision-free executions, while red trajectories denote collisions (×). The blue-shaded region around the SCOPE trajectory visualizes the predicted uncertainty tube. The blue dot and star denote the start and goal, respectively. For SCOPE and Diff-MPPI, the dashed gray line denotes the shared diffusion-prior trajectory. Across these selected cases, SCOPE reaches the goal without collision at all shown rollout budgets, whereas the baselines require larger rollout budgets in several cases and may still collide or become trapped in concave obstacle regions.

D.5 Y-Corridor Closed-Loop Navigation

The online junction simulator runs 100 episodes for each of four crowd densities. Every forecaster passes the same mean/covariance interface to a controller with 1,024 rollouts and four-step horizon. All diffusion decoding is batched; NF/LKF, unimodal/multimodal MID, ensembles and calibration ablation differ in forecasting. Success excludes collision and timeout, while distance/time include only successful trials. Source results do not supply independent training-seed confidence intervals for those success rates.

D.6 Direct NLL Training of the Precision Head

A direct-NLL precision head shares architecture, frozen backbone and means with SCOPE. Its navigation success is 100/91/84/81%, versus 100/96/93/92% for curvature training as crowd density rises. The proposed explanation is that NLL can widen covariance to absorb fixed-mean error, increasing the controller’s risk cost outside a collision radius. Figure 9 contains selected examples, so this mechanism is an interpretation consistent with the experiment rather than a universal causal proof against likelihood training.

Complete original Figure 9: Qualitative comparison of uncertainty tubes from Direct NLL PH (top) and SCOPE (bottom). Columns show selected examples with 3, 6, 9, and 12 pedestrians. Colors identify predicted modes. Shaded ellipses visualize uncertainty along each mode. Dashed gray lines show observed histories, and solid black lines show ground-truth futures. Each column uses matching axis limits across rows. Several Direct NLL PH tubes extend far beyond their nominal trajectories, whereas SCOPE produces more localized uncertainty in these cases.
Figure 9: Qualitative comparison of uncertainty tubes from Direct NLL PH (top) and SCOPE (bottom). Columns show selected examples with 3, 6, 9, and 12 pedestrians. Colors identify predicted modes. Shaded ellipses visualize uncertainty along each mode. Dashed gray lines show observed histories, and solid black lines show ground-truth futures. Each column uses matching axis limits across rows. Several Direct NLL PH tubes extend far beyond their nominal trajectories, whereas SCOPE produces more localized uncertainty in these cases.
Complete original Table 10: Navigation success rate across pedestrian densities. Bold indicates the best result for each density.
Table 10: Navigation success rate across pedestrian densities. Bold indicates the best result for each density.
Accessible table and original citation links
SR [%] ↑\uparrow
Method 3 pedestrians 6 pedestrians 9 pedestrians 12 pedestrians
Direct NLL PH 100 91 84 81
SCOPE (Ours) 100 96 93 92

D.7 Real-Robot Experiments

The complete hardware appendix retains calibrated geometry, task/trial definitions, model adaptation, mode selection and execution details. Static and dynamic tests share the shelf task; dynamic trials add a reaching hand. The demonstrations are selected qualitative examples alongside the full 30-trial tables.

Complete original Figure 10: Real-robot shelf setup for pick-and-place task.
The Franka arm operates in a constrained shelf workspace.
In this example, the OREO box is the human target and the tomato is the robot target, while the remaining objects serve as static obstacles for collision-aware MPPI control.
Figure 10: Real-robot shelf setup for pick-and-place task. The Franka arm operates in a constrained shelf workspace. In this example, the OREO box is the human target and the tomato is the robot target, while the remaining objects serve as static obstacles for collision-aware MPPI control.

D.7.1 Environment and Hardware

The platform is a seven-joint Franka Panda with Franka Hand gripper. Prebuilt cuboids represent the shelf and movable boxes, with poses aligned using wrist RealSense point clouds before execution. Two RGB webcams observe the hand in dynamic trials. Control uses an RTX 5090 workstation, rather than deriving the complete obstacle scene from raw images online.

D.7.2 Task Setup and Evaluation Protocol

Each method receives 30 trials per condition, with boxes repositioned near the tomato. A collision-free run that has not completed by 120 s is a timeout. Dynamic environment and hand collisions are counted separately and can co-occur. Front and tighter side approach modes vary in room for correction; the reported trials do not establish safety over arbitrary shelf geometry or unrestricted human motion.

Complete original Figure 11: Real-robot setup and representative approach modes.
(A) Front approach, where the arm reaches the target through a wider opening.
(B) Side approach, where the arm reaches the target with tighter clearance.
(C) Low-cost RGB webcam setup used for hand tracking in the dynamic experiments.
Figure 11: Real-robot setup and representative approach modes. (A) Front approach, where the arm reaches the target through a wider opening. (B) Side approach, where the arm reaches the target with tighter clearance. (C) Low-cost RGB webcam setup used for hand tracking in the dynamic experiments.

D.7.3 Controller and System Implementation

The system calibrates cuboid poses, generates joint-space candidate modes and uses tube shape both for mode selection and MPPI exploration. Hand forecasts add moving collision primitives. The appendix gives the 50 Hz controller rate but does not equate that with continuously regenerating the full diffusion path at 50 Hz.

Static scene calibration.

Point-cloud calibration establishes SE(3) poses for the fixed collision primitives before each trial. The controller keeps those primitives fixed during execution. Updating their poses when layouts change is part of the experimental setup, not an evaluated open-world perception capability.

Diffusion and SCOPE.

The robot diffusion/head predicts trajectories in eight dimensions: seven joint coordinates and one gripper coordinate. Scene conditioning becomes calibrated cuboid poses instead of a 2D wall layout. Joint-space tube estimates support the selected approach, without representing all possible environment or perception uncertainty.

Mode selection.

Of two approach modes, the system selects the one maximizing the minimum covariance eigenvalue encountered along its normalized trajectory. This favors flexibility at the tightest point. It is covariance-based mode selection, not simply choosing the most probable mode or proving the selected path has maximum physical clearance.

Equations
wk=mini⁡λmin​(Σk,i),w_{k}=\min_{i}\lambda_{\min}(\Sigma_{k,i}),
k⋆=arg⁡maxk∈𝒦⁡wk.k^{\star}=\arg\max_{k\in\mathcal{K}}w_{k}.

Tube-informed MPPI controller.

MPPI tracks a local point ahead on the selected smooth joint-space reference. Tube shape adjusts perturbations, while calibrated collision geometry checks rollouts. A constraint-aware score strongly downweights unsafe rollouts, inspired by COSMIK-MPPI, instead of relying solely on additive penalties. The soft reference permits deviations but finite-horizon recovery can still fail.

Complete original Figure 12: Representative real-robot failure cases. A. Static Diff-MPPI failure.
Without SCOPE, the planner lacks clearance- and flexibility-aware mode selection and may choose the tighter side approach.
Near the object, a potential shelf collision blocks further progress; limited-horizon MPPI cannot retreat and reorient, so the trial times out (TO). B. Dynamic failure of Ours (no WM), where SCOPE guides the robot policy but is not used as a human-motion world model.
Without hand prediction, the controller reacts only after the hand enters the shelf workspace, causing a late upward evasive motion that collides with the shelf.
Figure 12: Representative real-robot failure cases. A. Static Diff-MPPI failure. Without SCOPE, the planner lacks clearance- and flexibility-aware mode selection and may choose the tighter side approach. Near the object, a potential shelf collision blocks further progress; limited-horizon MPPI cannot retreat and reorient, so the trial times out (TO). B. Dynamic failure of Ours (no WM), where SCOPE guides the robot policy but is not used as a human-motion world model. Without hand prediction, the controller reacts only after the hand enters the shelf workspace, causing a late upward evasive motion that collides with the shelf.

Human hand tracking and prediction.

Two-camera markerless tracking supplies hand observations to a diffusion predictor. SCOPE augments its future trajectory with uncertainty, converted to time-varying collision primitives. The human model covers the hand relevant to this tabletop task, rather than the whole body or all possible human behavior.

Execution.

At trial start the system calibrates geometry, measures robot state, predicts candidate modes and chooses one. The 50 Hz MPPI loop then projects current state onto a forward-progress index and follows a lookahead reference, balancing collision, joint-limit and control costs. The robot is not committed to executing every diffusion waypoint open-loop.

D.7.4 Real-Robot Case Studies

Figure 12 illustrates a static timeout when a tight side approach leaves insufficient room to back out, and a dynamic no-world-model failure where late hand avoidance drives the arm into the shelf. Full hand prediction can prompt earlier yielding before entering that constrained region. These cases explain observed failure modes; full SCOPE still records static and dynamic collisions.

Concept demonstration

Open image ↗