Stanley

Stanley is one of the four steering laws in pure_pursuit_node. It was the steering law of Stanford’s car that won the 2005 DARPA Grand Challenge (Hoffmann et al., 2007). Where pure pursuit aims at a point ahead, Stanley looks at the front axle: how far it is to the side of the racing line, and how far the car’s heading is turned from the line’s. It steers to remove both at once.

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, so a poorly tuned law can track the line worse or lap slower but cannot switch off a safety layer. Stanley has been driven only in the simulator. The code is StanleyLaw in tracking_laws.pyGitHub, and the excerpts below are from it with docstrings and input checks removed.

Topics

Stanley runs inside pure_pursuit_node, so its topics are that node’s: see Pure pursuit for the full tables. Of those inputs, Stanley uses the pose, the measured speed from /odom, and the racing line’s points and curvature.

The two errors

Stanley, LQR and MPC all work from two errors measured against the racing line. The line is a list of points, so the node finds the segment nearest the car and drops a perpendicular onto it. The foot of that perpendicular is the projection $q$, and the segment’s direction is the line’s heading ${\psi }_p$ there.

• Cross-track error $e$: the car’s sideways distance from the line, in metres, positive when the car is to the left of the line’s direction of travel.
• Heading error ${\theta }_e$: the angle from the car’s heading $\psi$ to the line’s heading, ${\theta }_e={\psi }_p-\psi$, wrapped into $(-\pi ,\pi ]$. Positive means the line points further left than the car does.

The signed cross-track error comes from rotating the offset from $q$ to the car into the line’s frame and keeping its sideways part:

This is the same rotation pure pursuit uses to bring a target into the car’s frame, applied here to the line’s frame. Stanley measures both errors at the front axle, which is $L$ = 0.36 m ahead of the rear axle along the car’s heading:

The law

The first term turns the wheels parallel to the line. The second turns them further, toward the line, by an angle that grows with the sideways error. The minus sign is there because $e_f$ is positive to the left: a car left of the line steers right.

Why the correction is an arctangent

Leave the heading term aside, so ${\theta }_e=0$ and the car is parallel to the line. The front wheels then point at the angle $\beta =-\arctan (k{e}_f/(v+v_s))$ from the line, and the front axle moves at speed $v$ in the direction the wheels point. Its sideways speed toward the line is the sideways part of that velocity:

For a small error the arctangent and the sine are both close to their argument, so

The error’s rate of change is proportional to the error itself, which is the equation of exponential decay. At race speed, where $v\gg v_s$, the error halves every $\ln 2/k=0.14$ s whatever the speed. The gain $k$ has units of s⁻¹ for this reason: it is a decay rate.

For a large error, $\sin (\arctan z)=z/\sqrt{1+z^2}$ approaches 1, so ${\dot{e}}_f$ approaches $-v$: the wheels turn toward the line at up to 90° and the car drives straight at it. The steering limit clips that long before 90°.

Why the softening term

Without $v_s$, the fraction $k{e}_f/v$ would divide by zero at a standstill, and at a crawl a 5 cm error would ask for full lock. Adding $v_s$ = 1.0 m/s to the denominator keeps the correction finite. Starting from rest with $e_f$ = 0.1 m, the correction is $\arctan (5\times 0.1/1.0)=0.46$ rad, which the steering limit then clips to 0.33 rad on car 1.

Where the front axle should be in a bend

Every racing line in the workspace is recorded at the rear axle. If the rear axle follows a circle of radius $R$, the front axle is $L$ further along the tangent, so it is at distance $\sqrt{R^2+L^2}$ from the centre, outside the circle by

In a 1 m radius corner that is 0.063 m. Stanley steers the front axle to this offset, outside the bend, instead of to zero. A car with its rear axle exactly on the circle then gets exactly $\arctan (L/R)$, the steering that keeps it there. Steering the front axle to zero instead would make the rear axle cut $L^2/(2R)$ inside every bend.

@staticmethod
def front_axle_offset(kappa: float, wheelbase: float) -> float:
    if abs(kappa) < 1e-9:
        return 0.0
    radius = 1.0 / abs(kappa)
    return -math.copysign(math.hypot(radius, wheelbase) - radius, kappa)

The sign is negative for a left turn (positive $\kappa$), because outside a left bend is to the right.

No separate curvature term

The textbook form of Stanley often adds $\arctan (L\kappa )$, the steering the bend needs. The workspace’s version leaves it out. The heading error is measured at the front axle’s point on the line, where the line has already turned further than at the rear axle, so ${\theta }_e$ already contains that steering. Adding the term counted it twice. At 3 m/s in a 1 m radius corner, the doubled term forced the front axle about 0.6 m to the inside, and in the simulator the car hit the wall on every Silverstone run.

The code

def steer(self, inp: TrackingInput, path: PathContext) -> SteeringResult:
    L = self.limits.wheelbase
    previous = inp.previous_steering
    inp = predict_input(inp, path, self.params.delay_compensation, L)
    xf = inp.x + L * math.cos(inp.yaw)
    yf = inp.y + L * math.sin(inp.yaw)
    walked = racing_math.find_lookahead_index(
        path.seg_len, inp.nearest_idx, L, closed=path.closed)
    nearest_f, _ = racing_math.find_nearest_index(
        path.xy, (xf, yf), closed=path.closed,
        prev_index=walked, search_window=4)
    proj = racing_math.project_onto_path(
        path.xy, path.seg_len, (xf, yf), nearest_f, path.closed)
    e_f = proj.lateral_error
    theta_e = _wrap(proj.heading - inp.yaw)
    kappa_f = racing_math.interpolate_along(path.curvature, proj, path.closed)
    e_target = self.front_axle_offset(kappa_f, L)
    v = max(inp.speed, 0.0)

    unclipped = (theta_e
                 - math.atan(self.params.gain * (e_f - e_target)
                             / (v + self.params.softening)))
    limit = self.limits.max_steering_angle
    steering = float(np.clip(smooth_steering(
        float(np.clip(unclipped, -limit, limit)), previous,
        self.params.steering_time_constant, self.limits.control_dt), -limit, limit))
    ...

To find the front axle’s nearest point, the code walks one wheelbase forward from the rear axle’s nearest point, then searches four points either side. On a straight the walk lands exactly. On a curve it can be a segment off, and the small search corrects it.

Delay compensation

A command does not take effect when it is computed. The pose is a scan or two old, the command waits a tick, and the servo takes about 40 ms to move: about 0.1 s in all, in the simulator’s model of the car. Pure pursuit hides the delay by aiming a metre or more ahead. Stanley corrects the error at the car, so without help it always corrects an error the car has already moved past, and at race speed that becomes a weave.

With stanley.delay_compensation set to $d$ (0.10 s), the law first predicts where the car will be $d$ seconds ahead if it holds its speed and steering, and steers from there. Holding the steering means driving an arc of constant curvature $\kappa =\tan \delta /L$. Along an arc of length $s$, the heading changes at $d\psi /ds=\kappa$, and the position changes at $dx/ds=\cos \psi$, $dy/ds=\sin \psi$. Integrating from heading ${\psi }_0$ to $\psi ={\psi }_0+\kappa s$:

with $s=vd$. When $\kappa$ is near zero the car drives straight instead: $x=x_0+s\cos {\psi }_0$, $y=y_0+s\sin {\psi }_0$.

def predict_input(inp, path, delay, wheelbase):
    if delay <= 0.0:
        return inp
    distance = max(inp.speed, 0.0) * delay
    if distance <= 0.0:
        return inp
    curvature = math.tan(inp.previous_steering) / wheelbase
    yaw0 = inp.yaw
    if abs(curvature) < 1e-9:
        x = inp.x + distance * math.cos(yaw0)
        y = inp.y + distance * math.sin(yaw0)
        yaw = yaw0
    else:
        yaw = yaw0 + curvature * distance
        x = inp.x + (math.sin(yaw) - math.sin(yaw0)) / curvature
        y = inp.y - (math.cos(yaw) - math.cos(yaw0)) / curvature
    ...

The same function serves LQR and MPC, with their own delay values.

Smoothing

Stanley reacts to the measured error immediately, so noise in the pose reaches the steering at full strength. With the simulator’s 2 cm of pose noise, Stanley at gain 3 and 1 m/s jittered about 0.04 rad a tick, three times as much as pure pursuit. The jitter is not free: the speed cap reads it as curvature and slows the car, by up to 10 s a lap before smoothing was added.

The output therefore passes through a first-order low-pass filter. Each tick, the command moves a fraction $a$ of the way from the last command toward the law’s answer:

with $\Delta t$ = 0.025 s, the control period, and $T$ = stanley.steering_time_constant, 0.05 s. That gives $a=1/3$: a third of the way each tick. A small $T$ removes tick-to-tick jitter, and the lag it adds is part of what the delay compensation makes up for.

def smooth_steering(target, previous, time_constant, dt):
    if time_constant <= 0.0:
        return target
    alpha = dt / (time_constant + dt)
    return previous + alpha * (target - previous)

Parameters

Set in pure_pursuit.yamlGitHub. The law and its parameters are read at launch only. A change from the dashboard is refused, so edit the YAML and relaunch.

A new value has to be written in three places together: StanleyParams in tracking_laws.py, pure_pursuit.yaml, and the declare_parameter default in pure_pursuit_node.py. Two tests fail if they disagree.

Run it

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

The startup log prints the law and the values it loaded, for example steering law: stanley StanleyParams(gain=5.0, ...). An unknown law name stops the node at startup rather than falling back to another law.

The first time it runs on a car: wheels off the ground, then the floor at about 1 m/s (max_speed, which can be changed live), then race speed.

Read the decision log

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

stanley e_f=+0.012m (target -0.063m) th_e=+0.004rad

e_f is the front axle’s cross-track error, target is $e_f^{\ast }$, the offset it is being steered to, and th_e is the heading error. On a straight, target reads 0.000 and e_f should stay within a few centimetres. In a bend, e_f should settle near target, not near zero.

Tune it

Tuning runs in the simulator with no ROS:

python3 tools/f1tenth_sim/law_sweep.py --laws stanley --seed 1000 --repeat-seeds 3 \
    --law-param stanley.gain=4.0 --law-param stanley.delay_compensation=0.12

It prints a row per circuit, then PROMOTE or REJECT with reasons. Any run below 3/3 completed laps, or with wall contact, always rejects. More avoid steps with a slower lap usually means jitter: raise delay_compensation or steering_time_constant. A worse cross-track error means the gain is too low.

In the October 19, 2026 comparison, three circuits with five seeds each, Stanley passed 15 of 15 runs with a mean cross-track error of 0.0679 m (1.6% better than pure pursuit) and a mean lap of 151.7 s (2.8% slower). See the comparison of the four laws.

Troubleshooting

Limitations

• Stanley has been driven only in the simulator.
• It corrects the error at the car, so it depends on the delay compensation being close to the real delay. The values were tuned against the simulator’s model of about 0.1 s, not against the car.
• It is more sensitive to pose noise than pure pursuit, and relies on the smoothing filter to keep that noise out of the speed cap.