Part 2: MPC - A Driver Who Knows the Car

PID worked well in Part 1. The car followed the track, tracked the speed, and everything looked fine. It can make controller design look easy.

But let’s make the track harder. Add a sharper S-curve, ask the car to carry more speed, or give it a planner trajectory that looks clean on a map but asks the car to turn faster than it physically can. How do we still control the car smoothly and safely?

Tuning the PID controller can help in one scenario, but it does not make the controller robust enough for every scenario. PID sees the speed error, the tracking error, and the change in those errors, then tries to reduce them. It does not know whether the car can physically make that correction.

The controller should respect the planner output, but it should not copy the planner blindly. A planner can make mistakes because it usually works with a simpler model. It cannot account for every detail of vehicle dynamics without paying too much compute cost. We will discuss that tradeoff more in a later part. If the controller tries to execute a difficult trajectory exactly, without considering the real vehicle, it can become unstable.

To solve this, we need a controller that knows the car’s details: how fast it can turn the wheel, how hard it can accelerate, and how its position changes after each command. We call that knowledge the car model. Think of MPC as a driver who knows the car well enough to predict how it will respond before making a move. PID is more like a driving instructor who gives correction commands after the car has already drifted off line.

That is what we will learn in this post: MPC, or Model Predictive Control, and how MPC uses the car model.

Think of PID as driving with one goal: follow a point ahead. If the car drifts from the lane, PID makes an adjustment to bring it back. MPC is more like a driver who scans the road ahead, mentally simulates the next few seconds, and picks the steering and throttle that work best for the whole sequence. It may not choose the action that looks best for the next instant. It chooses the next action that creates the best predicted future.

Before You Start

This post uses SteerPy, an autonomous driving playground that runs in the browser. If you have not used it yet, start with Part 0 for the quick tour: open the page, write Python, press run, and watch the car react.

Setup

World Config

# world_config.py

waypoints = [
    # Bottom curve (smooth, wide arc)
    (-50, -30), (-30, -45), (0, -50), (30, -45), (50, -30),
    # First turn - sharp up and left
    (62, -20), (68, -5), (65, 10),
    # Second turn - sharp down and right
    (55, 22), (40, 18), (28, 5),
    # Third turn - sharp up and left
    (18, 20), (12, 38), (0, 42),
    # Fourth turn - sharp down and right
    (-12, 35), (-22, 20), (-32, 5),
    # Back to bottom curve
    (-42, -10), (-48, -22), (-50, -30),
]
road_width = 8.0
sample_distance = 1.0
loop = True
obstacles = []

# Start on the bottom curve
# x, y, heading_deg, steering_deg, speed_mps
car_init = [-30.0, -45.0, 45.0, 0.0, 0.0]

Planner

# planner.py

def planner(car, world_model=None):
    if world_model is None:
        return []

    path = world_model.road_data.get("path", [])
    if not path:
        return []

    closest_i = min(
        range(len(path)),
        key=lambda i: (path[i][0] - car.x) ** 2 + (path[i][1] - car.y) ** 2,
    )

    trajectory = []
    horizon = 50
    for step in range(horizon):
        idx = (closest_i + step) % len(path)
        x, y = path[idx]
        target_speed = 6.0
        trajectory.append((x, y, target_speed))

    return trajectory

Full Working Code

Paste this into controller.py and run SteerPy. The car should move through the S-curve with smoother steering than the PID controller from Part 1.

# controller.py
import math
import random
from collections import namedtuple

# ---- Predictive model parameters (must match your car_config.py) ----
WHEELBASE   = 2.8     # axle-to-axle distance in meters
ACCEL_FORCE = 12.0    # peak acceleration at accel_cmd = 1.0 (m/s^2)
FRICTION    = 0.16    # speed decay coefficient per second
STEER       = 40.0    # max front-wheel angle (deg); steer_cmd * STEER = wheel angle

# DT_MPC: time step used inside the predictive model.
# At 8 m/s: DT_MPC=0.1 -> each step ~0.8 m, HORIZON=12 -> ~9.6 m of lookahead.
# If set to 1/60 (real frame time): each step = 0.13 m, HORIZON=12 -> only 1.6 m.
DT_MPC = 0.1

# ---- MPC settings ----
HORIZON   = 12    # how many steps to simulate forward

# Random shooting
N_SAMPLES = 200   # candidate sequences per frame

# Gradient descent
OPT_ITERS = 8     # gradient steps per frame
OPT_EPS   = 0.05  # finite difference perturbation size
OPT_STEP  = 0.3   # normalized gradient step size

# Switch: False = random shooting, True = gradient descent
USE_OPTIMIZER = True

# ---- Cost weights ----
W_LAT    = 10.0   # lateral deviation from lane center
W_SPEED  = 2.0    # speed error
W_STEER  = 0.05   # steering magnitude
W_DSTEER = 0.5    # steering rate: penalizes (steer[k] - steer[k-1])^2
W_CORNER = 0.3    # lateral g-force: penalizes speed^2 * steer^2 (fast cornering)

# ---- Module state ----
_opt_start = None   # warm start for gradient descent

CarState = namedtuple("CarState", ["x", "y", "heading", "speed", "steer_deg"])

def clamp(v, lo, hi):
    return lo if v < lo else (hi if v > hi else v)


def model(state, accel_cmd, steer_cmd):
    """One bicycle-model step at DT_MPC seconds. Returns a new CarState."""
    x, y, heading, speed, steer_deg = state
    steer_deg = steer_cmd * STEER

    speed += accel_cmd * ACCEL_FORCE * DT_MPC
    speed *= (1.0 - FRICTION * DT_MPC)
    speed  = clamp(speed, -12.0, 36.0)

    yaw_rate = (speed / WHEELBASE) * math.tan(-math.radians(steer_deg))
    heading += math.degrees(yaw_rate * DT_MPC)

    rad = math.radians(heading)
    x  += math.cos(rad) * speed * DT_MPC
    y  += math.sin(rad) * speed * DT_MPC

    return CarState(x, y, heading, speed, steer_deg)


def rollout(init, controls, trajectory):
    """Simulate a control sequence from init. Returns total accumulated cost."""
    state      = init
    cost       = 0.0
    n          = len(trajectory)
    ref_idx    = 1
    prev_steer = state.steer_deg / STEER   # normalize to [-1, 1] command space

    for accel_cmd, steer_cmd in controls:
        state = model(state, accel_cmd, steer_cmd)

        # Advance ref_idx to the closest trajectory point ahead of the car.
        # Needed because DT_MPC > DT: each planning step skips several path points.
        while ref_idx + 1 < n:
            d_cur  = (trajectory[ref_idx][0]     - state.x) ** 2 + (trajectory[ref_idx][1]     - state.y) ** 2
            d_next = (trajectory[ref_idx + 1][0] - state.x) ** 2 + (trajectory[ref_idx + 1][1] - state.y) ** 2
            if d_next < d_cur:
                ref_idx += 1
            else:
                break
        tx, ty, t_speed = trajectory[ref_idx]

        # Lateral error: cross product of heading and displacement
        dx, dy  = tx - state.x, ty - state.y
        rad     = math.radians(state.heading)
        hx, hy  = math.cos(rad), math.sin(rad)
        lat_err = hx * dy - hy * dx

        cost += W_LAT    * lat_err ** 2
        cost += W_SPEED  * (t_speed - state.speed) ** 2
        cost += W_STEER  * steer_cmd ** 2
        cost += W_DSTEER * (steer_cmd - prev_steer) ** 2
        cost += W_CORNER * state.speed ** 2 * steer_cmd ** 2
        prev_steer = steer_cmd

    return cost


def mpc_step_random(init, trajectory):
    """Random shooting: try N_SAMPLES random sequences, return the best first action."""
    best_cost = float("inf")
    best_seq  = [(0.0, 0.0)] * HORIZON

    for _ in range(N_SAMPLES):
        seq = [
            (random.uniform(-1.0, 1.0),
             random.uniform(-1.0, 1.0))
            for _ in range(HORIZON)
        ]
        cost = rollout(init, seq, trajectory)
        if cost < best_cost:
            best_cost = cost
            best_seq  = seq

    return best_seq[0]


def fd_gradient(init, controls, trajectory):
    """Finite-difference gradient of rollout cost w.r.t. every control variable."""
    controls  = list(controls)
    base_cost = rollout(init, controls, trajectory)
    grads     = []

    for k in range(len(controls)):
        accel, steer = controls[k]

        controls[k] = (clamp(accel + OPT_EPS, -1.0, 1.0), steer)
        grad_accel   = (rollout(init, controls, trajectory) - base_cost) / OPT_EPS
        controls[k]  = (accel, steer)

        controls[k] = (accel, clamp(steer + OPT_EPS, -1.0, 1.0))
        grad_steer   = (rollout(init, controls, trajectory) - base_cost) / OPT_EPS
        controls[k]  = (accel, steer)

        grads.append((grad_accel, grad_steer))

    return grads


def mpc_step_opt(init, trajectory):
    """Gradient descent: improve a control sequence."""
    global _opt_start

    if _opt_start is None or len(_opt_start) != HORIZON:
        controls = [(0.0, 0.0)] * HORIZON
    else:
        # Warm start: shift last solution forward by one step
        controls = list(_opt_start[1:]) + [(0.0, 0.0)]

    for _ in range(OPT_ITERS):
        grads      = fd_gradient(init, controls, trajectory)
        total_norm = sum(ga ** 2 + gs ** 2 for ga, gs in grads) ** 0.5 + 1e-9
        controls   = [
            (clamp(a - OPT_STEP * ga / total_norm, -1.0, 1.0),
             clamp(s - OPT_STEP * gs / total_norm, -1.0, 1.0))
            for (a, s), (ga, gs) in zip(controls, grads)
        ]

    _opt_start = controls
    return controls[0]


def controller(car, trajectory):
    if not trajectory:
        return [0.0, 0.0]

    init = CarState(car.x, car.y, car.angle, car.speed, car.steer_angle)

    if USE_OPTIMIZER:
        accel_cmd, steer_cmd = mpc_step_opt(init, trajectory)
    else:
        accel_cmd, steer_cmd = mpc_step_random(init, trajectory)

    return [accel_cmd, steer_cmd]

Model Predictive Controller

The MPC workflow is:

Generate candidate control sequences over a short time window. For example: at 0.1s, apply steering and throttle A; at 0.2s, apply steering and throttle B; keep going until the prediction horizon ends.
Simulate how the car behaves if it applies the entire sequence.
Compute the cost of each sequence: how well the car follows the planned trajectory, how smooth it feels, and how safe the motion looks.

MPC repeats this process many times, picks the sequence with the lowest cost, and applies the first command from that sequence.

MPC is built from three components: the model, the cost, and the optimizer.

The model

PID reacts to error after it happens. MPC asks what will happen before it commits to a control. The model answers: “If I apply this throttle and steering now, where will the car be in 0.1 seconds?”

Think of it like a video game physics engine. The game does not guess where your car ends up. It runs the physics equations forward every frame. The model in MPC does the same thing. MPC calls the model hundreds of times per frame, testing different control combinations.

In this example, the controller uses a bicycle model, a common simplified vehicle model. It treats the two front wheels as one wheel and the two rear wheels as one wheel. That sounds suspicious until you remember we are trying to predict lane following, not simulate tire rubber at a PhD-defense level.

The state has five values:

Value	Meaning
`x`, `y`	Car position in the world
`heading`	Direction the car points, in degrees
`speed`	Forward speed in meters per second
`steer_deg`	Current front-wheel angle in degrees

The control command has two values:

Value	Meaning
`accel_cmd`	Normalized throttle/brake command from `-1.0` to `1.0`
`steer_cmd`	Normalized steering command from `-1.0` to `1.0`

Inside model(), the controller converts steer_cmd into a real wheel angle:

steer_deg = steer_cmd * STEER

Then it updates speed:

speed += accel_cmd * ACCEL_FORCE * DT_MPC
speed *= (1.0 - FRICTION * DT_MPC)

ACCEL_FORCE says how hard the engine can push. FRICTION removes a little speed each step. DT_MPC is the prediction timestep, so the model applies 0.1 seconds of acceleration at a time.

The steering update comes from the bicycle model:

yaw_rate = (speed / WHEELBASE) * math.tan(-math.radians(steer_deg))
heading += math.degrees(yaw_rate * DT_MPC)

yaw_rate means how fast the car rotates. Higher speed rotates the car faster for the same steering angle. A longer WHEELBASE rotates slower because long vehicles need more room to turn. Anyone who has tried to park a moving truck has learned this lesson against their will.

Last, the car moves forward along its new heading:

rad = math.radians(heading)
x += math.cos(rad) * speed * DT_MPC
y += math.sin(rad) * speed * DT_MPC

The cost

MPC tries hundreds of candidate control sequences and picks the best one. But “best” needs a definition. The cost function is that definition: lower cost equals better future.

Think of the cost function as a driving instructor with a clipboard. It watches each simulated future and adds penalties:

Too far from lane center? Add cost.
Too slow or too fast? Add cost.
Steering too hard? Add cost.
Steering snapping back and forth? Add cost.
Cornering too fast? Add cost.

The rollout() function runs one full control sequence through the model and accumulates those penalties:

cost += W_LAT    * lat_err ** 2
cost += W_SPEED  * (t_speed - state.speed) ** 2
cost += W_STEER  * steer_cmd ** 2
cost += W_DSTEER * (steer_cmd - prev_steer) ** 2
cost += W_CORNER * state.speed ** 2 * steer_cmd ** 2

Each W_ value is a weight. Bigger weight means the controller cares more about that penalty.

Term	What it penalizes	Why it helps
`W_LAT`	Lane-center error	Keeps the car on the path
`W_SPEED`	Speed error	Tracks the target speed
`W_STEER`	Large steering commands	Avoids unnecessary steering
`W_DSTEER`	Fast steering changes	Smooths the wheel motion
`W_CORNER`	Speed while steering	Discourages harsh cornering

The lateral error uses the same cross-product idea from Part 1:

lat_err = hx * dy - hy * dx

hx, hy point in the car’s heading direction. dx, dy point from the car to the target point. The cross product tells us whether the target is left or right of the car, and how far off the car is.

Try this: set W_DSTEER = 0.0. The car may still stay in the lane, but the steering commands become choppy. The optimizer found a path that works in the math, which is a polite way of saying your passenger now hates you.

Change	Expected behavior
`W_DSTEER = 0.0`	Jagged steering
`W_DSTEER = 2.0`	Smoother steering, possible corner cutting
`W_CORNER = 0.0`	Faster cornering, less comfort
`W_CORNER = 1.0`	Slower, more conservative turns

The cost function is where you tell MPC what kind of driver you want.

The Optimizer

The optimizer is the way to generate the control sequence with the lowest cost.

A control sequence is a list of future commands:

[
    (accel_0, steer_0),
    (accel_1, steer_1),
    (accel_2, steer_2),
    ...
]

With HORIZON = 12, the controller chooses 12 throttle commands and 12 steering commands. In a perfect world, we could generate every possible control sequence, apply each one in the model, score it, and select the best. But there are too many possibilities. Acceleration and steering live in continuous space, so we cannot enumerate them all. The controller also needs to respond fast. We need an efficient way to search for a good sequence.

Random Shooting

The most naive approach is to sample randomly.

Generate N_SAMPLES random control sequences.
Simulate each sequence with rollout().
Keep the sequence with the lowest cost.
Return its first command.

That is what mpc_step_random() does.

Random shooting works well as a teaching tool because there is no magic inside it. Sample randomly, score the samples, and pick the best one.

We can limit the number of samples to control computation time. The downside is waste: each sample is generated blindly, and most samples get discarded.

What if we could guess which “direction” improves a control sequence? Then we would not need to throw away the whole sequence. We could improve it bit by bit until it becomes good enough.

That is gradient descent.

Gradient Descent

Gradient descent starts with a guess, measures which changes reduce cost, and nudges the sequence in that direction. In this example, the first guess is either all zeros or the warm-started solution from the previous frame.

Think of gradient descent as standing on a hill. Your height is the cost, and you want to walk downhill. You test the ground around you, feel which direction slopes down, and take a step that way. Repeat that enough times, and you move toward the bottom of the hill: the place with lower cost.

For MPC, the “hill” is the cost of the whole control sequence. A small change to throttle at step 3 or steering at step 8 can move the sequence uphill or downhill.

The code estimates each gradient with finite differences. For each command value, it perturbs the value a little, reruns the rollout, and checks how the cost changed:

controls[k] = (clamp(accel + OPT_EPS, -1.0, 1.0), steer)
grad_accel = (rollout(init, controls, trajectory) - base_cost) / OPT_EPS

If increasing accel raises the cost, the gradient points upward, so the optimizer steps the other way. If increasing steer lowers the cost, the optimizer keeps moving in that direction.

After it computes all gradients, the optimizer updates the whole sequence:

controls = [
    (clamp(a - OPT_STEP * ga / total_norm, -1.0, 1.0),
     clamp(s - OPT_STEP * gs / total_norm, -1.0, 1.0))
    for (a, s), (ga, gs) in zip(controls, grads)
]

OPT_STEP controls how far each update moves. A small value takes more iterations. A large value may jump past the good answer and oscillate.

The warm start makes this usable in a real-time loop:

controls = list(_opt_start[1:]) + [(0.0, 0.0)]

Warm start means the optimizer starts from the previous best sequence instead of starting cold every frame. If you already stood near the bottom of the hill in the last frame, you should not teleport to a random place before taking the next step. The code shifts the previous solution forward by one step, appends a neutral command at the end, and improves from there.

Tuning Checklist

Match the model to the simulator: Set WHEELBASE, ACCEL_FORCE, FRICTION, and STEER to match car_config.py. A mismatched model can still work, but it will spend effort correcting its own bad predictions.
Pick a useful lookahead: HORIZON * DT_MPC gives the prediction time. With HORIZON = 12 and DT_MPC = 0.1, the controller looks 1.2 seconds ahead. At 6 m/s, that covers about 7.2 meters.
Tune lane tracking first: Raise W_LAT until the car stays near the lane center. Then add W_DSTEER for smooth steering and W_CORNER for comfort. Exact speed matters less than staying on the road. Bold claim, but the ditch tends to agree.
Balance optimizer work: OPT_ITERS = 8 is a good starting point. Lower it if your frame rate drops. Raise it if the steering still looks rough and you have CPU time left.
Watch the steering plot: Good MPC starts steering before the curve. If the plot shows sharp corrections inside the curve, increase HORIZON, increase W_DSTEER, or reduce DT_MPC.

MPC tuning is a tradeoff between lookahead, smoothness, and compute time.