Control-Ready Uncertainty for Trajectory Diffusion

1 Introduction

• Reliable uncertainty estimates from diffusion models commonly require repeated reverse-diffusion sampling, which can be too slow for closed-loop control.Reducing the sample count can improve speed but degrades estimation fidelity.Source: §1 · p. 1
• A lightweight precision head distills score-curvature information to produce per-timestep covariance estimates and analytic confidence regions around multiple trajectory hypotheses.At inference, one diffusion pass and one head evaluation produce a mixture of tubes for use in controllers.Source: §1 · p. 2, Figure 1 · p. 2
• SCOPE matches diffusion ensembles’ uncertainty quality in pedestrian forecasting and improves closed-loop control across navigation and real-robot manipulation settings.The evaluations include pedestrian forecasting, MPPI-guided maze exploration, crowded navigation, and Franka Panda manipulation.Source: §1 · p. 2

2 Preliminaries

• Diffusion trajectory generation progressively corrupts a clean trajectory with Gaussian noise, then learns reverse steps that denoise a trajectory conditioned on context.The denoiser can be viewed as learning the score of the trajectory density; at the terminal step it approximates the clean-data log-density gradient.Source: §2 · p. 2
• MPPI samples control perturbations, rolls them through the dynamics, and updates nominal controls with exponentially greater weight for lower-cost rollouts.The method is presented as a flexible, gradient-free planner.Source: §2 · p. 3
• MPPI performance depends heavily on its exploration covariance, which the paper identifies as a key limitation it addresses.Source: §2 · p. 3

3.1 Control Interface Representation: From Local Tubes to Mixtures

• The multimodal representation combines each nominal trajectory with a local tube, whose marginal covariance blocks provide per-timestep uncertainty while mode probabilities represent distinct futures.Together, the modes and tubes form a mixture usable for predictive world modeling and policy-side control guidance.Source: §3.1 · pp. 3–4

3.2 Multimodal Diffusion Backbone

• Explicit latent modes separate ambiguity over qualitatively different futures from local trajectory spread, avoiding a continuous prediction that can blur incompatible routes.The paper uses a discrete-latent multimodal diffusion backbone to produce hypotheses with explicit probabilities.Source: §3.2 · p. 4

3.3 Score-Curvature Precision Head

• Score curvature at a decoded near-modal trajectory serves as a local precision signal: sharp log-density curvature indicates a narrow tube, while flatter curvature indicates greater uncertainty.Under the local Laplace approximation, the trajectory distribution is represented as a Gaussian around the anchor.Source: §3.3 · pp. 4–5
• A lightweight head learns the teacher precision using score-curvature information, while Hessian-vector products and random probes avoid forming the dense teacher matrix directly.The second-order computation is moved to training time; inference uses a diffusion pass, a SCOPE pass, and structured covariance recovery.Source: §3.3 · p. 5
• The predicted precision uses block-banded structure plus a low-rank term to model nearby temporal coupling and longer-range correlations while enabling efficient marginal covariance queries.Banded Cholesky solves and a Woodbury update support those queries.Source: §3.3 · p. 5

3.4 Using SCOPE in Downstream Control

• For other agents, MPPI uses calibrated tube covariances to penalize collision probability; for robot policies, tube covariance adapts exploration over time.Larger covariance expands exploration in flexible regions, while smaller covariance contracts it in narrow or high-precision regions.Source: §3.4 · pp. 5–6

4.1 SCOPE Precision Head Properties

• SCOPE matches or improves ensemble uncertainty across crowd densities, with under-1-ms head and calibration overhead; MID-Ensemble is up to 7× slower.Banded precision models temporal correlations, while the low-rank term and calibration further improve trajectory-level NLL.Source: §4.1 · p. 6, Table 1 · p. 6

4.2 Case Study 1: Maze2D Navigation

• On non-convex mazes with distinct training and test layouts, SCOPE achieves the highest success rate and shortest paths across rollout budgets.Its tube expands in open regions and contracts in tight passages; fixed-covariance Diff-MPPI trades off escaping local basins against wall collisions.Source: §4.2 · pp. 6–7, Figure 4 · p. 7

4.3 Case Study 2: Y-Corridor Pedestrian Navigation

• Across four crowd densities, SCOPE has the best or tied-best success rate, especially with 9 and 12 pedestrians, while navigation time and path length remain comparable to strong baselines.Skipping calibration consistently lowers success rate in dense crowds.Source: §4.3 · pp. 7–8, Table 2 · p. 8

4.4 Case Study 3: Real-Robot Manipulation

• In static Franka trials SCOPE yields the highest success and lowest collision rates; dynamically, it outperforms ensemble baselines in success and human-collision avoidance.Each condition uses 30 trials; increasing MID-Ensemble from 64 to 128 samples slows prediction updates and delays response to human motion.Source: §4.4 · p. 8, Table 3 · p. 8, Table 4 · p. 8

• SCOPE makes diffusion uncertainty actionable for real-time robot control through score-curvature-based Gaussian tubes.Source: §5 · p. 9

6 Limitations

• SCOPE assumes a reliable diffusion backbone; limited training coverage or failure to learn separate modes can miscalibrate its tubes, and current experiments use low-dimensional inputs.Extending the method to raw images or point clouds is identified as future work.Source: §6 · p. 9

A.1 Diffusion models for trajectory prediction.

• Diffusion trajectory models generate multimodal futures as policies or world models, but most do not expose structured joint precision or fast per-timestep covariance for control.The related work includes motion forecasting, trajectory denoising, visuomotor policies, and reactive generation.Source: §A.1 · p. 13

A.2 Uncertainty quantification from generative trajectory models.

• Sampling and ensemble approaches estimate aleatoric trajectory uncertainty at high decoding cost, while state-wise uncertainty and conformal sets lack SCOPE’s joint precision interface.SCOPE targets per-mode structured precision with efficiently queried marginal covariances.Source: §A.2 · p. 13

A.3 Structured uncertainty for sampling-based control.

• Risk-aware MPPI methods either use limited predictive models or assume distributions are supplied externally; SCOPE instead derives structured uncertainty from diffusion score curvature.Its precision head uses HVP/Hutchinson supervision to expose marginal covariances to controllers.Source: §A.3 · p. 14

B.1 Laplace’s Method

• 94.8% of 3,000 curvature matrices were positive definite, and the mode-weighted median RMS shift to the local quadratic center was 1.33 cm.The Gaussian approximation relies on each mode-conditional density being locally Gaussian; moving covariance queries from that center to the decoded anchor increased mixture NLL by only 0.027.Source: §B.1 · pp. 14–15

B.2 Markov Sparsity in Trajectory Precision

• Teacher precision matrices show dominant near-diagonal bands plus off-band global patterns, motivating a block-banded term with a low-rank correction.The correction represents residual long-range correlations associated with goals, obstacles, and scene context.Source: §B.2 · p. 16, Figure 5 · p. 16

B.3 Banded Cholesky Structure

• A block-banded SPD precision has a Cholesky factor with the same bandwidth, so the parameterization is not restrictive within that matrix family.For fixed state dimension and bandwidth, factorization scales linearly with prediction horizon and supports efficient marginal covariance extraction.Source: §B.3 · p. 17

B.4 Inference Complexity Analysis

• With fixed block dimension, bandwidth, and low-rank dimension, structured marginal covariance extraction scales linearly with horizon; reported experiments instead use dense solves for D = 24.The dense implementation completes in under 0.5 ms on GPU; the banded analysis describes the scalable path for longer horizons.Source: §B.4 · p. 18

B.5 Mixture-of-Diffusion Training Details

• The multimodal diffusion backbone uses a discrete mode and a variational diffusion objective, with exact enumeration over modes and added mutual-information and mode-usage regularization.The MI reward coefficient is 0.12, while the usage-penalty coefficient decays from 0.20 to 0.02.Source: §B.5 · pp. 18–19

B.6 Multimodal Backbone Diagnostics

• Final training uses all six modes with effective usage 5.90, compared with 1.84 without regularization; MI alone yields 5.79.At K = 9, best-of-20 ADE/FDE are 0.048/0.074 m, versus 0.057/0.085 m at K = 3, and 93.34% of evaluated probability mass passes the single-basin diagnostic (95% CI 89.94%–96.40%).Source: §B.6 · pp. 20–21, Figure 6 · p. 20

B.7 Trajectory-Level NLL Calibration

• Calibration fits per-horizon covariance inflation factors by minimizing trajectory-level mixture NLL, preserving the shared mode and within-mode temporal correlations.The scales are fitted on a disjoint calibration split, with predicted mode weights, means, and raw covariances held fixed.Source: §B.7 · pp. 21–22

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

• Hutchinson probe matching replaces dense matrix supervision with random-direction responses while preserving Frobenius-norm matching in expectation.The teacher precision matrix need not be materialized.Source: §B.8 · pp. 22–23

B.9 HVP Identity: Efficient Teacher Signal

• A single autodiff backward pass computes the teacher curvature’s response to a probe without materializing the full Hessian.Together, the HVP and Hutchinson steps yield vector-level rather than matrix-level supervision.Source: §B.9 · p. 23

C.1 MPPI Cost Function Design

• MPPI evaluates sampled action sequences by forwarding their state trajectories through a stage cost and returning an importance-weighted action average.The appendix identifies goal, wall-avoidance, and collision terms as active cost components.Source: §C.1 · p. 23

C.1.1 Goal-Reaching Cost

• Summing goal distance across the planning horizon guides intermediate waypoints toward the target, rather than penalizing only the terminal state.The resulting cost has a smooth gradient.Source: §C.1.1 · p. 24

C.1.2 Wall-Avoidance Cost

• The softplus wall barrier decays exponentially beyond the comfortable clearance and rises steeply as clearance falls below it.The smoothing parameter controls barrier sharpness.Source: §C.1.2 · p. 24

C.1.3 GMM Chance-Constrained Collision Cost

• Collision avoidance uses a union-bound risk estimate and a squared-softplus penalty that is near zero below the per-timestep risk budget and grows quadratically above it.The soft penalty encourages the planner to shift probability mass toward safer trajectories.Source: §C.1.3 · pp. 24–25

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

• The collision region lies within a half-space on the robot’s side of the pedestrian’s mode position, enabling an upper bound on collision probability.The half-space is defined using the robot position, combined radii, and the direction from the mode position.Source: §C.2 · pp. 25–26

C.3.1 Notation and overview

• The navigation setup precomputes path arclength and queries SCOPE for per-waypoint state covariances along the global path.The robot follows single-integrator dynamics.Source: §C.3.1 · p. 26

C.3.2 Nominal MPPI control sequence

• MPPI initializes its nominal controls from diffusion-path velocities, then shifts and refines them using tube-covariance perturbations and standard weighted updates.This centers the initial rollouts on the diffusion-generated reference trajectory.Source: §C.3.2 · p. 26

C.3.3 Subgoal selection

• Subgoal selection tracks a forward-only waypoint index and chooses a target by arclength lookahead, switching to the true goal near the path end.The forward-only search avoids snapping backward at self-intersections.Source: §C.3.3 · pp. 26–27

C.3.4 Precision tubes for exploration covariance

• SCOPE’s per-waypoint state covariances are converted into time-varying action-sampling covariances and cached for lookup during control.The covariance windows are precomputed once per episode, and each control step retrieves the window using the current path position.Source: §C.3.4 · p. 27

D.2 Forecaster Quality on Y-Corridor

• All SCOPE variants share the same backbone and achieve identical ADE/FDE, so the listed trajectory-accuracy metrics do not distinguish their uncertainty designs.Table 5 reports trajectory accuracy as ADE/FDE.Source: Table 5 · p. 27

D Experiment Details

• Q1 and Q3 use the same MID checkpoints and Precision Tube Head, while Q2 uses an independently trained Maze2D diffusion prior and head.Q1 replays ground-truth trajectories; Q3 generates pedestrians online and supplies the GMM to MPPI.Source: D Experiment Details · p. 27

D.1 Shared Y-Corridor Setup

• The shared Y-corridor setup encodes pedestrian histories with Trajectron++ and six discrete modes, then uses five DDIM inference steps and an HVP-supervised precision head.The head predicts a block lower-banded factor and a low-rank factor; trajectory-level calibration is fitted on a 10% calibration split and metrics use a disjoint 20% test split.Source: §D.1 · p. 28

D.2 Forecaster Quality on Y-Corridor

• Y-corridor data cover four ORCA crowd densities, with 3.2-second histories and 4.8-second futures evaluated using scene-aggregated metrics.The test protocol samples up to 1,000 pairs per dataset and defines trajectory-level NLL over the full future without timestep-wise mode switching.Source: §D.2 · p. 28
• The forecaster comparison includes a constant-velocity Kalman filter, a sampled MID ensemble, and calibrated SCOPE variants retaining the top three of six modes.Structural ablations vary the precision head from independent variances to a Markov band, with or without a low-rank tail and post-hoc calibration.Source: §D.2 · pp. 28–29

D.3 Inference Time Comparison

• SCOPE uses one diffusion pass regardless of uncertainty resolution; its precision tube head adds 0.60 ms and calibration adds 0.08 ms.The comparison fully batches ensemble diffusion passes on one GPU, and those passes account for over 97% of ensemble prediction time.Source: §D.3 · p. 29, Table 7 · p. 29
• At nine pedestrians and 256 ensemble samples, MID-Ensemble latency reaches approximately 200 ms, delaying the use of new observations in control.When a new forecast is unavailable at a control update, the evaluation uses interpolated predictions from the previous forecast.Source: §D.3 · pp. 29–30
• NLL calibration moves empirical coverage closer to the ideal diagonal than uncalibrated tubes, but the diagnostic does not provide formal coverage guarantees.The calibration optimizes trajectory-level NLL rather than target coverage, with no coverage-specific scaling or conformal correction.Source: §D.3 · p. 30, Figure 7 · p. 30

D.4 Maze2D MPPI Sample-Efficiency

• The 5×5 Maze2D task uses L-shaped obstacles that create local minima for sampling-based controllers whose goal-distance gradient can point into walls.Evaluation uses 100 held-out mazes with 20 start–goal problems each; success requires reaching the goal without collision.Source: §D.4 · pp. 30–31
• The shared diffusion prior is trained on roughly 2.34 million demonstration trajectories, and the evaluation samples one latent style and decodes one path per problem.The protocol sweeps MPPI rollout budgets from 4 to 1,024 and does not select among batches of diffusion paths.Source: §D.4 · pp. 30–32
• Vanilla MPPI uses isotropic Gaussian perturbations, while Diff-MPPI tracks a diffusion-generated global path but retains isotropic sampling.Both diffusion-augmented methods share the same prior; Diff-MPPI uses an arclength-based moving subgoal.Source: §D.4 · pp. 31–32
• SCOPE conditions a Precision Tube Head on the selected path and uses its per-timestep action covariance to concentrate samples in confident directions and widen them where the prior is uncertain.The Maze2D head predicts a banded-plus-low-rank precision matrix, with a trace-variation floor regularizer used to preserve per-timestep tube structure.Source: §D.4 · p. 32

D.4.1 Maze2D Navigation Results

• Diffusion-only planners achieve high goal-reaching rates but retain high collision rates, so goal reaching alone does not establish safe execution in held-out mazes.MPPI improves safety by evaluating candidate rollouts against current maze geometry; short-horizon sampling can nevertheless trap Vanilla and Log-MPPI in concave basins.Source: §D.4.1 · p. 32
• SCOPE achieves higher success, lower collision rate, and shorter paths than Diff-MPPI, which uses the same diffusion prior with fixed covariance.Table 9 reports Maze2D navigation results for MPPI-based planners at 1,024 rollouts per planning step.Source: Table 9 · p. 34, §D.4.1 · p. 34
• The trajectory-dependent tube tightens in constrained regions and broadens where the prior is flexible, combining global diffusion guidance with local MPPI refinement.Across selected challenging cases, SCOPE reaches the goal without collision at all shown rollout budgets, while baselines struggle at some budgets.Source: Figure 8 · p. 33, §D.4.1 · p. 34

D.5 Y-Corridor Closed-Loop Navigation

• In selected Y-corridor examples with 3, 6, 9, and 12 pedestrians, SCOPE produces more localized uncertainty tubes than Direct NLL PH.The closed-loop evaluation uses the Q1 checkpoints with online ORCA pedestrians and reports success, path length, and time to goal across pedestrian counts.Source: §D.5 · pp. 34–35, Figure 9 · p. 35

D.6 Direct NLL Training of the Precision Head

• Direct NLL PH success rates decline more than SCOPE’s as crowd density increases, widening their gap; both methods perform equally in the sparsest setting.Table 10 reports navigation success rates across pedestrian densities.Source: §D.6 · p. 35, Table 10 · p. 36
• Direct NLL PH can produce overly broad tubes that absorb nominal-trajectory errors, raising collision-risk costs and discouraging usable routes, especially in dense crowds.SCOPE instead produces more localized tubes whose uncertainty varies across modes and along trajectories.Source: §D.6 · p. 36

D.7.1 Environment and Hardware

• The real-robot study uses a 7-DOF Franka Panda with a gripper in a shelf workspace where the tomato target and nearby objects create constrained, non-convex geometry.The target must be transferred to the upper shelf.Source: §D.7.1 · p. 37

D.7.2 Task Setup and Evaluation Protocol

• Trials vary nearby box-obstacle positions and compare stationary-obstacle control with a dynamic condition that adds a human hand; each condition has 30 trials.The dynamic condition evaluates whether predicted hand tubes improve avoidance; outcomes include success, collision, and timeout rates, with timeout defined as a collision-free failure to finish within 120 s.Source: §D.7.2 · p. 37

D.7.3 Controller and System Implementation

• For dynamic trials, hand-only tracking feeds future hand trajectories to the diffusion model; SCOPE turns predictions into time-varying collision primitives so MPPI can yield before the hand reaches the robot’s nominal path.The hand is modeled as the dominant moving obstacle for the tabletop interaction considered.Source: §D.7.3 · p. 38
• At 50 Hz, MPPI uses the diffusion trajectory as a soft local reference and shapes its rollout covariance with the precision tube, allowing broader exploration in flexible regions and conservative perturbations in tight ones.The controller can deviate from the reference to reduce total cost or avoid collision.Source: §D.7.3 · p. 38

D.7.4 Real-Robot Case Studies

• Case studies show static Diff-MPPI can time out after a tight approach leaves too little room to recover, while omitting hand prediction can prompt a late evasive motion that collides with the shelf.The latter condition still uses SCOPE as a policy-side prior but not as a human-motion world model.Source: §D.7.4 · p. 39, Figure 12 · p. 39