Control Theory (PID / LQR / MPC)

$\delta = \theta_e + \arctan\!\left(\dfrac{k\,e}{v}\right)$

The first term aligns the heading with the path's tangent; the second steers proportionally to the cross-track error $e$, softened by speed $v$. A bigger gain $k$ corrects harder but risks oscillation, and the term jumps sharply as $v \to 0$.

$\kappa = \dfrac{2\sin\alpha}{L_d}$

Fit a circular arc from the vehicle to a lookahead point a distance $L_d$ ahead on the path; $\alpha$ is the angle between the vehicle's heading and the line to that point. For a bicycle model, the steering angle is $\delta = \arctan\!\left(\dfrac{2L\sin\alpha}{L_d}\right)$.

A larger $L_d$ smooths the path but cuts corners; a smaller $L_d$ tracks tighter but can oscillate, especially at speed.

$u(t) = K_p e(t) + K_i \displaystyle\int_0^t e(t')\,dt' + K_d \dfrac{de(t)}{dt}$

P reacts to current error, I eliminates steady-state error by accumulating it over time, D damps overshoot by reacting to the error's rate of change.

If the actuator saturates, the integral term keeps accumulating error it can't act on, then overshoots badly once the actuator unsaturates.

Anti-windup clamps or back-calculates the integral term so it stops growing while the actuator is saturated.

$\dot x = Ax + Bu$

The system is stable (state decays to zero with no input) exactly when every eigenvalue of $A$ has negative real part. Controllability asks whether some input can drive the state anywhere; observability asks whether the state can be reconstructed from the outputs alone.

Minimize the quadratic cost $J = \displaystyle\int_0^\infty \big(x^\top Q x + u^\top R u\big)\,dt$

subject to $\dot x = Ax+Bu$. The optimal control is linear state feedback $u = -Kx$, with $K = R^{-1}B^\top P$ where $P$ solves the algebraic Riccati equation. $Q$ penalizes state error, $R$ penalizes control effort — their ratio sets how aggressive vs. gentle the resulting controller is.

At each step, solve a finite-horizon optimization for the best sequence of future controls given the current state, apply only the first control, then re-solve at the next step with updated measurements.

Unlike PID or LQR, MPC can directly enforce constraints (joint limits, obstacle avoidance, actuator saturation) inside the optimization — at the cost of solving an optimization problem online, every control cycle.