MPC

Model predictive control is one of the four steering laws in pure_pursuit_node. Each tick it plans the next second of steering: 20 moves, 50 ms apart. The plan is the one that keeps the car closest to the racing line for the least steering, subject to the servo’s angle limit and its rate limit, both treated as hard limits. The controller applies the first move, discards the rest, and plans again on the next tick from where the car now is. It is the only law that sees a bend coming precisely, and the only one that can run out of time.

Only the steering step changes. The LB check, the pose watchdogs, the safety net, overtaking and every speed limit are the same as for pure pursuit. MPC has been driven only in the simulator. The code is MpcLaw in tracking_laws.pyGitHub, and the excerpts below are from it with docstrings and input checks removed. It builds on the LQR page’s model, which is worth reading first.

Topics

MPC runs inside pure_pursuit_node, so its topics are that node’s: see Pure pursuit for the full tables. Of those inputs, MPC uses the pose, the measured speed from /odom, and the racing line’s points, curvature and speed profile. The path the dashboard draws ahead of the car, from /drive_intent, replays MPC’s planned steering sequence rather than solving again.

Looking ahead along the line

MPC uses LQR’s state, the cross-track error $e$ and the heading error $\theta =\psi -{\psi }_p$, and LQR’s model of how they change. The difference is that it looks ahead. Starting from where the car is on the line, it steps forward $N$ = 20 times (mpc.horizon_steps), $\Delta t_p$ = 0.05 s apart (mpc.dt), and at each step $k$ samples the line’s profiled speed $v_k$ and curvature ${\kappa }_k$. The arc length advances by $v_k\Delta t_p$ each step, so the horizon covers what the car will drive in the next second at the profiled speeds. Each sample gives a feedforward ${\delta }_{ff,k}=\arctan (L{\kappa }_k)$.

s = s0
for k in range(n):
    speeds[k] = max(sample_at_arc(path.speed_profile, path.cumulative, s, path.closed),
                    LqrGainTable.V_FLOOR)
    kappas[k] = sample_at_arc(path.curvature, path.cumulative, s, path.closed)
    ff[k] = math.atan(self.limits.wheelbase * kappas[k])
    if k + 1 < n:
        s = s + speeds[k] * dt_p

The model now changes from step to step, because the speed and the bend change along the horizon. That is what “linear time-varying” means: linear at each step, with different numbers at each step.

$B_k$ has an extra factor of $1/{\cos }^2{\delta }_{ff,k}$ compared with LQR’s. The bicycle model’s turning rate depends on $\tan \delta$, and LQR approximates $\tan \delta$ by a straight line around the feedforward. The slope of $\tan$ at ${\delta }_{ff}$ is its derivative, ${\sec }^2{\delta }_{ff}=1/{\cos }^2{\delta }_{ff}$. On a straight that is 1, so the two models agree. In a tight bend it is larger, because a little more steering turns the car noticeably more when the wheels are already turned.

The optimization problem

The unknowns are the 20 steering moves ${\delta }_0,\dots ,{\delta }_{19}$. The plan minimizes

subject to

Each part has a job:

${\delta }_{-1}$ is the command the car is already holding. The first move is a control period away, so ${\tau }_0$ = 0.025 s, and the rest are a prediction step apart, ${\tau }_k$ = 0.05 s.

The terminal cost. A one-second plan cannot see what comes after it, and a plan that ignores the future can end the horizon pointing the wrong way. LQR’s $P$ matrix is exactly the least cost still to come from any final state, if LQR drove from there on. Charging ${\mathbf{x}}_N^{\top }{P}_N{\mathbf{x}}_N$ at the end therefore makes the short plan behave like an infinitely long one on a straight. $P_N$ is computed for the prediction step $\Delta t_p$ and the speed at the end of the horizon, with MPC’s own weights.

Removing the states

The states ${\mathbf{x}}_1\dots {\mathbf{x}}_N$ depend only on the start state ${\mathbf{x}}_0$ and the steering moves, so they can be written in terms of the moves and removed. Stacking all the steps:

Each $A_k$ is a shear, a matrix of the form $\left[\begin{array}{cc}1& a\\ 0& 1\end{array}\right]$, and multiplying two shears adds their corner entries:

So a product of many $A_k$ is the shear whose corner is the running sum $c_k=a_0+\cdots +a_k$, with $a_k=v_k\Delta t_p$. That gives $\Phi$ and $\Gamma$ in closed form, without multiplying 2×2 matrices in a loop, which matters because this runs inside an 8 ms budget every tick.

a = speeds * dt_p
b = speeds * dt_p / (np.cos(ff) ** 2) / wheelbase
c = np.cumsum(a)
lower = np.tril(np.ones((n, n)))
Phi = np.zeros((2 * n, 2))
Phi[0::2, 0] = 1.0
Phi[0::2, 1] = c
Phi[1::2, 1] = 1.0
Gamma = np.zeros((2 * n, n))
Gamma[0::2] = (c[:, None] - c[None, :]) * b[None, :] * lower
Gamma[1::2] = b[None, :] * lower

Rows 0::2 are the $e$ rows and rows 1::2 the $\theta$ rows. lower keeps only moves at or before each step: a steering move can only affect the future.

Substituting the stacked states into $J$ leaves a function of the 20 moves alone, of the form

with $H={\Gamma }^{\top }W\Gamma +rI+R_{\text{rate}}$, where $W$ holds $Q$ for every step and $P_N$ for the last. That is a quadratic program: a quadratic cost with linear limits. Both limits fit one form, $\mathbf{l}\le M\mathbf{\delta }\le \mathbf{u}$, by stacking the identity (for the angle) on a difference matrix (for the rate).

Solving it

The OSQP solver solves the quadratic program. Three things keep it fast enough to run 40 times a second on the Jetson:

• The problem shape never changes. OSQP is set up once with the pattern of non-zero entries, and each tick only the numbers are updated, not rebuilt.
• Warm start. Each solve starts from the last plan. The car moved only 25 ms since then, so the last plan is close to the new answer.
• A deadline. mpc.deadline_sec (8 ms) is both OSQP’s own time limit and a wall-clock check after the solve.

On the Jetson, with the full simulated race stack running beside it, solves took 1.9 ms typically and 3.5 ms at the 99.9th percentile. 4 solves in 20,000 were late, none of them consecutive.

When a solve fails

A solve that is late, returns any status other than solved, contains a non-finite number, or breaks the angle limit by more than $10^{-4}$ rad is a miss:

OSQP meets its limits only to about $10^{-5}$, so the check allows $10^{-4}$ rad, which is far below anything the servo can resolve. Before it is used, the plan is clamped move by move onto the exact angle and rate limits.

status_ok = result.info.status == 'solved'
if not status_ok:
    self._warm_start = None
    raise MpcMiss(f'status: osqp returned "{result.info.status}"')
if elapsed > self.params.deadline_sec:
    self._warm_start = None
    raise MpcMiss(f'deadline: solve took {elapsed * 1e3:.1f}ms ...')

Like Stanley and LQR, MPC predicts the pose mpc.delay_compensation (0.05 s) ahead before planning; see the Stanley page. It does not use the smoothing filter, because the rate term and the rate limit already keep its output smooth.

Parameters

Set in pure_pursuit.yamlGitHub, read at launch only.

$N\times \Delta t_p$ is the preview: 1.0 s with the defaults. A longer horizon sees further but costs solve time.

Run it

MPC needs the osqp Python package. See Set up the workspace, step 5. Without it, the node refuses to start with steering_law 'mpc' needs the 'osqp' package.

ros2 launch racerbot_launch race_launch.py run_dir:=$HOME/racerbot-ws/data/<id>/racerbot_auto/<run> steering_law:=mpc

The first time it runs on a car: wheels off the ground, then the floor at about 1 m/s, then race speed.

Read the decision log

A decision line for normal driving includes the law’s own detail, for example:

mpc e=+0.015m th=-0.006rad solve=1.9ms it=25

e and th are the two errors, solve is how long OSQP took, and it is how many iterations it used. While MPC runs, the node also prints a timing summary every 5 s. Solve times creeping toward 8 ms mean something else is loading the CPU.

Tune it

python3 tools/f1tenth_sim/law_sweep.py --laws mpc --seed 1000 --repeat-seeds 3 \
    --law-param mpc.q_lateral=6.0

In the October 19, 2026 comparison, MPC passed 15 of 15 runs with a mean cross-track error of 0.0694 m (0.6% worse than pure pursuit) and a mean lap of 145.3 s (1.5% faster). See the comparison of the four laws.

Troubleshooting

Limitations

• MPC has been driven only in the simulator.
• It plans with the kinematic bicycle model and the profiled speeds, not the car’s actual speed along the horizon, and it has no tyre slip.
• It is the only law that depends on finishing a computation in time. A loaded Jetson turns into fallbacks and, after three in a row, a stop.