Stochastic MPC: MPPI and Friends

Control by sampling — a thousand imagined futures, scored and reweighted every frame. MPPI is the particle filter's twin, and this chapter proves it term by term.

The problem with a plan

A path is a claim about a world that no longer exists. It was computed from a map, and the map was recorded before the chair moved, before somebody left a box in the corridor, before the person walking toward Rusty decided which side to pass on.

A tracking controller cannot fix this, because it is not being asked to. Give it a path and it will minimize the distance to that path, which is precisely the wrong objective when the path goes through a chair. Re-running the global planner instead is honest but expensive: at twenty hertz, with a planner that takes a hundred milliseconds and returns a structurally different path each time, Rusty would spend his life twitching between homotopy classes.

The receding-horizon answer is narrower and cheaper. Do not re-plan the route. Re-solve, every cycle, a short optimal control problem — two seconds, not two minutes — that already knows about the chair, and execute one step of its answer.

Watch a few cycles before reading on. Three things are worth naming now; each one becomes a theorem later in the chapter.

Nothing here is optimized in the usual sense. There is no gradient, no line search, no convexity. Two hundred perturbed plans are simulated forward, each is assigned a number, and the next plan is a weighted average. That is the entire algorithm, and it is the reason the obstacle cost is allowed to be a hard if statement over an occupancy grid.

The purple plan is not one of the orange rollouts. It is their weighted mean, and it is visibly smoother than any of them — the signature of an expectation rather than a selection. When the temperature is pushed toward zero this stops being true, and the plan snaps onto whichever single rollout got lucky. Watch the effective sample size collapse toward 1 when you do it.

The storm re-centres itself. Each cycle inherits the previous plan, shifted forward one step. That warm start is why two hundred samples suffice where a cold start would need thousands, and it is the exact analogue of the particle filter's prediction step.

Building intuition: search velocities, not paths

Before the path-integral machinery, the classical baseline — and it is a good one, still shipping in production stacks thirty years later. Fox, Burgard and Thrun's Dynamic Window Approach starts from a physical observation: a differential-drive robot cannot execute an arbitrary curve. Over one short control period it executes an arc, and which arcs are available is decided by its acceleration limits. So do not search over paths. Search over the velocities reachable in the next period.

That search space is two-dimensional and tiny, which is why DWA ran on 1997 hardware at speeds up to 95 cm/s. Its structure is worth internalizing because MPPI keeps one half of it and replaces the other:

• Keep: commands are the decision variable, and the reachable set is a box set by acceleration limits.
• Replace: each candidate is scored along one constant-curvature arc, evaluated at its endpoint. A hypothesis class of single arcs cannot express "slow, drift left, then turn hard right" — not badly, but at all.

The velocity plane on the left is where both controllers actually live, and putting them in the same axes is the fastest way to see what changed. DWA lays a grid over the reachable box and asks one question per cell: if I hold this command, where do I end up and what do I hit? MPPI scatters two hundred samples over the same plane, but each dot is the head of a whole sequence, and its weight depends on what the other twenty-four steps of that sequence cost.

Two honest observations from the bench, both of which the numbers later in this chapter pin down.

First, DWA threads the chairs down the centreline with five centimetres to spare, and no setting of its clearance weight changes that. The reason is worth the paragraph: Fox et al.'s clearance term is $\mathrm{dist}(v,\omega)$, the distance travelled before the arc touches something. A gap you fit through at all returns the same value as a gap with a metre to spare, so the term is saturated and its weight is inert. Swap in the modern repair — score the minimum margin anywhere along the arc, which the widget lets you toggle — and the weight bites, but what it buys is paralysis: above $w_c \approx 0.1$ Rusty stops 0.7 m short of the chair and never moves again. There is no setting in between, because the maneuver that keeps margin and makes progress is an S-curve, and an S-curve is not one arc.

Second, in the counter pocket both controllers stall. A local controller is local. Getting out of that pocket is Chapter 20's job, and no temperature setting substitutes for it.

The mathematics

Setting

Receding-horizon control is the following loop, and its most consequential line is the last one.

Line 5 is the trick. The controller solves a problem it does not intend to finish, which buys two things: the optimization only has to be locally right, and every cycle gets a fresh measurement. Feedback enters not through a gain matrix but through re-solving.

DWA, formally

The dynamic window is the intersection of three sets. The actuator envelope $V_s = [v_{\min}, v_{\max}] \times [-\omega_{\max}, \omega_{\max}]$; the reachable set

around the current command; and the admissible set $V_a$, the commands from which the robot can still stop before whatever its arc runs into.

$\sigma$ is normalization across the window: each term is scaled to $[0,1]$ before the weighted sum, so that metres and radians cannot decide the outcome by their units. (Fox et al. call the three weights $\alpha, \beta, \gamma$; this chapter needs those letters shortly for the temperature family, so they are $w_h, w_c, w_v$ here.) Everything else is a design choice, and every one of those choices is a place where a real deployment spends a week.

The optimal distribution

Now the modern half. Fix a base distribution $p(V)$ over control sequences — think of it as a prior over what the robot might do — and define the free energy of the control system,

This object is the bridge between "minimize a cost" and "sample from a distribution", and the bridge is a single inequality.

The twin theorem: MPPI is importance sampling

We now have a target, $q^\star$, and no way to sample from it — the familiar predicament of Chapter 8, one dimension per horizon step deeper. The familiar remedy applies.

Set the two algorithms side by side and there is nothing left to argue about.

The correspondence is not an analogy, and the widget is not a metaphor: both panels run library code that the rest of this book already uses. The left one calls lowVarianceResample from lib/filters/pf.ts; the right one calls the same Mppi class that drives the corridor at the top of this chapter, restricted to one dimension.

One asymmetry is real and worth stating. The particle filter's weights are handed to it — the likelihood is physics, and there is no knob. MPPI's weights come from a cost you invented, so $\lambda$ is a free parameter: it says how much you believe your own objective. That freedom is the subject of the next section.

Temperature, and the two limits

Here is that trade-off measured, on the corridor run of w23.1 (seed 23, $K = 200$, $H = 25$, $\Delta t = 0.1$ s, $\alpha = 1$). "Jitter" is the mean $|\Delta u|$ between consecutive executed commands — the number that shows up on screen as twitching.

Read the extremes. At $\lambda = 0.5$ the controller is fast to react and horrible to ride, and its clearance suffers because a single lucky rollout is allowed to steer. At $\lambda = 120$ it is serene, twice as slow, and cuts its margin in half — the weights are nearly uniform, so the barrier cost barely moves the plan. The useful band is the one where the ESS sits somewhere around a tenth of $K$: enough samples voting to average out the noise, few enough that the cheap ones dominate.

Constraints, and how sampling gets away with them

Input limits. Do not reject samples and do not solve a QP. Push the clamp into the dynamics: $x_{k+1} = f(x_k, g(v_k))$ with $g$ the saturation. The sampler stays a plain Gaussian, and the non-smooth $g$ costs nothing because the update law never differentiates $f$. Clamp before integrating, so that the trajectory you score is one the actuators would actually produce.

Obstacles. Two options, and the difference is measured in ESS. An indicator cost $c_{obs} = M \cdot \mathbb{1}[\text{in collision}]$ is legal — no gradient is required — but in a tight corridor it makes most rollouts equally infinite, the weights collapse onto whichever sample squeaked through, and the ESS falls to 1. The smooth barrier over the signed distance field of Chapter 19,

grades the rollouts instead of censoring them, which keeps the importance weights informative. This chapter's lab uses the barrier plus a flat 900 for actual collision: graded where grading helps, brutal where it must be.

Chance constraints. Inflating the footprint by the belief covariance, as in the callout above, is the cheap version. The honest version optimizes over belief trajectories and is a research field.

Implementation in Rust

The design is two traits and one struct. Making Dynamics and CostFn traits rather than closures is what leaves the socket open for the gradient methods at the end of this chapter: iLQR needs the same simulator, plus derivatives.

use nalgebra::SVector;

/// A simulator. Note what is *not* required: no Jacobians, no differentiability,
/// no continuity. This trait is the entire interface MPPI needs from a model.
pub trait Dynamics<const NX: usize, const NU: usize>: Sync {
    fn step(&self, x: &SVector<f64, NX>, u: &SVector<f64, NU>, dt: f64) -> SVector<f64, NX>;

    /// Input constraints, pushed into the dynamics (Williams et al. §III-D3).
    /// Clamp before integrating: a plan scored on commands the motors would
    /// refuse is a plan scored on a lie.
    fn clamp(&self, u: SVector<f64, NU>) -> SVector<f64, NU>;
}

/// Rusty: unicycle kinematics, the Chapter 9 velocity model with the noise off.
pub struct DiffDrive {
    pub v_lim: (f64, f64),
    pub w_max: f64,
    pub a_max: f64,
}

impl Dynamics<3, 2> for DiffDrive {
    fn step(&self, x: &SVector<f64, 3>, u: &SVector<f64, 2>, dt: f64) -> SVector<f64, 3> {
        let (v, w) = (u[0], u[1]);
        let th = x[2];
        // The exact arc solution (Thrun et al., eq. 5.9). As ω → 0 the radius
        // blows up while the arc flattens; rather than trust that cancellation
        // numerically we switch to the straight-line limit, exactly as the
        // TypeScript port in `lib/sim/world.ts` does.
        if w.abs() < 1e-9 {
            SVector::<f64, 3>::new(x[0] + v * th.cos() * dt, x[1] + v * th.sin() * dt, th)
        } else {
            let r = v / w;
            let nt = th + w * dt;
            SVector::<f64, 3>::new(
                x[0] - r * th.sin() + r * nt.sin(),
                x[1] + r * th.cos() - r * nt.cos(),
                wrap_pi(nt),
            )
        }
    }

    fn clamp(&self, u: SVector<f64, 2>) -> SVector<f64, 2> {
        SVector::<f64, 2>::new(
            u[0].clamp(self.v_lim.0, self.v_lim.1),
            u[1].clamp(-self.w_max, self.w_max),
        )
    }
}

pub trait CostFn<const NX: usize, const NU: usize>: Sync {
    fn stage(&self, x: &SVector<f64, NX>, u: &SVector<f64, NU>, k: usize) -> f64;
    fn terminal(&self, x: &SVector<f64, NX>) -> f64;
}

/// Track Chapter 20's path, keep clear of Chapter 19's distance field.
pub struct TrackAndClear<'a> {
    pub path: &'a [SVector<f64, 2>],
    pub esdf: &'a ch19_maps::Esdf2,
    pub obstacles: &'a [Circle],
    pub robot_radius: f64,
    pub w_track: f64,
    pub w_progress: f64,
    pub w_obs: f64,
    pub sigma_o: f64,
    pub collision: f64,
}

impl CostFn<3, 2> for TrackAndClear<'_> {
    fn stage(&self, x: &SVector<f64, 3>, u: &SVector<f64, 2>, _k: usize) -> f64 {
        let (cross, s) = self.project(x[0], x[1]);
        let clear = self.clearance(x[0], x[1]);
        let obs = if clear <= 0.0 {
            // A discontinuity. Legal here, fatal for a gradient method.
            self.collision
        } else {
            self.w_obs * (-clear / self.sigma_o).exp()
        };
        self.w_track * cross * cross + obs - self.w_progress * s + 0.4 * u[1] * u[1]
    }

    fn terminal(&self, x: &SVector<f64, 3>) -> f64 {
        let (_, s) = self.project(x[0], x[1]);
        8.0 * (self.path_length() - s)
    }
}

The controller itself. The only subtle line is where the randomness happens: perturbations are drawn serially from a seeded generator and only then handed to rayon, because a parallel iterator that draws its own noise would make the run non-reproducible — and every simulation in this book is reproducible on purpose.

use nalgebra::{SMatrix, SVector};
use rand::{rngs::SmallRng, SeedableRng};
use rand_distr::{Distribution, Normal};

pub struct Mppi<D, C, const NX: usize, const NU: usize, const H: usize>
where
    D: Dynamics<NX, NU>,
    C: CostFn<NX, NU>,
{
    pub lambda: f64,
    pub sigma_u: SMatrix<f64, NU, NU>,
    pub k_samples: usize,
    /// γ = λ(1 − α). Zero means α = 1: the base distribution is the current
    /// plan, so the KL term charges deviation from it rather than effort.
    pub gamma: f64,
    nominal: [SVector<f64, NU>; H],
    dynamics: D,
    cost: C,
    rng: SmallRng,
}

pub struct MppiDiag<const NX: usize> {
    pub weights: Vec<f64>,
    pub ess: f64,
    pub s_min: f64,
    /// Every rollout, for the widget's storm. Native builds drop this.
    pub rollouts: Vec<Vec<SVector<f64, NX>>>,
}

impl<D, C, const NX: usize, const NU: usize, const H: usize> Mppi<D, C, NX, NU, H>
where
    D: Dynamics<NX, NU>,
    C: CostFn<NX, NU>,
{
    /// One control cycle. Returns the command to execute and the diagnostics
    /// the widget renders; the improved plan is left in `self.nominal`.
    pub fn plan(&mut self, x0: &SVector<f64, NX>, dt: f64) -> (SVector<f64, NU>, MppiDiag<NX>) {
        let sigma_inv = self.sigma_u.try_inverse().expect("Σ_u must be positive definite");
        let normals: Vec<Normal<f64>> = (0..NU)
            .map(|i| Normal::new(0.0, self.sigma_u[(i, i)].sqrt()).unwrap())
            .collect();

        // Draw every perturbation up front, from the seeded generator. This is
        // the price of determinism under rayon, and it is worth paying: the
        // same seed must produce the same storm on 1 core and on 32.
        let (k_samples, rng) = (self.k_samples, &mut self.rng);
        let eps: Vec<[SVector<f64, NU>; H]> = (0..k_samples)
            .map(|_| {
                std::array::from_fn(|_| SVector::<f64, NU>::from_fn(|i, _| normals[i].sample(rng)))
            })
            .collect();

        // Embarrassingly parallel: K independent simulations of H steps. On
        // wasm32 there are no threads, so the same closure runs serially — and
        // at K = 200, H = 25 that is 5 000 rollout steps a cycle, which this
        // book's TypeScript port measures at about 1.6 ms on a laptop core.
        #[cfg(not(target_arch = "wasm32"))]
        let costs: Vec<f64> = { use rayon::prelude::*; eps.par_iter().map(|e| self.rollout(x0, e, &sigma_inv, dt)).collect() };
        #[cfg(target_arch = "wasm32")]
        let costs: Vec<f64> = eps.iter().map(|e| self.rollout(x0, e, &sigma_inv, dt)).collect();

        let (weights, ess, s_min) = information_theoretic_weights(&costs, self.lambda);

        // u_k ← u_k + Σ_i w_i ε_k^(i): the self-normalized IS estimate.
        for k in 0..H {
            let mut delta = SVector::<f64, NU>::zeros();
            for (i, e) in eps.iter().enumerate() {
                delta += weights[i] * e[k];
            }
            self.nominal[k] = self.dynamics.clamp(self.nominal[k] + delta);
        }
        savitzky_golay_inplace(&mut self.nominal); // §III-D4: kill the Monte-Carlo chatter

        let u0 = self.nominal[0];
        (u0, MppiDiag { weights, ess, s_min, rollouts: vec![] })
    }

    /// S̃ = S(V) + γ Σ_k u_kᵀ Σ_u⁻¹ ε_k — state cost plus the cross term.
    fn rollout(
        &self,
        x0: &SVector<f64, NX>,
        eps: &[SVector<f64, NU>; H],
        sigma_inv: &SMatrix<f64, NU, NU>,
        dt: f64,
    ) -> f64 {
        let mut x = *x0;
        let mut s = 0.0;
        for k in 0..H {
            let u = self.dynamics.clamp(self.nominal[k] + eps[k]);
            x = self.dynamics.step(&x, &u, dt);
            s += self.cost.stage(&x, &u, k);
            s += self.gamma * self.nominal[k].dot(&(sigma_inv * eps[k]));
        }
        s + self.cost.terminal(&x)
    }

    /// The receding step: drop the executed command, slide, repeat the tail.
    /// The twin of the particle filter's prediction — the same belief, moved
    /// one step into the future and re-centred there.
    pub fn shift(&mut self) {
        for k in 0..H - 1 {
            self.nominal[k] = self.nominal[k + 1];
        }
    }
}

/// `ComputeWeights` — Williams et al., Algorithm 2.
pub fn information_theoretic_weights(costs: &[f64], lambda: f64) -> (Vec<f64>, f64, f64) {
    let rho = costs.iter().cloned().fold(f64::INFINITY, f64::min);
    let mut w: Vec<f64> = costs.iter().map(|s| (-(s - rho) / lambda).exp()).collect();
    let eta: f64 = w.iter().sum();
    w.iter_mut().for_each(|x| *x /= eta);
    let ess = 1.0 / w.iter().map(|x| x * x).sum::<f64>();
    (w, ess, rho)
}

A worked example you can check by hand

Take the smallest MPPI that is still MPPI: horizon $H = 1$, $K = 3$ rollouts, temperature $\lambda = 2$, perturbation $\sigma_v = 0.4$ m/s, and a nominal command $\hat u = 0.5$ m/s. Suppose the three perturbations come out $\epsilon = (+0.4,\, 0,\, -0.4)$ and the simulator returns state costs

With $\alpha = 1$ (the base is the current plan, $\gamma = 0$, no cross term) the shift is $\rho = 2$, so the unnormalized weights are $e^{-1},\, e^{0},\, e^{-2} = 0.36788,\, 1,\, 0.13534$, summing to $\eta = 1.50321$. Normalizing:

Two of three rollouts are effectively voting. The update is the weighted mean of the perturbations,

Now switch to $\alpha = 0$, the uncontrolled base, so $\gamma = \lambda = 2$ and $\Sigma_u^{-1} = 1/0.16 = 6.25$. Each rollout picks up $\gamma\, \hat u\, \Sigma_u^{-1} \epsilon = 2 (0.5)(6.25)\epsilon = 6.25\,\epsilon$:

The same three rollouts, the same state costs, and the update changes sign. Judged on state cost alone, going faster looked good; charged for the effort of going faster against a base distribution that would sit still, it no longer does. That is what the cross term is for, and why dropping it silently changes the problem you are solving.

#[test]
fn worked_example_ch23_three_rollouts() {
    // α = 1: no cross term. Weights are e^{-1}, e^{0}, e^{-2}, normalized.
    let (w, ess, rho) = information_theoretic_weights(&[4.0, 2.0, 6.0], 2.0);
    assert_relative_eq!(rho, 2.0, epsilon = 1e-12);
    assert_relative_eq!(w[0], 0.2447284, epsilon = 1e-6);
    assert_relative_eq!(w[1], 0.6652406, epsilon = 1e-6);
    assert_relative_eq!(w[2], 0.0900310, epsilon = 1e-6);
    assert_relative_eq!(ess, 1.958699, epsilon = 1e-5);

    let eps = [0.4, 0.0, -0.4];
    let du: f64 = w.iter().zip(eps).map(|(wi, e)| wi * e).sum();
    assert_relative_eq!(du, 0.0618787, epsilon = 1e-6);

    // α = 0: γ = λ = 2, Σ_u⁻¹ = 1/0.4² = 6.25, nominal u = 0.5.
    let tilted: Vec<f64> = [4.0, 2.0, 6.0]
        .iter()
        .zip(eps)
        .map(|(s, e)| s + 2.0 * 0.5 * 6.25 * e)
        .collect();
    assert_relative_eq!(tilted[0], 6.5, epsilon = 1e-12);

    let (w2, _, _) = information_theoretic_weights(&tilted, 2.0);
    let du2: f64 = w2.iter().zip(eps).map(|(wi, e)| wi * e).sum();
    assert_relative_eq!(du2, -0.0930347, epsilon = 1e-6);
    assert!(du2 < 0.0, "the control cost must reverse the update's sign");
}

/// Self-normalized weights cannot see a constant shift of the cost — which is
/// exactly why subtracting ρ is safe.
#[test]
fn shift_invariance() {
    let (a, _, _) = information_theoretic_weights(&[3.0, 7.0, 11.0, 2.0], 1.5);
    let (b, _, _) = information_theoretic_weights(&[1003.0, 1007.0, 1011.0, 1002.0], 1.5);
    for (x, y) in a.iter().zip(b) {
        assert_relative_eq!(*x, y, epsilon = 1e-12);
    }
}

The TypeScript port runs the same assertions in lib/control/__checks_ch23__.ts, and the widgets on this page call the same functions the tests do. If the prose and the code ever disagree, the tests settle it.

Putting it together

Here is the corridor run of w23.1, executed headless over twelve seeds: Rusty tracks a path planned before two chairs moved, with a third obstacle being pushed along the corridor while he drives. $K = 200$, $H = 25$ (2.5 s), $\Delta t = 0.1$ s, $\lambda = 8$, $\Sigma_u = \mathrm{diag}(0.22^2,\, 0.55^2)$, $\alpha = 1$.

Both controllers get there, and reading this table as "MPPI wins" would be the wrong lesson. DWA is smoother and marginally faster, because it always picks a single arc and the arc it picks is usually "straight ahead, fast". What it will not do is deviate laterally to buy margin — and, as the bench shows, cannot be made to by tuning. Its clearance term saturates the moment an arc is collision-free, and replacing it with a true minimum margin turns the controller timid rather than graceful: measured on this run, $w_c \le 0.08$ still shaves the chairs at 0.050 m, and $w_c \ge 0.12$ freezes 0.7 m short. MPPI's exponential barrier grades the last few centimetres continuously and its hypothesis is a whole 2.5-second maneuver, so it swings out and back and keeps four times the margin at the same speed.

The differences that are structural are these. MPPI evaluates a whole 2.5-second maneuver as one hypothesis, so it can commit to a plan whose first half looks worse. And MPPI does not care what the cost is made of — the barrier, the flat 900 for collision, and a raw occupancy lookup are all the same to it. Neither property is available to a controller that scores one arc by its endpoint.

When gradients beat samples

That last row is the one people forget. iLQR returns $u_k = \bar u_k + K_k (x_k - \bar x_k)$ — a time-varying feedback law, valid between control cycles. MPPI returns a single open-loop command and relies on re-solving fast enough that open loop never has time to matter. At 20 Hz on a diff-drive that is a fine bet; on a machine whose joints need commands hundreds of times faster it is not, which is why sampling controllers at those rates are wrapped in a fast ancillary feedback law that tracks the sampled plan between updates.

What MPPI is not

The counter pocket in w23.2 is in this chapter because it is the failure most likely to bite a reader who has just watched the storm and concluded that sampling solves planning. Rusty sits behind the kitchen counter; the goal is two metres north, through it. Escaping costs 1.9 m of travel in the wrong direction before a single metre is repaid.

MPPI does not escape it. Not at $H = 10$, not at $H = 60$: the perturbations are white noise around the current plan, so the probability that some rollout traces a coherent four-metre detour is negligible, and every rollout that starts the detour looks worse than standing still. DWA does not escape it either, and neither of them is broken. A local controller optimizes; it does not search. Give the same MPPI a reference path around the counter and it follows it out in 13.6 seconds. That is the layered architecture the whole of Part VI has been building toward, and the reason Chapter 20 exists.

The small end of a real continuum

It would be easy to read this chapter as a toy. It is not. The algorithm above is what Williams et al. ran on a fifth-scale rally car sliding around a dirt track: about 1 200 rollouts of 2.5 seconds at 40 Hz, roughly 4.8 million queries to the full nonlinear vehicle dynamics every second, on a GPU, over more than 100 km of autonomous driving. The same update law, with a stack of about a dozen pluggable cost critics, ships as a stock controller in ROS 2's Nav2 for exactly the kind of differential-drive robot Rusty is. The reader's rayon loop is the small end of that continuum, not a different thing.

And there is a thread left deliberately loose. Every cost in this chapter has been about where the robot should be. Nothing stops a term in $c(x_k, u_k)$ from being about what the robot would learn — the expected entropy reduction of the map, say. Put that term in the cost and the same sampler that dodges chairs starts choosing where to look. Chapter 24 makes goals out of information.

Exercises

1. Foundation exerciseFDifficulty 2 of 3Derive the cross term, then delete it

   Starting from $w(V) \propto e^{-S(V)/\lambda}\, p(V) / q(V \mid \hat U, \Sigma_u)$ with $p = q(\cdot \mid \alpha\hat U, \Sigma_u)$, carry out the two-quadratic expansion in Step 3 of the twin derivation and confirm that the sample-dependent part of the log-ratio is exactly $-\sum_k (1-\alpha)\hat u_k\T \Sigma_u^{-1} \epsilon_k$. Then explain, using the free-energy bound rather than intuition, what objective you are optimizing when you set $\gamma = 0$: what replaces the effort penalty, and why that makes the resulting motion smoother rather than faster.

2. Foundation exerciseFDifficulty 2 of 3Both limits, and the gauge between them

   Prove the two limits of the previous derivation carefully: that $\lambda \to 0^+$ gives best-of-$K$ (state the tie-breaking assumption you need) and that $\lambda \to \infty$ gives an update of size $O(\sigma/\sqrt{K})$. Then show that $\mathrm{ESS} = 1$ and $\mathrm{ESS} = K$ are attained exactly in those limits, and use that to explain why the measured table above has its shortest time-to-goal in the middle rather than at either end.

3. Foundation exerciseFDifficulty 3 of 3Why the mean, not the mode

   Step 1 of the twin derivation projects $q^\star$ onto a Gaussian by minimizing $\KL(q^\star \| q(\cdot|U,\Sigma_u))$ and lands on moment matching. Do the algebra. Then argue what would change if you minimized the reverse KL instead, $\KL(q(\cdot|U,\Sigma_u) \| q^\star)$, and relate your answer to what happens in w23.1 as $\lambda \to 0$. Which of the two divergences describes a controller that commits to one option in a bimodal cost landscape, and which describes one that steers between them into the obstacle?

4. Conceptual exerciseCDifficulty 1 of 3Predict, then check: starve the proposal

   In w23.1, set the exploration $\sigma_v$ to roughly a quarter of its default (0.06 m/s) and leave everything else alone. Before you press play, predict: does Rusty still find the gap beside the first chair, and does the ESS go up or down? Run it. Explain what you see in the proposal-coverage language of Chapter 8 — and note which of the two possible ESS answers is the bad one here, which is the opposite of the filtering case.

5. Conceptual exerciseCDifficulty 2 of 3Try to make DWA safe

   In w23.2, run the corridor with DWA and note the minimum clearance. Predict what raising the clearance weight $w_c$ from 0.02 to 0.5 will do, then do it — and explain the result from the definition of $\mathrm{dist}(v,\omega)$ rather than from the plot. Now switch on minimum-margin clearance and find the largest $w_c$ that still reaches the goal, and the smallest that freezes the robot. Report both, and say why there is no useful value in between: what shape of trajectory would be needed, and why is it not in DWA's hypothesis class? Finally, name the property of $\exp(-d/\sigma_o)$ that a normalized linear term lacks.

6. Practical exercisePDifficulty 2 of 3Colored noise

   White perturbations spend most of their probability mass on plans that reverse direction every step — plans no robot would consider. Implement temporally correlated sampling in mppi.rs: draw $\epsilon_k = \beta \epsilon_{k-1} + \sqrt{1-\beta^2}\, \xi_k$ with $\xi_k \sim \Normal(0, \Sigma_u)$, so the marginal variance is unchanged. Measure, on the corridor run over twelve seeds, the mean $|\Delta u|$, the minimum clearance, and the time to goal as a function of $\beta \in \{0, 0.3, 0.6, 0.9\}$. At which $\beta$ does the controller stop being able to react to the moving obstacle, and why?

7. Practical exercisePDifficulty 3 of 3A gradient rival on the same traits

   Implement one iLQR iteration against the same Dynamics and CostFn traits, adding only the two Jacobian methods it needs. Race it against MPPI on (a) the smooth-ESDF corridor and (b) a version whose obstacle cost is a raw occupancy-grid lookup with no smoothing. Report time to goal, minimum clearance, and iterations to convergence for both, and reproduce the top two rows of this chapter's scorecard with your own numbers. If iLQR fails on (b), say precisely at which line of your implementation it fails.

References

1. Fox, D., Burgard, W., and Thrun, S. (1997) The Dynamic Window Approach to Collision Avoidance. IEEE Robotics & Automation Magazine 4(1), 23–33.doi:10.1109/100.580977 (opens in a new tab)

   The classical baseline of this chapter, from the same authors as the book's spine. Section III is the source of the admissibility inequality and the three-term objective; the RHINO experiments ran at up to 95 cm/s in populated corridors.

2. Theodorou, E., Buchli, J., and Schaal, S. (2010) A Generalized Path Integral Control Approach to Reinforcement Learning. Journal of Machine Learning Research 11, 3137–3181.link to A Generalized Path Integral Control Approach to Reinforcement Learning (opens in a new tab)

   Where the exponentiated-cost weighting entered robotics, as PI². Read it for the continuous-time path-integral derivation this chapter deliberately replaces with the discrete-time information-theoretic one.

3. Williams, G., Aldrich, A., and Theodorou, E. A. (2017) Model Predictive Path Integral Control: From Theory to Parallel Computation. Journal of Guidance, Control, and Dynamics 40(2), 344–357.doi:10.2514/1.G001921 (opens in a new tab)

   The paper that named MPPI and made the case for GPU rollouts. Its parallel-computation analysis is the reason the Rust here draws noise serially and parallelizes only the simulations.

4. Williams, G., Drews, P., Goldfain, B., Rehg, J. M., and Theodorou, E. A. (2018) Information-Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving. IEEE Transactions on Robotics 34(6), 1603–1622.doi:10.1109/TRO.2018.2865891 (opens in a new tab)

   This chapter's Foundation section follows its §III exactly: the free-energy bound, the optimal distribution, the importance-sampling weight with its cross term, and Algorithms 1–2. The preprint is arXiv:1707.02342, and the epigraph is from its §III-A.

5. Macenski, S., Moore, T., Lu, D. V., Merzlyakov, A., and Ferguson, M. (2023) From the Desks of ROS Maintainers: A Survey of Modern & Capable Mobile Robotics Algorithms in the Robot Operating System 2. Robotics and Autonomous Systems 168, 104493.doi:10.1016/j.robot.2023.104493 (opens in a new tab)

   Deployment evidence for the claim that MPPI is now a default rather than a curiosity: written by the Nav2 maintainers, it surveys the controller stack that ships MPPI alongside DWB, the direct descendant of the 1997 paper above.

6. Kazim, M., Hong, J., Kim, M.-G., and Kim, K.-K. K. (2024) Recent Advances in Path Integral Control for Trajectory Optimization: An Overview in Theoretical and Algorithmic Perspectives. Annual Reviews in Control 57, 100931.doi:10.1016/j.arcontrol.2023.100931 (opens in a new tab)

   The map of the field since 2018 — cross-entropy variants, covariance adaptation, smoothing schemes — and the best single source for what to read next after this chapter.

7. Trevisan, E. and Alonso-Mora, J. (2024) Biased-MPPI: Informing Sampling-Based Model Predictive Control by Fusing Ancillary Controllers. IEEE Robotics and Automation Letters 9(6), 5871–5878.doi:10.1109/LRA.2024.3397083 (opens in a new tab)

   The modern answer to this chapter's counter-pocket failure: keep the proposal, but seed it with samples from controllers that already know the way out. The importance weights stay valid, which is exactly the point of deriving them properly.

8. Homburger, H., Messerer, F., Diehl, M., and Reuter, J. (2025) Optimality and Suboptimality of MPPI Control in Stochastic and Deterministic Settings. IEEE Control Systems Letters.doi:10.1109/LCSYS.2025.3574151 (opens in a new tab)

   An honest accounting of what MPPI actually returns: the suboptimality of the sampled solution grows second-order in the noise scaling for smooth unconstrained problems. Read it before promising anyone that MPPI is optimal.