A planner can hand you a list of (x, y) points. That list is not a drive. The car still needs a steering angle every few hundredths of a second, and the kinematic bicycle in modules/07_control_sim only moves when you pass steer and acceleration into KinematicBicycleModel.step.
This notebook builds closed-loop tracking, then stares at the place a simple rule fails. Pointing the wheels at the next point cuts the corner. Stanley is heading error plus how far you are off the line, scaled by speed. Too much gain wobbles.
Each idea shows up three times: a picture, a handful of numbers, then a few lines of code. Predict first, then run the cell. The paragraph after the cell says what was surprising, and what it would mean for a car.
import os, subprocess, sys
from pathlib import Path
import matplotlib
import matplotlib.pyplot as plt
import numpy as np
def keep_inline():
matplotlib.use("module://matplotlib_inline.backend_inline", force=True)
ip = None
try:
from IPython import get_ipython
ip = get_ipython()
except Exception:
ip = None
if ip is not None:
ip.run_line_magic("matplotlib", "inline")
%matplotlib inline
def find_repo(start: Path) -> Path:
for p in [start, *start.parents]:
if (p / "modules" / "07_control_sim" / "bicycle_model.py").is_file():
return p.resolve()
return start.resolve()
REPO = find_repo(Path.cwd())
if not (REPO / "modules" / "07_control_sim" / "bicycle_model.py").is_file():
dest = Path.cwd() / "self-driving-ai-course"
if not (dest / "modules" / "07_control_sim" / "bicycle_model.py").is_file():
subprocess.run(
["git", "clone", "--depth", "1", "https://github.com/vvknyn/self-driving-ai-course.git", str(dest)],
check=True,
)
REPO = dest.resolve()
os.chdir(REPO)
sys.path.insert(0, str(REPO / "modules" / "07_control_sim"))
from bicycle_model import KinematicBicycleModel, VehicleState
from controllers import PIDLongitudinalController, PurePursuitController, StanleyController
from simulator import generate_curved_track
try:
import pytest # noqa: F401
except ImportError:
subprocess.run([sys.executable, "-m", "pip", "install", "-q", "pytest"], check=True)
def wrap_angle(angle: float) -> float:
return float((angle + np.pi) % (2.0 * np.pi) - np.pi)
print("Repo:", REPO)
print("wheelbase:", KinematicBicycleModel().L, "m")
Repo: /tmp/sdc
wheelbase: 2.8 m
Repo path and wheelbase 2.8 m come from the module defaults. The rest of the notebook imports controllers from the same folder and stays on CPU.
1. The destination
Three panels, one story. Left: closed loop on the S-curve. Stanley steering plus a speed command, and the bicycle trace on the planned path. Middle: aim at the next waypoint on a quarter turn, and the shortcut inside the curve. Right: the same straight line with a modest Stanley gain versus a huge one, and the lateral wobble.
You do not need the control law yet. This is where the notebook lands.
Predict: the middle trace sits inside the arc. The right panel with large k oscillates across y = 0 while the modest gain settles near the line.
keep_inline()
def aim_at_next(state, path_x, path_y, car):
dist = np.hypot(path_x - state.x, path_y - state.y)
nearest = int(np.argmin(dist))
idx = min(nearest + 1, len(path_x) - 1)
alpha = wrap_angle(np.arctan2(path_y[idx] - state.y, path_x[idx] - state.x) - state.psi)
delta = float(np.clip(alpha, -car.max_steer, car.max_steer))
return delta, alpha, idx
def run_s_curve():
car = KinematicBicycleModel(wheelbase=2.8, max_steer_deg=35.0)
stanley = StanleyController(k_gain=0.75, soft_factor=1.0)
pid = PIDLongitudinalController(kp=1.5, ki=0.05, kd=0.1)
ref_x, ref_y, ref_psi = generate_curved_track(length_m=100.0)
state = VehicleState(x=0.0, y=1.5, psi=0.0, v=8.0)
dt = 0.05
target_speed = 12.0
ego_x, ego_y, ctes = [], [], []
for step in range(180):
front_x, front_y = car.front_axle_position(state)
delta, cte, heading_err = stanley.compute_steering(
front_x, front_y, state.psi, state.v, ref_x, ref_y, ref_psi
)
accel = pid.compute_acceleration(state.v, target_speed, dt)
state = car.step(state, accel, delta, dt)
ego_x.append(state.x)
ego_y.append(state.y)
ctes.append(abs(cte))
if state.x >= ref_x[-1] - 5.0:
break
return ref_x, ref_y, ego_x, ego_y, np.array(ctes)
def run_corner_cut():
R = 15.0
step_deg = 30.0
theta = np.deg2rad(np.arange(0.0, 90.0 + step_deg, step_deg))
corner_x = R * np.sin(theta)
corner_y = R * (1.0 - np.cos(theta))
car = KinematicBicycleModel()
state = VehicleState(x=0.0, y=0.0, psi=0.0, v=10.0)
cut_x, cut_y = [], []
for _ in range(120):
delta, alpha, idx = aim_at_next(state, corner_x, corner_y, car)
state = car.step(state, throttle_accel=0.0, steer_delta=delta, dt=0.05)
cut_x.append(state.x)
cut_y.append(state.y)
if state.x > R * 0.95 and state.y > R * 0.65:
break
radius = np.hypot(np.array(cut_x), np.array(cut_y) - R)
return corner_x, corner_y, cut_x, cut_y, R, radius.min()
def stanley_straight_y(k_gain, dt, seconds=8.0):
path_x = np.arange(0.0, 200.0, 0.5)
path_y = np.zeros_like(path_x)
path_psi = np.zeros_like(path_x)
car = KinematicBicycleModel()
stanley = StanleyController(k_gain=k_gain, soft_factor=1.0)
state = VehicleState(x=0.0, y=1.5, psi=0.0, v=8.0)
ys = []
steps = int(round(seconds / dt))
for _ in range(steps):
front_x, front_y = car.front_axle_position(state)
delta, cte, heading_err = stanley.compute_steering(
front_x, front_y, state.psi, state.v, path_x, path_y, path_psi
)
state = car.step(state, throttle_accel=0.0, steer_delta=delta, dt=dt)
ys.append(state.y)
ys = np.array(ys)
crossings = int(np.sum(ys[1:] * ys[:-1] < 0))
t = np.arange(1, len(ys) + 1) * dt
return t, ys, crossings, ys.min()
ref_x, ref_y, ego_x, ego_y, ctes = run_s_curve()
corner_x, corner_y, cut_x, cut_y, R, closest_r = run_corner_cut()
t_lo, y_lo, cross_lo, min_lo = stanley_straight_y(0.75, 0.20)
t_hi, y_hi, cross_hi, min_hi = stanley_straight_y(15.0, 0.20)
print(f"S-curve steps {len(ctes)} mean |CTE| {ctes.mean():.3f} m max |CTE| {ctes.max():.3f} m")
print(f"corner radius {R:.1f} m closest radius {closest_r:.3f} m")
print(f"k=0.75 crossings {cross_lo} lowest y {min_lo:.3f} m")
print(f"k=15 crossings {cross_hi} lowest y {min_hi:.3f} m")
fig, axes = plt.subplots(1, 3, figsize=(12.5, 3.4))
axes[0].plot(ref_x, ref_y, "k--", label="planned path")
axes[0].plot(ego_x, ego_y, label="Stanley + PID")
axes[0].set_xlabel("x (m)")
axes[0].set_ylabel("y (m)")
axes[0].set_title("S-curve closed loop")
axes[0].legend(fontsize=8)
arc = np.linspace(0.0, np.pi / 2, 200)
axes[1].plot(R * np.sin(arc), R * (1.0 - np.cos(arc)), color="0.75", label=f"{R:.0f} m arc")
axes[1].plot(corner_x, corner_y, "k.", ms=3, label="waypoints")
axes[1].plot(cut_x, cut_y, label="aim at next")
axes[1].set_aspect("equal", adjustable="box")
axes[1].set_xlabel("x (m)")
axes[1].set_ylabel("y (m)")
axes[1].set_title("Corner cut")
axes[1].legend(fontsize=8)
axes[2].plot(t_lo, y_lo, label="k = 0.75")
axes[2].plot(t_hi, y_hi, label="k = 15")
axes[2].axhline(0.0, color="k", lw=0.8)
axes[2].set_xlabel("time (s)")
axes[2].set_ylabel("y (m)")
axes[2].set_title("Gain wobble")
axes[2].legend(fontsize=8)
plt.tight_layout()
plt.show()
S-curve steps 165 mean |CTE| 0.162 m max |CTE| 0.733 m
corner radius 15.0 m closest radius 14.540 m
k=0.75 crossings 0 lowest y 0.006 m
k=15 crossings 37 lowest y -0.271 m
The S-curve preview ran 165 steps with mean cross-track error 0.162 m and max 0.733 m. On the quarter turn, closest radius 14.540 m sits just inside the 15.0 m arc. On the straight line, k = 0.75 crosses y = 0 zero times with lowest y = 0.006 m, while k = 15 crosses 37 times and dips to -0.271 m.
For a car, the left panel is the planner handoff you want: tires on the drawn lane. The middle panel shows waypoint aiming barely clipping the arc. The right panel is why Stanley's cross-track gain stays modest.
2. A path on paper is not a drive
Module 06 ends with arrays of x and y. Those arrays are a drawing. The bicycle moves only when you pass a steering angle and an acceleration into KinematicBicycleModel.step.
Hold the wheel at 0 and the car travels along its current heading. The path can bend away from that line.
Predict: 8 m/s for 4 seconds, heading 0, steer 0. The car's y stays 0. Does the path y = 4 sin(x / 15) stay 0 at the car's x?
keep_inline()
path_x = np.arange(0.0, 40.0, 0.5)
path_y = 4.0 * np.sin(path_x / 15.0)
car = KinematicBicycleModel()
state = VehicleState(x=0.0, y=0.0, psi=0.0, v=8.0)
dt = 0.05
xs, ys = [state.x], [state.y]
for _ in range(80):
state = car.step(state, throttle_accel=0.0, steer_delta=0.0, dt=dt)
xs.append(state.x)
ys.append(state.y)
nearest = int(np.argmin(np.abs(path_x - state.x)))
print("speed 8.0 m/s for 4.00 s, steer 0 deg")
print(f"car x, y = {state.x:.3f}, {state.y:.3f}")
print(f"path y at that x = {path_y[nearest]:.3f}")
fig, ax = plt.subplots(figsize=(7, 3))
ax.plot(path_x, path_y, "k--", label="planned path")
ax.plot(xs, ys, label="steer held at 0")
ax.set_xlabel("x (m)")
ax.set_ylabel("y (m)")
ax.legend()
ax.set_title("A path does not steer the car")
plt.show()
speed 8.0 m/s for 4.00 s, steer 0 deg
car x, y = 32.000, 0.000
path y at that x = 3.384
After 4.00 s the car is at x = 32.000, y = 0.000. The path at that x is y = 3.384. Straight integration never sees the sine bend.
On a real vehicle, this is what happens if you log a route and never close the loop on steering: the map line and the tires diverge.
3. One step of the bicycle
Rear-axle state is (x, y, psi, v). Heading psi is in radians, and 0 points along +x. L is the wheelbase, 2.8 m here. One forward-Euler step with steer delta updates heading by (v / L) * tan(delta) * dt.
Predict: at v = 8.0 m/s, delta = 10 deg, dt = 0.5 s, heading should move by roughly a quarter radian from psi = 0.4.
keep_inline()
v = 8.0
delta = np.deg2rad(10.0)
dt = 0.5
x0, y0, psi0 = 0.0, 1.0, 0.4
car = KinematicBicycleModel()
heading_change = (v / car.L) * np.tan(delta) * dt
state = VehicleState(x=x0, y=y0, psi=psi0, v=v)
after = car.step(state, throttle_accel=0.0, steer_delta=delta, dt=dt)
print(f"v = {v:.1f} m/s")
print(f"delta = {np.degrees(delta):.2f} deg")
print(f"L = {car.L:.1f} m")
print(f"dt = {dt:.1f} s")
print(f"heading change formula {heading_change:.4f} rad")
print(f"psi before {psi0:.4f} after {after.psi:.4f}")
print(f"x {x0:.3f} -> {after.x:.3f} y {y0:.3f} -> {after.y:.3f}")
v = 8.0 m/s
delta = 10.00 deg
L = 2.8 m
dt = 0.5 s
heading change formula 0.2519 rad
psi before 0.4000 after 0.6519
x 0.000 -> 3.684 y 1.000 -> 2.558
Heading change is 0.2519 rad, so the new heading is 0.6519. Position moves to (3.684, 2.558) because velocity projects along the old heading for one Euler step.
For a car, this is the discrete-time model the controller assumes every 50 ms tick.
4. Pointing at the next waypoint cuts the corner
First controller: take the nearest path point to the rear axle, then aim the wheels at the next waypoint. The command is the angle from the car heading to that point, clipped to the mechanical steer limit.
Predict: on a straight road starting 2 m left of center, the first aim angle will be large and negative. On a 15 m radius quarter turn, the trace will sit inside the arc.
keep_inline()
straight_x = np.arange(0.0, 80.0, 0.5)
straight_y = np.zeros_like(straight_x)
car = KinematicBicycleModel()
state = VehicleState(x=0.0, y=2.0, psi=0.0, v=8.0)
aim_x, aim_y = [], []
for i in range(160):
delta, alpha, idx = aim_at_next(state, straight_x, straight_y, car)
if i == 0:
first_alpha, first_delta = alpha, delta
state = car.step(state, throttle_accel=0.0, steer_delta=delta, dt=0.05)
aim_x.append(state.x)
aim_y.append(state.y)
aim_y = np.array(aim_y)
crossings = int(np.sum(aim_y[1:] * aim_y[:-1] < 0))
print(f"first aim {np.degrees(first_alpha):.2f} deg")
print(f"clamped steer {np.degrees(first_delta):.2f} deg")
print(f"y crossings {crossings}")
print(f"lowest y {aim_y.min():.3f} m")
R = 15.0
step_deg = 30.0
theta = np.deg2rad(np.arange(0.0, 90.0 + step_deg, step_deg))
corner_x = R * np.sin(theta)
corner_y = R * (1.0 - np.cos(theta))
state = VehicleState(x=0.0, y=0.0, psi=0.0, v=10.0)
cut_x, cut_y = [], []
for _ in range(120):
delta, alpha, idx = aim_at_next(state, corner_x, corner_y, car)
state = car.step(state, throttle_accel=0.0, steer_delta=delta, dt=0.05)
cut_x.append(state.x)
cut_y.append(state.y)
if state.x > R * 0.95 and state.y > R * 0.65:
break
cut_x = np.array(cut_x)
cut_y = np.array(cut_y)
radius = np.hypot(cut_x, cut_y - R)
print(f"corner radius {R:.1f} m")
print(f"closest radius {radius.min():.3f} m")
fig, axes = plt.subplots(1, 2, figsize=(10, 3.6))
axes[0].plot(straight_x, straight_y, "k--", label="path")
axes[0].plot(aim_x, aim_y, label="aim at next waypoint")
axes[0].set_xlim(0, 40)
axes[0].set_ylim(-2, 2.5)
axes[0].set_xlabel("x (m)")
axes[0].set_ylabel("y (m)")
axes[0].set_title("Straight road, overshoot")
axes[0].legend()
arc = np.linspace(0.0, np.pi / 2, 200)
axes[1].plot(R * np.sin(arc), R * (1.0 - np.cos(arc)), color="0.75", label="15 m radius")
axes[1].plot(corner_x, corner_y, "k.", label="waypoints")
axes[1].plot(cut_x, cut_y, label="aim at next waypoint")
axes[1].set_aspect("equal", adjustable="box")
axes[1].set_xlabel("x (m)")
axes[1].set_ylabel("y (m)")
axes[1].set_title("Coarse corner, cut inside")
axes[1].legend()
plt.tight_layout()
plt.show()
first aim -75.96 deg
clamped steer -35.00 deg
y crossings 15
lowest y -1.386 m
corner radius 15.0 m
closest radius 14.540 m
The first aim is -75.96 deg. The bicycle clips it to -35.00 deg, a full lock toward the line. y crosses 0 15 times and dips to -1.386 m. On the corner, closest radius 14.540 m is slightly inside the 15.0 m arc.
For a car, waypoint chasing is a useful baseline and a bad finish: it overshoots on straights and can hunt on curves.
5. Stanley: heading error plus offset scaled by speed
Stanley, as implemented in StanleyController.compute_steering, is two angles added together.
- Heading error: wrap path tangent minus car heading.
- Cross-track term:
arctan2(k * (-cross_track), max(0.1, v) + soft).
Speed in the denominator is the scaling by speed: the same lateral offset commands a larger steer angle when you are slow.
Predict: with the front axle 2 m left of a straight path, heading aligned, v = 10 m/s, steer should be negative (turn right). At v = 2 m/s the same offset should demand a larger magnitude angle.
keep_inline()
k_gain = 0.8
soft_factor = 1.0
vehicle_v = 10.0
cte_hand = 2.0
heading_hand = 0.0
denom = max(0.1, vehicle_v) + soft_factor
cross = float(np.arctan2(k_gain * (-cte_hand), denom))
steer_hand = heading_hand + cross
path_x = np.linspace(0.0, 50.0, 100)
path_y = np.zeros_like(path_x)
path_psi = np.zeros_like(path_x)
steer, cte, heading = StanleyController(k_gain=k_gain, soft_factor=soft_factor).compute_steering(
front_x=5.0,
front_y=2.0,
vehicle_psi=0.0,
vehicle_v=vehicle_v,
path_x=path_x,
path_y=path_y,
path_psi=path_psi,
)
cross_slow = float(np.arctan2(k_gain * (-cte_hand), max(0.1, 2.0) + soft_factor))
print(f"cross-track error {cte_hand:.3f} m")
print(f"heading error {heading_hand:.3f} rad")
print(f"denominator {denom:.1f}")
print(f"cross-track angle {cross:.4f} rad ({np.degrees(cross):.3f} deg)")
print(f"hand steer {steer_hand:.4f} rad")
print(f"repo steer {steer:.4f} rad")
print(f"repo cross-track {cte:.3f} m")
print(f"repo heading error {heading:.3f} rad")
print(f"same error at 2 m/s: {np.degrees(cross_slow):.3f} deg")
assert abs(steer - steer_hand) < 1e-12
assert abs(cte - cte_hand) < 1e-12
cross-track error 2.000 m
heading error 0.000 rad
denominator 11.0
cross-track angle -0.1444 rad (-8.276 deg)
hand steer -0.1444 rad
repo steer -0.1444 rad
repo cross-track 2.000 m
repo heading error 0.000 rad
same error at 2 m/s: -28.072 deg
Hand and repo agree: cross-track error 2.000 m, heading error 0.000 rad, denominator 11.0, steer -0.1444 rad (-8.276 deg). At 2 m/s the cross-track angle alone is -28.072 deg — much larger than the -8.276 deg term at the hand-step speed.
For a car, Stanley is how you pull back to the lane center without waypoint clipping: heading alignment plus a lateral correction that relaxes as speed rises.
6. Too much gain wobbles
Stanley's k scales the cross-track angle. Same straight path, same 0.20 s step, two gains.
Predict: k = 15 will overshoot the centerline and oscillate. k = 0.75 should ease in without crossing y = 0.
keep_inline()
t_lo, y_lo, cross_lo, min_lo = stanley_straight_y(0.75, 0.20)
t_hi, y_hi, cross_hi, min_hi = stanley_straight_y(15.0, 0.20)
_, _, cross_fast, _ = stanley_straight_y(15.0, 0.05)
print(f"k=0.75 dt=0.20 s crossings={cross_lo} lowest y={min_lo:.3f} m")
print(f"k=15 dt=0.20 s crossings={cross_hi} lowest y={min_hi:.3f} m")
print(f"k=15 dt=0.05 s crossings={cross_fast}")
fig, ax = plt.subplots(figsize=(7.5, 3.2))
ax.plot(t_lo, y_lo, label="k = 0.75, dt = 0.20 s")
ax.plot(t_hi, y_hi, label="k = 15, dt = 0.20 s")
ax.axhline(0.0, color="k", lw=0.8)
ax.set_xlabel("time (s)")
ax.set_ylabel("y (m)")
ax.set_title("Same straight line, two gains")
ax.legend()
plt.show()
k=0.75 dt=0.20 s crossings=0 lowest y=0.006 m
k=15 dt=0.20 s crossings=37 lowest y=-0.271 m
k=15 dt=0.05 s crossings=0
Gain 0.75 held for 8.00 s crosses the line 0 times. The lowest y is 0.006 m: it eases in and stays on the positive side. Gain 15 reports 37 crossings in eight seconds at dt = 0.20 s and reaches -0.271 m. Tightening the step to 0.05 s with k = 15 reports 0 crossings in the same window but harsher steering each tick.
For a car, this is the tuning trade: a large cross-track gain pulls in fast but excites lateral oscillation; production gains stay near the modest branch.
7. Closed loop on the S-curve
Now send Stanley's steer and a PID acceleration command back into step on every tick. The path is the S-curve from generate_curved_track. Start 1.5 m off the centerline at 8 m/s and ask for 12 m/s.
Predict: cross-track error should shrink after the first second. Speed should climb toward 12 m/s but hit the acceleration clip on the first tick.
keep_inline()
car = KinematicBicycleModel(wheelbase=2.8, max_steer_deg=35.0)
stanley = StanleyController(k_gain=0.75, soft_factor=1.0)
pid = PIDLongitudinalController(kp=1.5, ki=0.05, kd=0.1)
ref_x, ref_y, ref_psi = generate_curved_track(length_m=100.0)
state = VehicleState(x=0.0, y=1.5, psi=0.0, v=8.0)
dt = 0.05
target_speed = 12.0
err0 = target_speed - state.v
p_term = pid.kp * err0
i_term = pid.ki * err0 * dt
d_term = pid.kd * err0 / dt
print(f"P {p_term:.3f} I {i_term:.3f} D {d_term:.3f} sum {p_term + i_term + d_term:.3f}")
ctes = []
ego_x, ego_y = [], []
for step in range(180):
front_x, front_y = car.front_axle_position(state)
delta, cte, heading_err = stanley.compute_steering(
front_x, front_y, state.psi, state.v, ref_x, ref_y, ref_psi
)
accel = pid.compute_acceleration(state.v, target_speed, dt)
if step == 0:
print(f"first accel request {accel:.3f} m/s^2")
print(f"accel clip {car.max_accel:.1f} m/s^2")
state = car.step(state, accel, delta, dt)
ctes.append(abs(cte))
ego_x.append(state.x)
ego_y.append(state.y)
if step in (0, 80, 160):
print(f"t={step * dt:.2f} s x={state.x:.3f} y={state.y:.3f} cte={cte:.3f} m")
if state.x >= ref_x[-1] - 5.0:
break
ctes = np.array(ctes)
print(f"steps {len(ctes)}")
print(f"mean |CTE| {ctes.mean():.3f} m")
print(f"max |CTE| {ctes.max():.3f} m")
print(f"steady-state |CTE| {ctes[-40:].mean():.3f} m")
fig, ax = plt.subplots(figsize=(7.5, 3.4))
ax.plot(ref_x, ref_y, "k--", label="planned path")
ax.plot(ego_x, ego_y, label="Stanley + speed controller")
ax.set_xlabel("x (m)")
ax.set_ylabel("y (m)")
ax.set_title("Closed loop on the S-curve")
ax.legend()
plt.show()
P 6.000 I 0.010 D 8.000 sum 14.010
first accel request 14.010 m/s^2
accel clip 3.5 m/s^2
t=0.00 s x=0.400 y=1.500 cte=0.733 m
t=4.00 s x=45.059 y=0.642 cte=0.091 m
t=8.00 s x=92.586 y=-0.476 cte=-0.039 m
steps 165
mean |CTE| 0.162 m
max |CTE| 0.733 m
steady-state |CTE| 0.047 m
The speed split is P 6.000, I 0.010, D 8.000, sum 14.010 m/s². The bicycle clips that request to 3.5 m/s². Cross-track error at t = 0.00 s is 0.733 m and falls to 0.091 m by t = 4.00 s. Mean absolute cross-track error over 165 steps is 0.162 m; the last forty steps average 0.047 m.
For a car, this is the full stack fragment: lateral Stanley on the front axle, longitudinal PID on speed, both feeding the same bicycle integrator the module tests exercise.
8. Exercises
Exercise — yaw_change. Return the heading change of one Euler step, (v / wheelbase) * tan(steer_rad) * dt, in radians.
def yaw_change_student(v, steer_rad, wheelbase, dt):
# TODO: (v / wheelbase) * tan(steer_rad) * dt
raise NotImplementedError
def yaw_change_reference(v, steer_rad, wheelbase, dt):
return (v / wheelbase) * np.tan(steer_rad) * dt
def get_yaw_change():
try:
yaw_change_student(1.0, 0.0, 2.8, 0.1)
except NotImplementedError:
print("Using reference yaw_change (TODO not implemented)")
return yaw_change_reference
return yaw_change_student
yaw_change = get_yaw_change()
before = VehicleState(x=0.0, y=1.0, psi=0.4, v=8.0)
after = KinematicBicycleModel().step(before, throttle_accel=0.0, steer_delta=np.deg2rad(10.0), dt=0.5)
expected_dyaw = after.psi - before.psi
got_dyaw = yaw_change(8.0, np.deg2rad(10.0), 2.8, 0.5)
print(f"yaw change {got_dyaw:.4f} rad")
assert abs(got_dyaw - expected_dyaw) < 1e-12
print("✅ correct: yaw_change =", f"{got_dyaw:.4f}")
Using reference yaw_change (TODO not implemented)
yaw change 0.2519 rad
✅ correct: yaw_change = 0.2519
✅ correct: yaw_change = 0.2519 rad. The reference implementation ran because the TODO still raises NotImplementedError.
Filling in the student function makes the check use your code instead of the reference.
Solution
def yaw_change(v, steer_rad, wheelbase, dt):
return (v / wheelbase) * np.tan(steer_rad) * dt
Exercise — stanley_steer. Return the repo's steer command in radians: heading error plus atan2(k * (-cross_track_error), max(0.1, speed) + soft_factor).
def stanley_steer_student(heading_error, cross_track_error, speed, k_gain, soft_factor):
# TODO: heading_error + atan2(k_gain * (-cross_track_error), max(0.1, speed) + soft_factor)
raise NotImplementedError
def stanley_steer_reference(heading_error, cross_track_error, speed, k_gain, soft_factor):
cross = np.arctan2(k_gain * (-cross_track_error), max(0.1, speed) + soft_factor)
return float(heading_error + cross)
def get_stanley_steer():
try:
stanley_steer_student(0.0, 0.0, 1.0, 0.8, 1.0)
except NotImplementedError:
print("Using reference stanley_steer (TODO not implemented)")
return stanley_steer_reference
return stanley_steer_student
stanley_steer = get_stanley_steer()
path_x = np.linspace(0.0, 50.0, 100)
path_y = np.zeros_like(path_x)
path_psi = np.zeros_like(path_x)
expected_steer, expected_cte, expected_heading = StanleyController(0.8, 1.0).compute_steering(
5.0, 2.0, 0.0, 10.0, path_x, path_y, path_psi
)
got_steer = stanley_steer(expected_heading, expected_cte, 10.0, 0.8, 1.0)
print(f"stanley steer {got_steer:.4f} rad ({np.degrees(got_steer):.3f} deg)")
assert abs(got_steer - expected_steer) < 1e-12
print("✅ correct: stanley_steer =", f"{got_steer:.4f}")
Using reference stanley_steer (TODO not implemented)
stanley steer -0.1444 rad (-8.276 deg)
✅ correct: stanley_steer = -0.1444
✅ correct: stanley_steer = -0.1444 rad (-8.276 deg), matching the hand step in section 5. The reference implementation ran because the TODO still raises NotImplementedError.
Solution
def stanley_steer(heading_error, cross_track_error, speed, k_gain, soft_factor):
cross = np.arctan2(k_gain * (-cross_track_error), max(0.1, speed) + soft_factor)
return float(heading_error + cross)
import pytest
print("pytest", pytest.__version__)
pytest 9.1.1
pytest 9.1.1 is available if you want to run the module tests locally after filling the exercises.
9. Recap
- A path is coordinates. With steer held at 0 for 4.00 s at 8 m/s, the car is at
(32.000, 0.000)while the path is aty = 3.384. - One Euler step at 10 deg steer changes heading by
0.2519rad to0.6519. - Aim-at-next locks at
-35.00deg and cuts a15.0m corner to closest radius14.540m. - Stanley at 2 m left and 10 m/s commands
-0.1444rad; at 2 m/s the cross-track angle alone is-28.072deg. - Gain
0.75crosses0times on a straight line; gain15crosses37times. - Closed loop on the S-curve: mean |CTE|
0.162m over165steps, steady-state0.047m.
The planner draws the lane. Closed-loop control is what makes the tires follow it.