Sampling-Based Planning and Model Predictive Control › Model Predictive Control

Model predictive control is what you get when you take the horizon-restricted planner we just built and run it repeatedly in a closed loop. The formal problem statement is the following constrained, finite-horizon optimal control problem, solved from the measured state $s_t$ at each control tick:

The pieces are:

• Stage cost $\ell(s, a)$: the running penalty for being at state $s$ and applying control $a$. In our example, $\ell = \|p - g\|^2 + \alpha \|a\|^2$.
• Terminal cost $\ell_f(s_H)$: an additional penalty on the last state, which shapes the behavior of the plan's tail.
• Dynamics constraint $s_k = f(s_{k-1}, a_{k-1})$: the model. This is the only place the model enters the optimization.
• Control set $\mathcal{U}$: allowed control values.
• State set $\mathcal{S}$: allowed states. In the double integrator, this could be a velocity limit or a region of the plane the robot must stay in.

Each piece deserves a sentence of motivation, because the design of these terms is where most MPC engineering happens. The stage cost is the only term that expresses what it means to do the task well at every intermediate moment; if it is misspecified, the controller optimizes the wrong thing confidently. The terminal cost matters more than its small footprint suggests, because it is the sole representation of everything that happens after the horizon. A well-chosen terminal cost can substitute for a long horizon, and a poorly chosen one can bias the controller into a bad habit, for example a terminal cost that rewards speed at the end of the plan produces controllers that arrive fast and overshoot. The dynamics constraint is where the model lives, and it is written as an equality because the plan must be consistent with the model; a plan that violates the dynamics is not a plan at all, it is a wish. The control set bounds what actuators can physically do. The state set encodes safety and feasibility requirements, such as keeping a drone inside a geofence or a velocity below a structural limit.

Written this way, the problem has $H d_a$ control variables in a single-shooting formulation; a multiple-shooting formulation also represents states explicitly and enforces dynamics with equality constraints. Linear dynamics, a convex quadratic objective, and linear constraints produce a convex quadratic program. Stability guarantees for MPC require additional conditions, such as suitable terminal ingredients, invariant sets, or sufficiently long horizons; they do not follow merely from using a quadratic program. Mayne et al.'s review develops these conditions for constrained MPC. With a learned nonlinear or non-differentiable model, sampling-based single shooting enforces the dynamics by rollout rather than by placing state-transition equalities in the numerical optimizer.

The MPC algorithm is:

1. Observe or estimate the current state $s_t$.
2. Approximately solve the finite-horizon problem for a candidate action sequence $\hat a_{0:H-1}$.
3. Apply only the first action $\hat a_0$ to the real system.
4. Advance time by one step, observe the new state $s_{t+1}$, and go back to step 2.

Applying the first action and solving again at the next state is the receding-horizon principle. This is the same structure introduced in classical optimal control (see Part II, Chapter 4), here implemented with a sampling-based solver. The horizon slides forward with time, so each optimization covers a fixed number of future steps from the current state estimate.

Receding-horizon feedback limits how long the controller follows an uncorrected prediction. Once the actual or estimated state after $a_0^\star$ is available, the next optimization starts from that updated condition rather than the previous predicted state. This can reject some disturbances and model mismatch, but it does not make an inaccurate model harmless. Closed-loop stability and constraint satisfaction depend on the system, estimator, horizon, objective, terminal design, solver quality, and mismatch bounds. Frequent replanning is feedback, not by itself a stability proof.

Open-loop planning applies a sequence without incorporating new state information, whereas MPC observes or estimates the state and replans after the first action or a short block. That feedback can reduce sensitivity to disturbances represented in the measurement and model. It does not automatically confer robustness: unobserved disturbances, estimator error, actuator delay, infeasible replans, or large model mismatch can still defeat the controller. How conservative the resulting policy should be depends on the uncertainty model and constraints, not simply on whether replanning is present.

There is an important connection here to state estimation. When the state is not directly measured, step 1 requires an estimate or belief constructed from observations. Under the standard linear-Gaussian assumptions of LQG control, the separation principle permits an optimal controller to combine a Kalman state estimate with the full-state LQR law. That result does not justify replacing a filter by an arbitrary learned encoder. For nonlinear learned systems, feeding a point estimate or latent state to MPC is an approximation unless the estimator and controller satisfy stronger problem-specific conditions. A confidently wrong estimate can therefore drive a precisely solved plan toward the wrong state. Part VII, Chapter 3 develops planning from beliefs rather than treating an estimate as truth.

Common inner solvers include random shooting, CEM, and MPPI. For a fixed time budget, iterative proposal fitting can outperform uniform random shooting when low-cost controls occupy a region that the chosen proposal can represent. Multi-modal or discontinuous objectives can instead cause a single Gaussian CEM proposal to average incompatible modes or collapse prematurely. Random shooting retains broad coverage but spends no evaluations refining a promising region. A useful mental model is that CEM performs adaptive local search through its proposal, while random shooting preserves a fixed global proposal. The outcome is empirical, so wall-clock budgets and seeds should be matched when choosing between them.

We now assemble the whole thing into a closed-loop controller on our double integrator, with a small amount of process noise to represent the environment not matching the model exactly. The controller will plan, execute one step, replan, and repeat.

The inner loop of mpc_rollout contains the MPC pattern: plan from the current simulated plant state, execute the first action, update the state, shift the proposal, and repeat. The state s is updated by the noisy plant step rather than the planner's nominal prediction. The plan persists only as a CEM proposal mean. Shifting aligns its remaining actions with the new time index; without the shift, the proposal is stale and can slow or misdirect finite-budget optimization, although CEM may still recover.

The step_with_noise function is the ground-truth system, not the model. The model (inside cem) still thinks it is driving a clean double integrator. The gap between them is what makes the comparison meaningful. Separating "model" and "plant" is a useful debugging experiment: if behavior changes only after mismatch is introduced, that implicates model error or the controller's sensitivity to it; failure even with matched dynamics instead directs attention toward the planner, objective, implementation, or state representation. This comparison narrows the diagnosis but does not prove a unique cause.

Let us run both strategies with the same noise sequence.

The numbers illustrate the mechanism in this fixed-seed example. Open-loop planning accumulates the injected process perturbations and cannot respond after the initial plan. MPC incorporates the perturbed state into the next optimization. Both runs use the same noise sequence; within this paired implementation, the operational difference is replanning. The example demonstrates disturbance correction, not a population-level performance guarantee.

The comparison applies independent perturbations with standard deviation 0.06 to each of the four discrete state components at every step for 40 steps, matching the open-loop plan length. Direct position perturbations accumulate like a random walk, while velocity perturbations also propagate into later positions through the dynamics. The open-loop plan cannot react to either source. MPC observes the resulting state each step and replans. Feedback explains the mechanism, but the printed improvement factor is specific to this seed, planner budget, noise model, and objective.

For this paired run, MPC ends closer to the goal and its distance-to-goal curve recovers after disturbances, while the open-loop run levels off. The distance curve shows the trajectory of the error rather than only its final value. Neither MPC nor its expected distance is generally monotone without additional assumptions; the point here is simply that the feedback controller retains a mechanism for correcting observed deviation.