LQR

LQR, the linear-quadratic regulator, is one of the four steering laws in pure_pursuit_node. It describes how far the car is off the racing line with two numbers, writes down how those numbers change from one tick to the next, and computes the steering rule that brings them to zero at the least total cost. The result is a fixed linear rule: the steering the bend needs, minus a gain times each error.

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. LQR has been driven only in the simulator. The code is LqrGainTable and LqrLaw in tracking_laws.pyGitHub, and the excerpts below are from it with docstrings and input checks removed.

Topics

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

The state

LQR works on the rear axle, the point the racing line was recorded at. Its state is the pair of errors

• $e$ is the cross-track error, the car’s sideways distance from the line in metres, positive when the car is to the left of the line.
• $\theta =\psi -{\psi }_p$ is the heading error, the car’s heading minus the line’s, in radians. Positive means the car points further left than the line. This is the opposite sign from Stanley’s ${\theta }_e$, which is line minus car.

Both come from projecting the rear axle onto the nearest segment of the line, as described on the Stanley page.

The model

How the errors change. The car moves at speed $v$ in the direction of its heading. If its heading is turned $\theta$ from the line’s, its sideways speed relative to the line is $v\sin \theta$, and for a small angle $\sin \theta \approx \theta$:

The heading error changes at the car’s turning rate minus the line’s. By the bicycle model, the car turns at $v\tan \delta /L$. The line turns at $v\kappa$, where $\kappa$ is its curvature, because a point moving along a curve of curvature $\kappa$ at speed $v$ turns at $v\kappa$. So

${\delta }_{ff}$, the feedforward, is the steering that follows the bend exactly: with $\delta ={\delta }_{ff}$, $\tan \delta =L\kappa$ and the heading error stops changing. The approximation replaces $\tan \delta$ by $\delta$ near that point, which is accurate for the small corrections LQR makes around it.

One tick at a time. The controller runs every $\Delta t$ = 0.025 s. Over one tick, each error changes by its rate times $\Delta t$ (Euler’s method):

In matrix form, with the correction $u_k$ as the input:

def _ab(self, v: float):
    v = max(v, self.V_FLOOR)
    A = np.array([[1.0, v * self.dt], [0.0, 1.0]])
    B = np.array([[0.0], [v * self.dt / self.wheelbase]])
    return A, B

At $v=0$ the input has no effect ($B=0$): a car that is not moving cannot steer its way back to the line. The code therefore never uses a speed below 0.1 m/s (V_FLOOR).

The cost

LQR chooses the corrections that make this total as small as possible, over all future ticks:

with $Q=\mathrm{diag}(q_e,q_{\theta })$. The first two terms charge for being off the line. The last charges for steering away from the feedforward. Squaring makes every term positive and makes large errors much more expensive than small ones. The weights set the trade-off: raising $q_e$ corrects sideways error harder, and raising $r$ makes the car steer more gently and accept more error. Only their ratios matter, and all three default to 1.0.

The gain

For a model of this form and a cost of this form, the best correction at every tick is a fixed linear function of the state:

The gain $K$ comes from a matrix $P$ that measures the cost still to come from any state: from state $\mathbf{x}$, the least possible remaining cost is ${\mathbf{x}}^{\top }P\mathbf{x}$. Taking one more step cannot change that answer for an infinite sum, so $P$ must satisfy the discrete algebraic Riccati equation:

and the gain follows from it:

The equation has no simple closed form for $P$. scipy.linalg.solve_discrete_are solves it numerically. Repeating the right-hand side from $P=Q$ until it stops changing reaches the same answer, which is a useful way to check it by hand.

Gain scheduling

$A$ and $B$ depend on the speed, so the best $K$ does too. At startup, the code solves the Riccati equation on a grid of speeds from 0.1 m/s to lqr.v_grid_max (6.0 m/s), every 0.05 m/s: 119 speeds. Each tick it interpolates linearly between the two nearest grid speeds.

speeds = np.arange(self.V_FLOOR, v_grid_max + 1e-9, self.V_STEP)
for i, v in enumerate(speeds):
    A, B = self._ab(v)
    P = scipy.linalg.solve_discrete_are(A, B, self.Q, self.R)
    K = np.linalg.solve(self.R + B.T @ P @ B, B.T @ P @ A)
    gains[i] = K[0]
    riccati[i] = P

np.linalg.solve(M, N) computes $M^{-1}N$ without forming the inverse. The table also keeps $P$ at each speed, which MPC reuses as its terminal cost.

Solving the same equations with the default weights and $\Delta t$ = 0.025 s gives these gains. They fall slowly with speed, because at higher speed the same steering corrects the error faster.

At 2 m/s, a car 0.10 m left of a straight line and parallel to it steers $-0.913\times 0.10=-0.091$ rad, 5.2° to the right.

The law

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)
    proj = racing_math.project_onto_path(
        path.xy, path.seg_len, (inp.x, inp.y), inp.nearest_idx, path.closed)
    e = proj.lateral_error
    theta = _wrap(inp.yaw - proj.heading)
    kappa = racing_math.interpolate_along(path.curvature, proj, path.closed)
    K = self._table.gain_at(max(inp.speed, LqrGainTable.V_FLOOR))
    feedforward = math.atan(L * kappa)
    unclipped = feedforward - float(K[0]) * e - float(K[1]) * theta
    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))
    ...

Like Stanley, LQR corrects the error at the car, so it first predicts the pose lqr.delay_compensation (0.08 s) ahead and then smooths its output with a time constant of 0.05 s. Both are explained on the Stanley page.

Parameters

Set in pure_pursuit.yamlGitHub. They are read at launch only; edit the YAML and relaunch.

Every weight must be positive; the node refuses to start otherwise. A new value has to be written in three places together: LqrParams in tracking_laws.py, pure_pursuit.yaml, and the declare_parameter default in pure_pursuit_node.py.

Run it

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

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:

lqr e=+0.021m th=-0.010rad K=[0.91,1.24] ff=+0.120rad

e and th are the two errors, K is the gain at the current speed, and ff is the feedforward. In a steady bend, ff carries most of the steering and e and th stay small. A large e that does not shrink means the feedback is too weak for the conditions.

Tune it

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

Raising lqr.q_lateral relative to lqr.r_steer tightens tracking at the cost of more steering activity. In the October 19, 2026 comparison, LQR passed 15 of 15 runs with a mean cross-track error of 0.0714 m (3.5% worse than pure pursuit) and a mean lap of 145.2 s (1.6% faster). See the comparison of the four laws.

Tests

test_tracking_laws.pyGitHub checks the gain table against a direct Riccati iteration, like the one described above, and checks that the closed loop $A-BK$ is stable at every grid speed: every eigenvalue inside the unit circle, so every error shrinks.

Troubleshooting

Limitations

• LQR has been driven only in the simulator.
• The model is the kinematic bicycle, linearized around the line. It has no tyre slip, so near the grip limit the real car turns less than the model expects.
• The gains are optimal for the model, not for the car: delay and noise are handled by the prediction and the smoothing, which are tuned separately.