Accepted at IROS 2026

Closed-loop Diffusion Planning over Multi-Modal Distribution for Robot Follow-Ahead

Qingyuan Jiang, Jun-Jee Chao, Volkan Isler
Robotic Sensor Network Lab, University of Minnesota

Paper Supplementary Minimum Code

The challenge of Robot Follow-Ahead

Imagine a robot carrying important luggages for you: to ensure their safety, you would like to have it maintaining being in your front so they are always in your line of sight. Or, you ask a mobile robot to serve you as a professional photographer, who wants to film you in your front to capture your facial expressions. In both cases, the robot must stay in front of a person while keeping them reliably in view. This problem is usually called Robot Follow-Ahead (RFA) [1, 2, 3].

Robot auto-cinematography application — (a) Robot auto-cinematography

Robot cargo-carrying application — (b) Robot cargo-carrying

Staying in front of someone means the robot has to move before they do. So, because of the robot’s own motion limits, it can’t just react to where a person is — it has to predict where they’re about to go.

That’s easy in a straight corridor: wherever the person is headed, so is the robot. At a T-junction, though, the robot hits the decision point first and has to commit before the person has fully committed themselves. If it waits, it falls behind. If it guesses wrong, it can lose visual contact entirely.

Motivation scene for CLDP at a junction At the junction, multiple futures are valid. Planning must preserve this multi-modality instead of collapsing it.

So the whole problem comes down to this: how do you predict where someone is about to go, when their next move is genuinely ambiguous?

Why a single guess isn’t enough

Existing work tries to model this uncertainty by generating a single trajectory. The underlying assumption is that the future motion distribution can be represented as a high-dimensional Gaussian, and these methods report the mean of the Gaussian as the prediction.

In the junction scenario, though, the underlying distribution is multi-modal, and representing it as a Gaussian leads to predictions which may not even be feasible: a mean sitting between the two modes, aimed at neither. The average of two good futures is not a good future.

Gaussian mean collapsing two modes into a prediction between them Averaging a bimodal distribution over left/right turns produces a mean trajectory that heads into the wall between them.

Representing the distribution, not just its mean

If a single number can’t capture “50% left, 50% right,” the fix is to stop asking for a single number. We use a diffusion model to sample directly from the conditional distribution of future human motion, conditioned on where the person has been and the local occupancy map.

Sample enough times and the samples themselves reconstruct the shape of the distribution: if the person really is a coin flip between the two corridors, and we sample 10 futures in total, we don’t get one hedge in the middle — we get 5 samples heading left and 5 heading right. Nothing is collapsed or averaged away — the ambiguity is represented directly, as a population of plausible paths.

Remain-centered strategy: weights shifting from 50/50 to 20/80 as the human commits to a turn — The robot starts with a 50/50 split between the left and right corridor. As the person takes a few real steps, the weights shift, say to 20/80, and the robot leans into the corridor that's actually being taken — more on how that update works below.

Open-loop planning: the right idea, on the wrong clock

Now the robot has a set of candidate futures instead of one guess. Plan a path that minimizes expected cost over all of them — penalizing lost visibility and collisions — and dynamic programming will find the best single action sequence. This is where most systems stop, and it works: it no longer walks into a wall.

But it has its own blind spot. If the samples really do split 50/50, the optimal open-loop action is to hedge — stay centered between the two corridors — and it keeps doing that until the next, expensive diffusion pass gives it new samples to plan against. Diffusion denoising isn’t cheap, so that next pass is relatively slow. Meanwhile the person has already started turning. An open-loop planner has no way to notice until its next slow cycle catches up, and by then it may have spent several timesteps hedging or, worse, committed to the majority-vote direction even after the person went the other way.

Closing the loop

Ideally, the robot should notice a change in direction immediately, at the very next timestep, rather than waiting on the next slow diffusion cycle to catch up. So CLDP closes the loop with posterior observations: as the person moves, we update the importance weight of every sample based on how well it agrees with what’s actually happening.

Formally, once we have a posterior observation, planning means choosing the action sequence that minimizes expected cost under the posterior motion distribution instead of the prior one. Using Bayes’ rule and importance sampling, that expectation reduces to a weighted mean over the sample costs, where each sample’s weight $\lambda_i$ is just a function of how far its predicted trajectory has drifted from the person’s observed positions:

\lambda_i \;=\; \exp\!\left(-\sum_{t=t_0}^{t_k} \frac{1}{2\sigma_t^2} \left\| \tau_t - \hat{\tau}^i_t \right\|^2\right)

Normalize the weights across all samples with a softmax, and the plan is just the cost-minimizing action sequence $\mathcal{U}$ under this reweighted population:

\mathcal{U}^{*} \;=\; \arg\min_{\mathcal{U}} \sum_{i} \lambda_i \, \mathcal{J}\!\left(\hat{\tau}^i,\, \mathcal{X}_{t_0:}\right)

Re-solving the plan is then just re-weighting — no new diffusion pass required.

Algorithmically, the loop looks like this: sample a batch of future trajectories once, then, while waiting for the next batch, repeatedly observe the person’s real position, update every sample’s importance weight, and re-plan against the new weights. That cheap update loops until the next full diffusion pass completes and the batch resets.

Two clocks, one loop: a slow one that periodically re-imagines the space of futures from scratch, and a fast one that continuously re-weights and re-plans against whatever futures it currently has in hand.

For the full formulation — the robot model, cost function, and how both the open-loop and closed-loop optimizations are solved — see the paper; the supplementary includes the algorithm pseudocode.

Does it actually work?

Revisit the junction from the opening example. Initially, more of the sampled futures point toward one corridor than the other, so an open-loop planner commits to that majority vote and starts heading there. CLDP starts from the same samples but keeps watching the real motion: as soon as the observed position disagrees with the majority-favored samples, their importance weights collapse, the minority samples take over, and the plan snaps toward the corridor the person is actually taking.

OLDP commits to the majority-vote direction even after the person actually moves the other way — Open-loop (OLDP): locks onto the direction most samples favored, and stays committed even once the person's real motion shows otherwise.

CLDP reweights its samples and corrects toward the direction the person is actually taking — Closed-loop (CLDP, ours): the posterior update lets the robot pivot to the correct corridor instead of committing to the wrong one.

The benefit isn’t a better initial prediction — it’s the ability to recover from a bad one before it costs the robot visual contact. For the full quantitative comparison across datasets (GTA-IM, HPS, and Junc) and additional ablations, see the paper.

What this buys us

Multi-modality that survives: the robot doesn’t collapse a left-or-right decision into an unsafe straight line into the wall.
Instant reactions: the fast Bayesian update makes the robot feel responsive to a person, even though the heavy lifting (the diffusion model) runs on its own slower schedule underneath.
Recovery from a bad start: if the initial guess leans the wrong way, CLDP doesn’t get stuck there. As soon as the person’s motion disagrees with the current bet, the weights shift and the robot re-commits to the right path.

Limitations

CLDP still rests on a couple of assumptions worth being upfront about.

First, it treats the human’s motion as independent of the robot’s — the samples are drawn as if the robot weren’t there. In practice, a person may well adjust their path because a robot is following in front of them, which turns this into a genuine two-body interaction problem. Modeling that properly would likely mean a game-theoretic formulation, optimizing against the worst case rather than assuming the human’s motion is given.

Second, closing the loop only helps if the right future was in the sample set to begin with. When the diffusion model fails to produce any sample close to what the person actually does, no amount of reweighting can rescue the plan — there’s nothing correct to up-weight. This is a shared failure mode across prediction-based planners, not unique to CLDP, and in those cases falling back to purely reactive behavior may be the safer bet. It also means the diversity of the training motion data matters at least as much as its volume.

For the full discussion, see the supplementary material.