PID Controller: Make Your Car Hold the Lane

Every self-driving car needs to answer two questions every single step: how fast to go, and where to steer. PID is one of the oldest, simplest answers to both. It shows up in cruise control, lane keeping, even the throttle response in a Tesla. Not because it is fancy, but because it actually works.

PID stands for Proportional, Integral, Derivative. Three terms. You give it an error (the gap between where you are and where you want to be), and it tells you how hard to push to close that gap. For an autonomous car, that means one PID loop watching your speed and another watching your lane position.

The formula looks simple enough that most people convince themselves they get it, right up until their robot does something spectacular and wrong. So let’s skip the boring theory and get the car moving first. The explanations will hit different once you have already seen it work.

Setup

World config

Open world_config.py and set a simple loop with no obstacles:

# world_config.py

waypoints = [
    # Bottom side (heading east) - loop seam is here, mid-straight
    (0, -50),
    (20, -50),
    (40, -50),
    # Bottom-right corner
    (46, -47),
    (50, -40),
    # Right side (heading north)
    (50, -20),
    (50, 0),
    (50, 20),
    (50, 40),
    # Top-right corner
    (46, 47),
    (40, 50),
    # Top side (heading west)
    (20, 50),
    (0, 50),
    (-20, 50),
    (-40, 50),
    # Top-left corner
    (-46, 47),
    (-50, 40),
    # Left side (heading south)
    (-50, 20),
    (-50, 0),
    (-50, -20),
    (-50, -40),
    # Bottom-left corner
    (-46, -47),
    (-40, -50),
    # Bottom side (heading east) - back to mid-straight
    (-20, -50),
]
road_width = 8.0
sample_distance = 1.0
loop = True

obstacles = []

# x, y, heading_deg, steering_deg, speed_mps
car_init = [0.0, -50.0, 0.0, 0.0, 0.0]

The track is a large rounded rectangle, roughly 100x100 units. The long straights let the speed controller settle and hold a steady cruising speed. The four corners stress the steering controller: it has to turn hard, hold the lane through the apex, then straighten back out for the next straight.

Planner

The planner’s only job here is to return lane-center points and a target speed. That is all the controller needs.

# 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 = 5.0
        trajectory.append((x, y, target_speed))

    return trajectory

Keep the planner boring so the controller is the interesting part.

The Full Controller

Here is the complete controller. Paste it in and run it first. Then read the sections below to understand what each part does.

# controller.py
import math

# Speed PID gains
KP_SPEED = 0.30
KI_SPEED = 0.02
KD_SPEED = 0.005

# Steering PID gains
KP_STEER = 0.50
KI_STEER = 0.0005
KD_STEER = 0.0   # see note in steering section - lookahead handles damping instead

# Lookahead index (which trajectory point to steer toward)
LOOKAHEAD = 6

# State (persists between calls)
speed_integral = 0.0
previous_speed_error = 0.0
steer_integral = 0.0
previous_steer_error = 0.0


def clamp(value, low, high):
    return max(low, min(high, value))


def speed_pid(target_speed, current_speed, dt):
    global speed_integral, previous_speed_error

    error = target_speed - current_speed

    speed_integral += error * dt
    speed_integral = clamp(speed_integral, -5.0, 5.0)

    derivative = (error - previous_speed_error) / dt
    previous_speed_error = error

    throttle = KP_SPEED * error + KI_SPEED * speed_integral + KD_SPEED * derivative
    return clamp(throttle, 0.0, 1.0)


def steering_pid(car, trajectory, dt):
    global steer_integral, previous_steer_error

    lookahead_idx = min(LOOKAHEAD, len(trajectory) - 1)
    target_x, target_y, _ = trajectory[lookahead_idx]

    heading_rad = math.radians(car.angle)
    heading_x = math.cos(heading_rad)
    heading_y = math.sin(heading_rad)

    dx = target_x - car.x
    dy = target_y - car.y

    # Positive = target is to the left of the car's heading
    lateral_error = heading_x * dy - heading_y * dx

    steer_integral += lateral_error * dt
    steer_integral = clamp(steer_integral, -10.0, 10.0)

    derivative = (lateral_error - previous_steer_error) / dt
    previous_steer_error = lateral_error

    steer = KP_STEER * lateral_error + KI_STEER * steer_integral + KD_STEER * derivative
    return clamp(-steer, -1.0, 1.0)


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

    dt = 1.0 / 60.0
    _, _, target_speed = trajectory[0]

    throttle_cmd = speed_pid(target_speed, car.speed, dt)
    steer_cmd = steering_pid(car, trajectory, dt)
    return [throttle_cmd, steer_cmd]

Run this. The car should hold 5 m/s and track the lane center around the loop.

If it looks good, keep reading. The next two sections explain exactly why.

Speed PID

What problem does it solve?

The car has an engine. You tell it a throttle value from 0.0 to 1.0. But you want to control speed, not throttle directly.

The gap between “what I want” and “what I have” is the error:

error = target_speed - current_speed

PID is a feedback loop. Every timestep it reads the error, computes a correction, and applies it. The car’s own physics (friction, inertia) shape how the response plays out.

P: react to the current error

The simplest possible controller:

throttle = KP_SPEED * error

If the car is 2 m/s slower than target speed, apply KP_SPEED * 2 throttle. If it is at target speed, apply zero throttle.

Try this: set KI_SPEED = 0 and KD_SPEED = 0 in your controller. Watch the speed plot in SteerPy.

You will probably see the car settle a bit below target speed. The car needs some throttle just to maintain speed against friction, but when the error is small, P alone does not produce enough throttle to overcome it. The car stops short.

That gap between target and actual is called steady-state error.

You might think: just set KP_SPEED = 1.0. Then even a small error produces enough throttle. True, but now the controller is too aggressive. When the car is 2 m/s too slow, it applies full throttle and shoots past the target. Then it backs off hard, undershoots, overcorrects again. The speed plot oscillates instead of settling. You traded one problem for another.

P alone forces a tradeoff: small KP means steady-state error, large KP means oscillation. That is exactly the gap I was designed to fill.

P reacts to error right now. Big error = big push. Zero error = zero push.

I: fix the persistent gap

The integral term accumulates error over time:

speed_integral += error * dt
throttle += KI_SPEED * speed_integral

The integral works in both directions. If the car is too slow, the error is positive and speed_integral grows, pushing more throttle. If the car is too fast (say, going downhill), the error is negative and speed_integral shrinks, pulling throttle back. It corrects persistent offset in whichever direction it appears.

Think of it as patience. P reacts right now. I notices that the car has been wrong for a while and pushes harder, or backs off if it has been too fast.

Try this: set KD_SPEED = 0 and KI_SPEED = 0 (P only). Watch the speed plot. The car should settle below target speed. That is the steady-state error P alone cannot close.

Now set KI_SPEED = 0.01. Watch the speed slowly climb the rest of the way to target. That is I doing its job.

One risk: if the integral is allowed to grow without bound, it builds up a huge debt that takes a long time to pay off. That is why the code clamps it:

speed_integral = clamp(speed_integral, -5.0, 5.0)

This is called integral windup protection. The clamp value of 5.0 is chosen so that the maximum contribution from I to the throttle is KI_SPEED * 5.0 = 0.02 * 5.0 = 0.10, about 10% throttle. That is enough to close a small steady-state gap, but not so much that the integral can build a debt the car takes seconds to pay off. Without this clamp, a long startup from rest could grow the integral to hundreds, causing a massive overshoot that takes forever to unwind.

I fixes errors that P ignores. If the car has been wrong for a while, I pushes harder until it is right.

D: resist sudden changes

The derivative term looks at how fast the error is changing:

derivative = (error - previous_error) / dt
throttle += KD_SPEED * derivative

Think of a driver easing off the gas before reaching highway speed. They do not wait until they are already at 100 km/h and then slam the brakes. They see the speedometer climbing fast and start backing off early. That is D: it sees the error shrinking quickly and reduces the push before the overshoot happens.

If the error is shrinking quickly (the car is accelerating hard toward target), D reduces throttle a little to avoid overshooting.

It is a damper. It makes the response smoother.

Try this: crank KD_SPEED up to 0.5 and watch the speed plot. The response will feel sluggish and may not reach target speed quickly.

D resists change. The faster the error is shrinking, the harder D brakes to prevent overshoot.

Suggested experiments

Open your controller and try each of these. Watch the speed plot in SteerPy after each change.

Experiment	What to expect
`KI_SPEED = 0`	Car settles below target speed
`KD_SPEED = 0`	Slight overshoot before settling
`KP_SPEED = 1.0`	Fast response, may oscillate
`KP_SPEED = 0.02`	Very slow to reach target
`KI_SPEED = 0.5`	Integral windup, big overshoot

PID only makes sense when you can see what it does. Use the plots.

Steering PID

What problem does it solve?

Speed control was easy: the error is just a number (speed gap).

Steering is harder because you need to know where the car is relative to the lane center. The car can be at the right speed but completely off course.

The error for steering is lateral error: how far to the left or right is the target point relative to the car’s current heading.

heading_rad = math.radians(car.angle)
heading_x = math.cos(heading_rad)
heading_y = math.sin(heading_rad)

dx = target_x - car.x
dy = target_y - car.y

# Cross product of heading and displacement vector
# Positive = target is to the left
lateral_error = heading_x * dy - heading_y * dx

This is the cross product of the car’s heading vector and the displacement vector to the target. Positive means the target is to the left, negative means right.

If lateral error is large, the car needs to steer toward the lane center.

Lookahead distance: why it matters

Here is the first decision that is not obvious: which trajectory point do you steer toward?

You might think “the closest one.” That is wrong.

If you steer toward the point directly underneath the car, the controller reacts to where you are, not where you are going. On a curve, the car ends up cutting corners and chasing its own tail.

Instead, pick a point some distance ahead on the trajectory. That is the lookahead point. The car steers toward that.

lookahead_idx = min(LOOKAHEAD, len(trajectory) - 1)
target_x, target_y, _ = trajectory[lookahead_idx]

LOOKAHEAD = 6 means “steer toward the 6th trajectory point ahead.”

Why does lookahead matter?

Too small: the car reacts to tiny local errors and oscillates nervously. It is like staring at the ground while walking.
Too large: the car ignores the immediate road shape and corners too wide. It is like looking too far ahead while parking.

But lookahead does something else that is not obvious: it makes the controller predictive instead of reactive. With a short lookahead, the car steers toward a point it has almost already passed, always reacting too late. With a longer lookahead, it steers toward where the path is going, so it starts turning before it has already drifted wide. On a straight, this also reduces the oscillation frequency: small lateral deviations get corrected gradually over a longer time horizon rather than snapped back immediately. This is the damping tool for steering, and it does the job without amplifying noise (explained in the D section below).

Try this: set LOOKAHEAD = 1. The steering will oscillate on curves. Then try LOOKAHEAD = 15. The car will track loosely and cut the inside of corners.

On a rectangular track, LOOKAHEAD = 6 is a good starting point. Large enough to smooth out straight-line noise, small enough to start turning before the corner is already behind you.

P: steer toward the lane center

Start with P only:

steer = -KP_STEER * lateral_error

Negative because if the target is to the left (positive error), you want to steer left (negative steer in this sign convention).

Try KI_STEER = 0 and KD_STEER = 0. Whether the car tracks cleanly or oscillates depends on how fast your sim applies steering. KP = 0.5 is on the aggressive end. If it oscillates (steer plot looks like a sine wave), lower KP_STEER toward 0.3, or increase LOOKAHEAD. Change one at a time and watch what happens.

One thing you will notice: changing KP_STEER barely affects the steering on a straight. That is because on a straight the lateral error is near zero, so KP * error is near zero regardless of KP. The difference shows up on corners, where lateral error is large and KP directly controls how hard the car turns in. Smaller KP means the car takes a wider line through corners but is smoother. Larger KP means tighter cornering but risks oscillation on the exit.

If the car barely responds to corners and drifts wide, KP is too small. Raise it.

P steers toward the lane center. The further off you are, the harder it steers.

I: fix persistent lane offset

If the car consistently drifts to one side of the lane, P alone will not fix it. The error never reaches zero, so the integral builds and gradually pushes the car back. Same concept as speed I, just applied to lane position instead of speed.

This matters less for steering than for speed (the track is symmetric and there is less persistent bias). But on real roads, crosswind and road camber create exactly this kind of drift.

Keep KI_STEER very small, around 0.0005. Too much and the car wobbles on the straights.

Try: set KI_STEER = 0.05. The integral accumulates fast and the car steering becomes unstable.

I fixes a car that hugs one side of the lane. Keep it tiny. Steering rarely needs much.

D: why it makes steering worse, not better

For speed control, D is great. Speed is a clean scalar and its derivative is smooth.

Steering is different. The lateral error is computed against a sampled path, a list of discrete points. Even on a perfectly straight road, the lateral error ticks slightly up and down every frame as the closest path index advances. That is not the car moving, that is just floating-point arithmetic stepping through a list.

D divides by dt, which amplifies those ticks into high-frequency spikes. The steering plot becomes jagged. The harder you push KD_STEER, the worse it gets.

Try it: set KD_STEER = 0.40. The steering plot should become noticeably noisier on the straights, even if the car looks roughly stable.

Now set KD_STEER = 0.0 and tune LOOKAHEAD (start at 6). The steering plot goes smooth. The car holds the lane just as well, and the oscillation is gone.

Lookahead provides damping without touching the derivative. By steering toward a point further along the path, small momentary errors produce smaller corrections, and the car has more time to settle before the next correction fires. It never divides by dt, so it never amplifies noise.

The code still has the derivative calculation in steering_pid and KD_STEER is set to 0.0. That is intentional. The structure is there if you want to experiment with it.

For steering against a sampled path, lookahead is the right damping tool. KD is not.

Suggested experiments

Experiment	What to expect
`KD_STEER = 0.40`	Steering plot gets noisy/jagged on straights
`KP_STEER = 1.5`	Oscillates around lane center
`KP_STEER = 0.05`	Car cannot track corners, drifts wide
`LOOKAHEAD = 1`	Nervous oscillation, especially on corners
`LOOKAHEAD = 15`	Cuts corners, lookahead points past the apex
`KI_STEER = 0.05`	Integral windup, unstable steering

Each experiment should be visible in the steering plot. The point is to build intuition for what each term is doing mechanically, not just conceptually.

Tuning Order

Always tune speed first, steering second. The steering loop sees the car’s position, which depends on how fast the car is moving. If speed is wrong, the steering will feel wrong too, even with good gains.

Speed

Fix steer_cmd = 0.0 in the controller so the car drives straight. This isolates the speed loop.
Set KI_SPEED = 0 and KD_SPEED = 0. Raise KP_SPEED until the car reaches target speed. If it overshoots and oscillates, back off.
Add KI_SPEED (start around 0.02) if the car settles below target speed.
Keep KD_SPEED very small or zero. Speed derivative is noisy. A 1 m/s change per frame gives a derivative of 60 at 60 Hz, which swamps the throttle command.

Steering

Restore normal steering. Set KD_STEER = 0 and KI_STEER = 0.
Start with KP_STEER = 0.5. The car should steer toward the lane center. If it oscillates, lower KP_STEER. If it cannot track corners and drifts wide, raise it.
If the car oscillates on straights, increase LOOKAHEAD, not KD_STEER. Lookahead is your damping tool here.
Add KI_STEER (around 0.0005) only if the car has a persistent lane offset after corners.
Leave KD_STEER = 0.0. On a raw sampled path it amplifies discretization noise, not the real signal.