In Chapter 15, we assume a global planner has already produced a collision-free reference path (a sequence of waypoints or a smooth curve). The local motion generation block must produce feasible controls that keep the vehicle close to the path under nonholonomic constraints. The Stanley controller outputs a steering angle by combining: \( e_\psi \): heading error to the path tangent, and \( e_y \): signed cross-track error (lateral displacement).

The key idea is: (i) point the vehicle toward the path (heading correction), and (ii) add an additional steering component proportional to lateral error, normalized by speed so the controller does not become overly aggressive at high velocity.

flowchart TD
  A["Inputs: reference path + current state (x,y,psi,v)"] --> B["Nearest point on path (projection)"]
  B --> C["Compute errors: e_y (signed), e_psi = psi_ref - psi"]
  C --> D["Compute steering: delta = delta_ff + e_psi + atan(k*e_y/(v+v0))"]
  D --> E["Saturate delta to +/- delta_max; apply rate limits if needed"]
  E --> F["Vehicle update (bicycle kinematics)"]
  F --> A
        

For low-to-moderate speeds on high-friction surfaces, a widely used control-oriented model is the kinematic bicycle model with wheelbase \( L \): and steering angle \( \delta \):

\[ \dot x = v \cos\psi,\quad \dot y = v \sin\psi,\quad \dot\psi = \frac{v}{L}\tan\delta \]

The reference path is represented as a polyline \( \{(x_i^{ref},y_i^{ref})\}_{i=1}^N \): or a smooth curve parameterized by arc-length \( s \): with tangent heading \( \psi_{ref}(s) \): and curvature \( \kappa_{ref}(s) \):

At each control cycle, we find the closest point on the path (projection). Let that point be \( \mathbf{p}_{ref} = (x_{ref},y_{ref}) \): and let the unit tangent be \( \mathbf{t} = (\cos\psi_{ref},\sin\psi_{ref}) \):

We define the heading error (note the sign convention is important for stability): \( e_\psi \):

\[ e_\psi \triangleq \mathrm{wrap}(\psi_{ref} - \psi) \in (-\pi,\pi] \]

We define the signed cross-track error using the 2D cross product (scalar) between the path tangent and the vector from the closest path point to the vehicle. Let \( \mathbf{v} = (x-x_{ref},\; y-y_{ref}) \): then compute \( c \):

\[ c \triangleq t_x v_y - t_y v_x \]

The sign of \( c \): indicates whether the vehicle is to the left or right of the path direction. The signed lateral error is

\[ e_y \triangleq \mathrm{sign}(c)\,\|\mathbf{v}\|_2 \]

These definitions are coordinate-free and work for any path segment (straight or curved) as long as the tangent is well-defined.

flowchart TD
  N["Nearest point n=(x_ref,y_ref)"] --> V["v = p - n"]
  T["Tangent t=(cos(psi_ref), sin(psi_ref))"] --> C["c = t_x*v_y - t_y*v_x"]
  V --> C
  C --> S["sign(e_y)=sign(c)"]
  S --> EY["e_y = sign * ||v||"]
        

The Stanley controller (popularized by the Stanford DARPA Grand Challenge vehicle) uses a sum of a heading correction and a nonlinear cross-track term. A practical AMR version also adds a curvature feedforward term (when the reference provides curvature).

Let \( k > 0 \): be the cross-track gain and \( v_0 > 0 \): a low-speed softening constant. The steering command is:

\[ \delta = \delta_{ff} + e_\psi + \arctan\!\left(\frac{k\,e_y}{v+v_0}\right) \]

The feedforward term tracks the curvature of the reference path:

\[ \kappa = \frac{\tan\delta}{L} \quad\Rightarrow\quad \delta_{ff} = \arctan(L\,\kappa_{ref}) \]

The term \( \arctan\left(\frac{k e_y}{v+v_0}\right) \): behaves like a proportional action for small errors but saturates to \( \pm \frac{\pi}{2} \): for large \( |e_y| \): which improves robustness when the vehicle starts far from the path. The division by \( v+v_0 \): is essential; otherwise the same lateral error would cause excessive steering at high speed.

To establish a rigorous baseline, consider a straight reference path along the x-axis with \( \psi_{ref}=0 \): and \( \kappa_{ref}=0 \): so \( \delta_{ff}=0 \): and the nearest-point projection yields approximately \( e_y \approx y \): for small deviation.

With our sign convention \( e_\psi = \psi_{ref} - \psi = -\psi \): we get:

\[ \dot e_y = \dot y = v\sin\psi = -v\sin e_\psi, \qquad \dot e_\psi = -\dot\psi = -\frac{v}{L}\tan\delta \]

For small errors (local analysis), use approximations \( \sin e_\psi \approx e_\psi \): and \( \tan\delta \approx \delta \): and linearize the Stanley law:

\[ \delta = e_\psi + \arctan\!\left(\frac{k e_y}{v+v_0}\right) \approx e_\psi + \frac{k}{v+v_0}e_y \]

The linearized closed-loop error dynamics become \( \dot{\mathbf{e} } = \mathbf{A}\mathbf{e} \): for \( \mathbf{e}=[e_y,\; e_\psi]^T \):

\[ \begin{aligned} \dot e_y &\approx -v\,e_\psi,\\ \dot e_\psi &\approx -\frac{v}{L}\,e_\psi - \frac{v}{L}\,\frac{k}{v+v_0}\,e_y \end{aligned} \]

Therefore \( \mathbf{A} = \begin{bmatrix} 0 & -v \\ -\frac{v k}{L(v+v_0)} & -\frac{v}{L} \end{bmatrix} \): has trace \( \mathrm{tr}(\mathbf{A}) = -\frac{v}{L} < 0 \): and determinant \( \det(\mathbf{A}) = \frac{v^2 k}{L(v+v_0)} > 0 \): for \( v>0 \): and \( k>0 \): By the 2nd-order Routh–Hurwitz criterion, both eigenvalues have negative real part, so the equilibrium \( (e_y,e_\psi)=(0,0) \): is locally exponentially stable.

Interpretation. The heading error term provides damping (negative diagonal term in \( \mathbf{A} \):), while the cross-track term couples \( e_y \): into the heading dynamics to steer back toward the path. Increasing \( k \): increases the coupling and can reduce lateral error, but too large \( k \): can cause oscillation once actuator limits and discrete-time sampling are considered.

In practice, your local controller sits below the local planner (Chapter 15 Lessons 3–4) and consumes a short horizon of the global path, usually expressed in the robot base frame or the map frame. A standard loop is:

• Transform the path into the controller frame (typically \( \{x,y\} \): in the map frame; yaw from localization).
• Find the closest point on the path and estimate its tangent heading \( \psi_{ref} \): (polyline segment direction).
• Compute \( e_y \): and \( e_\psi \): with the sign conventions in Section 2.
• Compute \( \delta \): using the Stanley law, apply steering saturation and (optionally) rate limits.
• Command speed \( v \): from a velocity profile; the controller is most reliable when \( v \): does not drop near zero (use \( v_0 \):).

Saturation and rate limits. Real steering systems have bounds and dynamics. A common refinement is to pass the computed steering through a first-order actuator model \( \dot\delta_{act} = \frac{1}{\tau}(\delta - \delta_{act}) \): and enforce \( |\delta_{act}| \le \delta_{max} \): and \( |\dot\delta_{act}| \le \dot\delta_{max} \): to avoid unrealistic oscillations.

Nearest-point computation. For short local horizons, brute force scanning is often acceptable. For long paths, use a spatial index (k-d tree) or incremental search starting from the last nearest index.

This implementation uses NumPy for geometry and Matplotlib for plots. It demonstrates: (i) projection to the nearest polyline segment, (ii) signed cross-track error via a 2D cross product, and (iii) curvature feedforward (optional).

Chapter15_Lesson2.py

"""
Chapter15_Lesson2.py
Stanley Controller for Ground Vehicles (kinematic bicycle model)
- Path: polyline (x_ref, y_ref)
- Control: delta = delta_ff + e_psi + atan2(k * e_y, v + v0)
Author: Abolfazl Mohammadijoo (educational example)
"""

from __future__ import annotations
import math
from dataclasses import dataclass
from typing import Tuple

import numpy as np
import matplotlib.pyplot as plt


def wrap_angle(a: float) -> float:
    """Wrap angle to (-pi, pi]."""
    return (a + math.pi) % (2.0 * math.pi) - math.pi


@dataclass
class State:
    x: float
    y: float
    psi: float  # yaw (rad)
    v: float    # forward speed (m/s)


@dataclass
class StanleyParams:
    L: float = 2.7                 # wheelbase (m)
    k: float = 1.2                 # cross-track gain
    v0: float = 0.5                # softening speed (m/s) to avoid divide-by-zero
    max_steer: float = math.radians(30.0)  # steering saturation (rad)


def _nearest_point_on_polyline(px: float, py: float, x_ref: np.ndarray, y_ref: np.ndarray) -> Tuple[int, float, float, float]:
    """
    Return:
      idx: segment index i such that nearest point lies on segment i->i+1 (or endpoint)
      nx, ny: nearest point coordinates
      t: projection parameter in [0,1] on that segment
    """
    x1 = x_ref[:-1]
    y1 = y_ref[:-1]
    x2 = x_ref[1:]
    y2 = y_ref[1:]
    dx = x2 - x1
    dy = y2 - y1
    denom = dx*dx + dy*dy
    denom = np.where(denom < 1e-12, 1e-12, denom)

    t = ((px - x1)*dx + (py - y1)*dy) / denom
    t = np.clip(t, 0.0, 1.0)

    projx = x1 + t*dx
    projy = y1 + t*dy
    d2 = (px - projx)**2 + (py - projy)**2

    i = int(np.argmin(d2))
    return i, float(projx[i]), float(projy[i]), float(t[i])


def _segment_heading(i: int, x_ref: np.ndarray, y_ref: np.ndarray) -> float:
    dx = float(x_ref[i+1] - x_ref[i])
    dy = float(y_ref[i+1] - y_ref[i])
    return math.atan2(dy, dx)


def _curvature_at_index(i: int, x_ref: np.ndarray, y_ref: np.ndarray) -> float:
    """
    Discrete curvature estimate around point i using finite differences on indices.
    Robust enough for educational demos; for production, compute curvature vs arc-length.
    """
    n = len(x_ref)
    i0 = max(0, i-1)
    i1 = i
    i2 = min(n-1, i+1)
    x0, y0 = x_ref[i0], y_ref[i0]
    x1, y1 = x_ref[i1], y_ref[i1]
    x2, y2 = x_ref[i2], y_ref[i2]

    # Approx derivatives w.r.t index (not arc length)
    x_p = (x2 - x0) * 0.5
    y_p = (y2 - y0) * 0.5
    x_pp = x2 - 2.0*x1 + x0
    y_pp = y2 - 2.0*y1 + y0

    denom = (x_p*x_p + y_p*y_p)**1.5
    if denom < 1e-9:
        return 0.0
    kappa = (x_p*y_pp - y_p*x_pp) / denom
    return float(kappa)


def stanley_control(state: State, x_ref: np.ndarray, y_ref: np.ndarray, params: StanleyParams) -> Tuple[float, dict]:
    """
    Compute Stanley steering command.
    Convention:
      e_psi = psi_ref - psi   (desired minus actual)
      e_y   = signed cross-track error (positive if vehicle is left of path tangent)
    """
    i, nx, ny, _ = _nearest_point_on_polyline(state.x, state.y, x_ref, y_ref)
    psi_ref = _segment_heading(i, x_ref, y_ref)
    e_psi = wrap_angle(psi_ref - state.psi)

    # Signed lateral error via 2D cross product between tangent and (vehicle - nearest)
    tx = math.cos(psi_ref)
    ty = math.sin(psi_ref)
    vx = state.x - nx
    vy = state.y - ny
    cross = tx*vy - ty*vx  # >0 => vehicle left of tangent
    e_y = math.copysign(math.hypot(vx, vy), cross)

    # Optional feedforward from reference curvature
    kappa_ref = _curvature_at_index(i, x_ref, y_ref)
    delta_ff = math.atan(params.L * kappa_ref)

    # Stanley feedback term
    delta_fb = e_psi + math.atan2(params.k * e_y, state.v + params.v0)

    delta = delta_ff + delta_fb
    delta = max(-params.max_steer, min(params.max_steer, delta))

    info = {
        "i": i,
        "nx": nx,
        "ny": ny,
        "psi_ref": psi_ref,
        "e_psi": e_psi,
        "e_y": e_y,
        "kappa_ref": kappa_ref,
        "delta_ff": delta_ff,
        "delta_fb": delta_fb,
    }
    return delta, info


def step_bicycle(state: State, delta: float, dt: float, L: float) -> State:
    x = state.x + state.v * math.cos(state.psi) * dt
    y = state.y + state.v * math.sin(state.psi) * dt
    psi = wrap_angle(state.psi + state.v / L * math.tan(delta) * dt)
    return State(x=x, y=y, psi=psi, v=state.v)


def make_s_path(n: int = 400) -> Tuple[np.ndarray, np.ndarray]:
    # A simple smooth polyline path
    x = np.linspace(0.0, 50.0, n)
    y = 2.5 * np.sin(0.18 * x) + 1.0 * np.sin(0.04 * x)
    return x, y


def main() -> None:
    x_ref, y_ref = make_s_path()

    params = StanleyParams(L=2.7, k=1.4, v0=0.5, max_steer=math.radians(32))
    dt = 0.02
    T = 25.0
    steps = int(T / dt)

    state = State(x=-2.0, y=3.5, psi=math.radians(-10), v=6.0)

    xs, ys, psis, deltas = [], [], [], []
    eys, epsis = [], []

    for _ in range(steps):
        delta, info = stanley_control(state, x_ref, y_ref, params)
        state = step_bicycle(state, delta, dt, params.L)

        xs.append(state.x)
        ys.append(state.y)
        psis.append(state.psi)
        deltas.append(delta)
        eys.append(info["e_y"])
        epsis.append(info["e_psi"])

    # Plots
    plt.figure(figsize=(9, 5))
    plt.plot(x_ref, y_ref, label="reference path")
    plt.plot(xs, ys, label="vehicle trajectory")
    plt.axis("equal")
    plt.grid(True)
    plt.legend()
    plt.title("Stanley Controller: path tracking (kinematic bicycle)")
    plt.xlabel("x [m]")
    plt.ylabel("y [m]")

    t = np.linspace(0.0, T, steps)
    plt.figure(figsize=(9, 4))
    plt.plot(t, eys, label="e_y [m]")
    plt.plot(t, np.degrees(epsis), label="e_psi [deg]")
    plt.grid(True)
    plt.legend()
    plt.title("Tracking errors")
    plt.xlabel("time [s]")

    plt.figure(figsize=(9, 4))
    plt.plot(t, np.degrees(deltas), label="delta [deg]")
    plt.grid(True)
    plt.legend()
    plt.title("Steering command")
    plt.xlabel("time [s]")

    plt.show()


if __name__ == "__main__":
    main()
      

Chapter15_Lesson2_Ex1.py

"""
Chapter15_Lesson2_Ex1.py
Exercise: Gain sweep for Stanley controller (RMS cross-track error vs gain k)
This script reuses a simplified Stanley implementation to sweep k and plot RMS(e_y).

Author: Abolfazl Mohammadijoo (educational example)
"""

from __future__ import annotations
import math
import numpy as np
import matplotlib.pyplot as plt


def wrap_angle(a: float) -> float:
    return (a + math.pi) % (2.0 * math.pi) - math.pi


def make_s_path(n: int = 400):
    x = np.linspace(0.0, 50.0, n)
    y = 2.5 * np.sin(0.18 * x) + 1.0 * np.sin(0.04 * x)
    return x, y


def nearest_segment(px, py, x_ref, y_ref):
    x1 = x_ref[:-1]; y1 = y_ref[:-1]
    x2 = x_ref[1:];  y2 = y_ref[1:]
    dx = x2 - x1; dy = y2 - y1
    denom = dx*dx + dy*dy
    denom = np.where(denom < 1e-12, 1e-12, denom)
    t = ((px - x1)*dx + (py - y1)*dy) / denom
    t = np.clip(t, 0.0, 1.0)
    projx = x1 + t*dx
    projy = y1 + t*dy
    d2 = (px - projx)**2 + (py - projy)**2
    i = int(np.argmin(d2))
    return i, float(projx[i]), float(projy[i])


def segment_heading(i, x_ref, y_ref):
    return math.atan2(float(y_ref[i+1] - y_ref[i]), float(x_ref[i+1] - x_ref[i]))


def simulate(k_gain: float, v: float = 6.0, v0: float = 0.5, L: float = 2.7, dt: float = 0.02, T: float = 18.0):
    x_ref, y_ref = make_s_path()
    steps = int(T/dt)
    x, y, psi = -2.0, 3.5, math.radians(-10)
    eys = []
    for _ in range(steps):
        i, nx, ny = nearest_segment(x, y, x_ref, y_ref)
        psi_ref = segment_heading(i, x_ref, y_ref)
        e_psi = wrap_angle(psi_ref - psi)
        tx, ty = math.cos(psi_ref), math.sin(psi_ref)
        vx, vy = x - nx, y - ny
        cross = tx*vy - ty*vx
        e_y = math.copysign(math.hypot(vx, vy), cross)
        delta = e_psi + math.atan2(k_gain * e_y, v + v0)
        delta = max(-math.radians(32), min(math.radians(32), delta))
        # bicycle step
        x += v * math.cos(psi) * dt
        y += v * math.sin(psi) * dt
        psi = wrap_angle(psi + v/L * math.tan(delta) * dt)
        eys.append(e_y)
    eys = np.asarray(eys)
    rms = float(np.sqrt(np.mean(eys**2)))
    return rms


def main():
    ks = np.linspace(0.2, 4.0, 20)
    rms = [simulate(k) for k in ks]

    plt.figure(figsize=(8, 4))
    plt.plot(ks, rms, marker="o")
    plt.grid(True)
    plt.xlabel("k (cross-track gain)")
    plt.ylabel("RMS(e_y) [m]")
    plt.title("Stanley gain sweep (educational)")
    plt.show()

    best = ks[int(np.argmin(rms))]
    print("Best k in sweep:", best)


if __name__ == "__main__":
    main()
      

The following single-file C++ example runs a kinematic simulation and reports RMS cross-track error. In a robotics stack, the same \( stanley\_control \): function would be called inside a real-time control node (e.g., ROS2), and the steering angle would be published as an Ackermann drive command.

Chapter15_Lesson2.cpp

// Chapter15_Lesson2.cpp
// Stanley Controller for Ground Vehicles (kinematic bicycle model, educational)
//
// Build (example):
//   g++ -O2 -std=c++17 Chapter15_Lesson2.cpp -o stanley
//
// Notes:
// - For ROS2 integration, compute psi_ref and e_y from nav_msgs::msg::Path
//   and publish ackermann_msgs::msg::AckermannDriveStamped steering_angle.

#include <cmath>
#include <iostream>
#include <limits>
#include <vector>

static double wrap_angle(double a) {
    const double pi = 3.14159265358979323846;
    a = std::fmod(a + pi, 2.0 * pi);
    if (a < 0) a += 2.0 * pi;
    return a - pi;
}

struct State {
    double x{0.0};
    double y{0.0};
    double psi{0.0}; // yaw [rad]
    double v{0.0};   // speed [m/s]
};

struct Params {
    double L{2.7};
    double k{1.4};
    double v0{0.5};
    double max_steer{30.0 * M_PI / 180.0};
};

static void make_s_path(std::vector<double>& xr, std::vector<double>& yr, int n = 400) {
    xr.resize(n);
    yr.resize(n);
    const double x0 = 0.0, x1 = 50.0;
    for (int i = 0; i < n; ++i) {
        double x = x0 + (x1 - x0) * (static_cast<double>(i) / (n - 1));
        xr[i] = x;
        yr[i] = 2.5 * std::sin(0.18 * x) + 1.0 * std::sin(0.04 * x);
    }
}

static int nearest_point_index(double px, double py, const std::vector<double>& xr, const std::vector<double>& yr) {
    // Brute-force nearest vertex (simple). For production, project onto segments / k-d tree.
    double best = std::numeric_limits<double>::infinity();
    int best_i = 0;
    for (int i = 0; i < static_cast<int>(xr.size()); ++i) {
        double dx = px - xr[i];
        double dy = py - yr[i];
        double d2 = dx*dx + dy*dy;
        if (d2 < best) { best = d2; best_i = i; }
    }
    return best_i;
}

static double heading_from_path(int i, const std::vector<double>& xr, const std::vector<double>& yr) {
    int n = static_cast<int>(xr.size());
    int j = std::min(i + 1, n - 1);
    int k = std::max(i - 1, 0);
    double dx = xr[j] - xr[k];
    double dy = yr[j] - yr[k];
    return std::atan2(dy, dx);
}

static double stanley_control(const State& s,
                              const std::vector<double>& xr,
                              const std::vector<double>& yr,
                              const Params& p,
                              double& e_y_out,
                              double& e_psi_out) {

    int i = nearest_point_index(s.x, s.y, xr, yr);
    double psi_ref = heading_from_path(i, xr, yr);

    // Convention: e_psi = psi_ref - psi (desired minus actual)
    double e_psi = wrap_angle(psi_ref - s.psi);

    // Signed cross-track: positive if vehicle is left of the path tangent
    double tx = std::cos(psi_ref);
    double ty = std::sin(psi_ref);
    double vx = s.x - xr[i];
    double vy = s.y - yr[i];
    double cross = tx * vy - ty * vx;
    double e_y = std::copysign(std::hypot(vx, vy), cross);

    double delta = e_psi + std::atan2(p.k * e_y, s.v + p.v0);
    if (delta > p.max_steer) delta = p.max_steer;
    if (delta < -p.max_steer) delta = -p.max_steer;

    e_y_out = e_y;
    e_psi_out = e_psi;
    return delta;
}

static State step_bicycle(const State& s, double delta, double dt, double L) {
    State ns = s;
    ns.x += s.v * std::cos(s.psi) * dt;
    ns.y += s.v * std::sin(s.psi) * dt;
    ns.psi = wrap_angle(s.psi + s.v / L * std::tan(delta) * dt);
    return ns;
}

int main() {
    std::vector<double> xr, yr;
    make_s_path(xr, yr);

    Params p;
    const double dt = 0.02;
    const double T  = 25.0;
    const int steps = static_cast<int>(T / dt);

    State s;
    s.x = -2.0;
    s.y = 3.5;
    s.psi = -10.0 * M_PI / 180.0;
    s.v = 6.0;

    double sum_sq = 0.0;
    for (int k = 0; k < steps; ++k) {
        double e_y = 0.0, e_psi = 0.0;
        double delta = stanley_control(s, xr, yr, p, e_y, e_psi);
        s = step_bicycle(s, delta, dt, p.L);
        sum_sq += e_y * e_y;
    }

    double rms = std::sqrt(sum_sq / steps);
    std::cout << "Done. RMS cross-track error e_y: " << rms << " m\n";
    std::cout << "Final state: x=" << s.x << ", y=" << s.y
              << ", psi=" << s.psi << "\n";
    return 0;
}
      

Java is often used in educational simulators or embedded middleware. The code below mirrors the C++ structure and keeps dependencies minimal.

Chapter15_Lesson2.java

// Chapter15_Lesson2.java
// Stanley Controller for Ground Vehicles (kinematic bicycle model, educational)
//
// Compile:
//   javac Chapter15_Lesson2.java
// Run:
//   java Chapter15_Lesson2

import java.util.Arrays;

public class Chapter15_Lesson2 {

    static double wrapAngle(double a) {
        double pi = Math.PI;
        a = (a + pi) % (2.0 * pi);
        if (a < 0) a += 2.0 * pi;
        return a - pi;
    }

    static class State {
        double x, y, psi, v;
        State(double x, double y, double psi, double v) {
            this.x = x; this.y = y; this.psi = psi; this.v = v;
        }
    }

    static class Params {
        double L = 2.7;
        double k = 1.4;
        double v0 = 0.5;
        double maxSteer = Math.toRadians(30.0);
    }

    static void makeSPath(double[] xr, double[] yr) {
        int n = xr.length;
        double x0 = 0.0, x1 = 50.0;
        for (int i = 0; i < n; i++) {
            double x = x0 + (x1 - x0) * (double)i / (double)(n - 1);
            xr[i] = x;
            yr[i] = 2.5 * Math.sin(0.18 * x) + 1.0 * Math.sin(0.04 * x);
        }
    }

    static int nearestIndex(double px, double py, double[] xr, double[] yr) {
        double best = Double.POSITIVE_INFINITY;
        int bestI = 0;
        for (int i = 0; i < xr.length; i++) {
            double dx = px - xr[i];
            double dy = py - yr[i];
            double d2 = dx*dx + dy*dy;
            if (d2 < best) { best = d2; bestI = i; }
        }
        return bestI;
    }

    static double headingFromPath(int i, double[] xr, double[] yr) {
        int n = xr.length;
        int j = Math.min(i + 1, n - 1);
        int k = Math.max(i - 1, 0);
        double dx = xr[j] - xr[k];
        double dy = yr[j] - yr[k];
        return Math.atan2(dy, dx);
    }

    static double stanleyControl(State s, double[] xr, double[] yr, Params p, double[] outErr) {
        int i = nearestIndex(s.x, s.y, xr, yr);
        double psiRef = headingFromPath(i, xr, yr);

        // Convention: ePsi = psiRef - psi
        double ePsi = wrapAngle(psiRef - s.psi);

        // Signed lateral error via 2D cross product
        double tx = Math.cos(psiRef), ty = Math.sin(psiRef);
        double vx = s.x - xr[i], vy = s.y - yr[i];
        double cross = tx*vy - ty*vx;
        double eY = Math.copySign(Math.hypot(vx, vy), cross);

        double delta = ePsi + Math.atan2(p.k * eY, s.v + p.v0);
        delta = Math.max(-p.maxSteer, Math.min(p.maxSteer, delta));

        outErr[0] = eY;
        outErr[1] = ePsi;
        return delta;
    }

    static State stepBicycle(State s, double delta, double dt, double L) {
        double x = s.x + s.v * Math.cos(s.psi) * dt;
        double y = s.y + s.v * Math.sin(s.psi) * dt;
        double psi = wrapAngle(s.psi + s.v / L * Math.tan(delta) * dt);
        return new State(x, y, psi, s.v);
    }

    public static void main(String[] args) {
        int n = 400;
        double[] xr = new double[n];
        double[] yr = new double[n];
        makeSPath(xr, yr);

        Params p = new Params();
        double dt = 0.02, T = 25.0;
        int steps = (int)(T / dt);

        State s = new State(-2.0, 3.5, Math.toRadians(-10.0), 6.0);

        double sumSq = 0.0;
        double[] err = new double[2];
        for (int k = 0; k < steps; k++) {
            double delta = stanleyControl(s, xr, yr, p, err);
            s = stepBicycle(s, delta, dt, p.L);
            sumSq += err[0] * err[0];
        }

        double rms = Math.sqrt(sumSq / steps);
        System.out.println("Done. RMS cross-track error eY: " + rms + " m");
        System.out.println("Final state: x=" + s.x + ", y=" + s.y + ", psi=" + s.psi);
    }
}
      

The MATLAB script below runs the same bicycle simulation. For a Simulink implementation, place the bicycle kinematics in an integrator loop and implement the Stanley law in a MATLAB Function block that outputs \( \delta \): from the current state and the (short-horizon) reference path.

Chapter15_Lesson2.m

% Chapter15_Lesson2.m
% Stanley Controller for Ground Vehicles (kinematic bicycle model, educational)
%
% Run:
%   Chapter15_Lesson2
%
% This script:
%   1) builds a smooth reference path (polyline)
%   2) tracks it using Stanley steering control
%   3) plots trajectory and tracking errors

function Chapter15_Lesson2
    % Reference path
    n = 400;
    xr = linspace(0, 50, n);
    yr = 2.5*sin(0.18*xr) + 1.0*sin(0.04*xr);

    % Parameters
    p.L = 2.7;
    p.k = 1.4;
    p.v0 = 0.5;
    p.maxSteer = deg2rad(32);

    dt = 0.02;  T = 25.0;
    steps = floor(T/dt);

    % State: [x;y;psi], constant speed v
    x = -2.0; y = 3.5; psi = deg2rad(-10);
    v = 6.0;

    xs = zeros(steps,1);
    ys = zeros(steps,1);
    eys = zeros(steps,1);
    epsis = zeros(steps,1);
    deltas = zeros(steps,1);

    for k = 1:steps
        [delta, e_y, e_psi] = stanley_control(x, y, psi, v, xr, yr, p);
        % Bicycle step
        x = x + v*cos(psi)*dt;
        y = y + v*sin(psi)*dt;
        psi = wrap_angle(psi + v/p.L * tan(delta) * dt);

        xs(k) = x; ys(k) = y;
        eys(k) = e_y; epsis(k) = e_psi;
        deltas(k) = delta;
    end

    % Plots
    figure; plot(xr, yr, 'LineWidth', 1.5); hold on;
    plot(xs, ys, 'LineWidth', 1.5);
    axis equal; grid on;
    legend('reference path','vehicle trajectory');
    title('Stanley Controller: path tracking');

    t = linspace(0, T, steps);
    figure; plot(t, eys, 'LineWidth', 1.5); hold on;
    plot(t, rad2deg(epsis), 'LineWidth', 1.5);
    grid on; legend('e_y [m]','e_\psi [deg]');
    title('Tracking errors');

    figure; plot(t, rad2deg(deltas), 'LineWidth', 1.5);
    grid on; legend('\delta [deg]');
    title('Steering command');
end

function [delta, e_y, e_psi] = stanley_control(x, y, psi, v, xr, yr, p)
    % Find nearest path index (simple vertex scan)
    d2 = (xr - x).^2 + (yr - y).^2;
    [~, i] = min(d2);

    % Estimate reference heading using neighbors
    i0 = max(i-1, 1);
    i2 = min(i+1, numel(xr));
    psi_ref = atan2(yr(i2)-yr(i0), xr(i2)-xr(i0));

    % Convention: e_psi = psi_ref - psi
    e_psi = wrap_angle(psi_ref - psi);

    % Signed cross-track via 2D cross product
    tx = cos(psi_ref); ty = sin(psi_ref);
    vx = x - xr(i);   vy = y - yr(i);
    cross = tx*vy - ty*vx;
    e_y = sign_safe(cross) * hypot(vx, vy);

    delta = e_psi + atan2(p.k * e_y, v + p.v0);
    delta = min(max(delta, -p.maxSteer), p.maxSteer);
end

function s = sign_safe(a)
    if a >= 0, s = 1; else, s = -1; end
end

function a = wrap_angle(a)
    a = mod(a + pi, 2*pi) - pi;
end
      

This Mathematica script demonstrates closed-loop tracking of a straight path using \( NDSolve \): and verifies local stability by inspecting eigenvalues of the linearized system matrix.

Chapter15_Lesson2.nb

(* Chapter15_Lesson2.nb
   Stanley Controller for Ground Vehicles (Mathematica code cells; paste into a notebook)
   Educational demonstration: straight-path tracking stability + simple simulation.
*)

ClearAll["Global`*"];

(* Kinematic bicycle model *)
L = 2.7;
v = 6.0;

(* Reference path: straight line along x-axis => y_ref=0, psi_ref=0 *)
psiRef[t_] := 0;

(* Error convention: ePsi = psiRef - psi *)
ePsi[psi_] := psiRef[t] - psi;

(* Stanley steering law for straight path:
   delta = ePsi + ArcTan[k*eY/(v+v0)]
   Here eY is lateral position y (signed), since y_ref=0.
*)
k = 1.4;
v0 = 0.5;
maxSteer = 32 Degree;

deltaLaw[y_, psi_] := Module[{d},
  d = ePsi[psi] + ArcTan[k*y/(v + v0)];
  Clip[d, {-maxSteer, maxSteer}]
];

(* Closed-loop dynamics for straight path *)
eqns = {
  x'[t] == v Cos[psi[t]],
  y'[t] == v Sin[psi[t]],
  psi'[t] == v/L Tan[ deltaLaw[y[t], psi[t]] ],
  x[0] == -2, y[0] == 3.5, psi[0] == -10 Degree
};

sol = NDSolve[eqns, {x, y, psi}, {t, 0, 25}][[1]];

(* Plots *)
Show[
 Plot[Evaluate[y[t] /. sol], {t, 0, 25}, PlotRange -> All, AxesLabel -> {"t","y(t)"}],
 PlotLabel -> "Lateral position y(t) under Stanley control (straight path)"
]

Show[
 ParametricPlot[Evaluate[{x[t], y[t]} /. sol], {t, 0, 25}, AxesLabel -> {"x","y"}, PlotRange -> All],
 Graphics[{Red, Dashed, Line[{ {-5, 0}, {55, 0} }]}],
 PlotLabel -> "Trajectory and reference line y=0"
]

(* Small-angle linearization around y=0, ePsi=0:
   eYdot ≈ -v ePsi
   ePsidot ≈ -(v/L) ePsi - (k/L) eY
   Check eigenvalues of A = { {0,-v},{-k/L,-v/L} }
*)
A = { {0, -v}, {-k/L, -v/L} };
Eigenvalues[A]
      

Problem 1 (Error Dynamics for a Straight Path): Consider a straight reference path along the x-axis with \( \psi_{ref}=0 \): and constant speed \( v>0 \): using the kinematic bicycle model. Using the convention \( e_\psi = \psi_{ref} - \psi \): and \( e_y = y \): derive \( \dot e_y \): and \( \dot e_\psi \): exactly (no small-angle approximations).

Solution: From \( \dot y = v\sin\psi \): and \( e_\psi=-\psi \): we have \( \dot e_y = \dot y = v\sin\psi = -v\sin e_\psi \): and since \( \dot\psi = \frac{v}{L}\tan\delta \): we get \( \dot e_\psi = -\dot\psi = -\frac{v}{L}\tan\delta \): which matches Section 4.

Problem 2 (Linear Stability and Routh–Hurwitz): Linearize the closed-loop system around \( (e_y,e_\psi)=(0,0) \): for the Stanley law \( \delta = e_\psi + \arctan\left(\frac{k e_y}{v+v_0}\right) \): and prove stability using trace/determinant (Routh–Hurwitz).

Solution: Using \( \sin e_\psi \approx e_\psi \):, \( \tan\delta \approx \delta \):, and \( \arctan(z) \approx z \): near zero yields

\[ \dot e_y \approx -v e_\psi, \qquad \dot e_\psi \approx -\frac{v}{L}e_\psi - \frac{v}{L}\frac{k}{v+v_0}e_y \]

Thus \( \mathbf{A} = \begin{bmatrix} 0 & -v \\ -\frac{v k}{L(v+v_0)} & -\frac{v}{L} \end{bmatrix} \): has \( \mathrm{tr}(\mathbf{A}) = -\frac{v}{L} < 0 \): and \( \det(\mathbf{A}) = \frac{v^2 k}{L(v+v_0)} > 0 \): for \( v>0 \):, \( k>0 \):. For a 2nd-order LTI system, these conditions imply both eigenvalues have negative real part, hence local exponential stability.

Problem 3 (Curvature Feedforward): Assume the reference path is a circle of radius \( R \): so \( \kappa_{ref} = 1/R \): and the vehicle travels at constant speed. Derive a feedforward steering \( \delta_{ff} \): that yields zero steady-state heading error if the vehicle were exactly on the path.

Solution: For the kinematic bicycle, path curvature is \( \kappa = \tan\delta/L \):. Setting \( \kappa = \kappa_{ref} = 1/R \): gives \( \tan\delta_{ff} = L/R \): hence \( \delta_{ff} = \arctan(L/R) \):. With this term, if \( e_y=0 \): and \( e_\psi=0 \):, then \( \delta=\delta_{ff} \): produces the desired curvature.

Problem 4 (Choosing k from Linear Poles): Using the linearized model in Problem 2, approximate the characteristic polynomial of \( \mathbf{A} \): and choose \( k \): such that the real parts of both eigenvalues are less than \( -\alpha \): for a desired rate \( \alpha>0 \): (ignore actuator limits).

Solution: The characteristic polynomial of \( \mathbf{A} \): is

\[ \lambda^2 - (\mathrm{tr}\,\mathbf{A})\lambda + \det(\mathbf{A}) = 0 \quad\Rightarrow\quad \lambda^2 + \frac{v}{L}\lambda + \frac{v^2 k}{L(v+v_0)} = 0 \]

A sufficient (conservative) way to enforce \( \Re(\lambda) \le -\alpha \): is to match the polynomial to \( (\lambda+\alpha)^2 = \lambda^2 + 2\alpha\lambda + \alpha^2 \): which suggests choosing \( \frac{v}{L} \approx 2\alpha \): (fixed by speed/geometry) and \( \frac{v^2 k}{L(v+v_0)} \approx \alpha^2 \):. Solving for \( k \): gives

\[ k \approx \frac{L(v+v_0)}{v^2}\,\alpha^2 \]

In reality, you validate \( k \): under discrete-time sampling and steering saturation (and often use gain scheduling vs speed).

Problem 5 (Saturation Region for Cross-Track Term): Suppose the steering is limited to \( |\delta| \le \delta_{max} \): and assume \( e_\psi \approx 0 \): (vehicle roughly aligned). Find the maximum \( |e_y| \): such that the cross-track term alone does not saturate.

Solution: With \( e_\psi \approx 0 \): and \( \delta_{ff}=0 \):, saturation is avoided if \( \left|\arctan\left(\frac{k e_y}{v+v_0}\right)\right| \le \delta_{max} \): which is equivalent to \( \left|\frac{k e_y}{v+v_0}\right| \le \tan\delta_{max} \):. Therefore

\[ |e_y| \le \frac{v+v_0}{k}\tan\delta_{max} \]

This bound illustrates why very large \( k \): can be counterproductive when steering is limited.

We formulated Stanley path tracking for a car-like ground vehicle by defining the signed cross-track error \( e_y \): and heading error \( e_\psi \): relative to the path tangent. The control law \( \delta = \delta_{ff} + e_\psi + \arctan\left(\frac{k e_y}{v+v_0}\right) \): combines heading correction with a speed-normalized lateral term and supports curvature feedforward. Local stability on straight paths follows from linearization and Routh–Hurwitz. Implementation hinges on robust nearest-point projection, consistent sign conventions, and handling saturation and low-speed behavior.