A differential-drive robot that plans with A* and localises with an EKF

What you will build

This project builds the smallest complete navigation stack for a wheeled robot, in one NumPy file. A differential-drive robot starts in one corner of a 10 m by 10 m room with two internal walls and a box. It plans a route to the far corner with A* on an occupancy grid, follows the route with a pure-pursuit controller, and estimates its own pose with an extended Kalman filter (EKF). The filter fuses wheel odometry, which is smooth but drifts, with position fixes that are noisy but do not drift.

The controller steers on the filter’s estimate, not on the true pose, which is how a real robot has to work. The simulation keeps the true pose separately so that you can score the estimate. In the run on this page the robot reaches the goal in 69.8 s. The EKF stays within 0.151 m of the truth for the whole drive, while odometry on its own ends 5.6 m away from where the robot really is.

The lessons on differential drive, grid path planning and sensors and noise cover the pieces one at a time. The Kalman filter cheat sheet has the equations in one place.

What you need

• Python 3.10 or later and NumPy (pip install numpy). We ran it with NumPy 2.5.4 on an Apple Silicon Mac mini.
• Nothing else. There is no simulator, no plotting library and no randomness that is not seeded.
• About 3 hours to read the design, run it and work through the variations.

Design

Map, inflation and A*

The room is a 100 by 100 grid of 0.1 m cells, indexed grid[row, col] with row = y / 0.1 and col = x / 0.1. The world frame follows REP 103: x to the right, y up, z out of the floor, angles anticlockwise from x. The robot is a disc of 0.20 m radius. Growing every obstacle by that radius turns the robot into a point, so A* can plan in the grid as it stands. This grown grid is the configuration space of a disc robot. We add a further 0.15 m of margin because the controller steers on an estimate that can be off by 0.1 m or more. Without the margin, the planned path touches the inflated walls and estimation error alone can push the real robot into them. The Things to try section shows this happening.

A* uses 8-connected moves with costs of 1 for straight steps and √2 for diagonal steps. The heuristic is the octile distance, the exact cost of the shortest 8-connected path on an empty grid. It never overestimates, so A* returns a shortest path on the grid. A diagonal step is blocked when either of the two cells it squeezes between is occupied, so the path cannot cut a corner through a wall. On this map A* expands 4,304 of the 10,000 cells and returns a path of 20.15 m.

Pure pursuit

Pure pursuit picks a point on the path 0.5 m ahead of the robot and drives along the circular arc that reaches it. If that point is at (x_r, y_r) in the robot frame (x forward, y left), the arc’s curvature is κ = 2 y_r / (x_r² + y_r²). The angular rate is ω = v κ. The forward speed is 0.5 m/s on straight sections and drops as 1 / (1 + 1.5 |κ|) in turns. A longer lookahead gives smoother paths that cut corners more. A shorter one tracks tightly and oscillates more. The function only searches 40 path points ahead of the last nearest point, so the robot cannot jump to a later part of the path that passes nearby.

Motion, noise and the EKF

The state is (x, y, θ). Each 50 ms step the controller turns (v, ω) into left and right wheel travel, d_l and d_r, for a wheel base of 0.30 m. The motion model is the midpoint form: the robot moves (d_l + d_r) / 2 along the heading halfway through the turn, and the heading changes by (d_r - d_l) / 0.30.

The simulated robot has two kinds of odometry error. The encoders report each wheel’s travel with zero-mean noise of 0.01 m per √m travelled, so the variance grows with distance. The true right wheel is also 0.5 % larger than the software assumes, a systematic error like a worn or under-inflated tyre. The EKF knows about the first error and not the second, which is the usual situation on a real robot.

Predict. The filter moves its mean with the same motion model, using the measured wheel travel. It propagates the covariance as P ← F P Fᵀ + G M Gᵀ. F is the Jacobian of the motion with respect to the state. G is its Jacobian with respect to the two wheel travels. M is the diagonal wheel-noise covariance for this step. This puts noise in where it physically enters, at the wheels, and lets the Jacobians turn it into the correct correlated uncertainty in x, y and θ.

Update. Every 0.5 s a position fix arrives: the true x and y plus Gaussian noise of 0.1 m per axis. On a real robot this could come from an ultra-wideband beacon system, a motion-capture system or a camera that sees fixed markers. The measurement model is linear, z = H x with H = [[1, 0, 0], [0, 1, 0]], so the update is the ordinary Kalman update. The fix does not measure heading. The filter still corrects heading, through the correlation between position and heading that the predict step builds up.

Two frames, two estimates. REP 105, the ROS convention for mobile-robot frames, describes this split exactly. The odometry-only pose is what ROS calls the odom frame: continuous and smooth, but free to drift without bound. The fused EKF pose corresponds to the map frame: it does not drift, but it jumps a little at each fix. The program tracks both so that you can see the difference.

Is the filter honest? At each step the program computes the squared Mahalanobis distance of the true position error under the EKF’s 2 x 2 position covariance. If the filter’s covariance is right, that value is below 5.99 (the 95 % point of a chi-squared distribution with 2 degrees of freedom) on about 95 % of steps. Much less than 95 % means the filter is overconfident. Much more means it is too cautious.

Code

Save this as astar_ekf.py. It is one block: map, planner, robot model, filter, controller and the closed loop.

# astar_ekf.py: plan with A*, follow with pure pursuit, localise with an EKF.
import heapq
import math

import numpy as np

rng = np.random.default_rng(7)

# ---------- 1. Map: a 10 m x 10 m room on a 0.1 m grid ----------
RES = 0.1                                  # metres per cell
N = 100                                    # cells per side
grid = np.zeros((N, N), dtype=bool)        # grid[row, col], row = y / RES, col = x / RES
grid[0, :] = grid[-1, :] = grid[:, 0] = grid[:, -1] = True     # outer walls
grid[0:65, 30:33] = True                   # wall from the bottom, gap at the top
grid[35:100, 60:63] = True                 # wall from the top, gap at the bottom
grid[70:80, 75:90] = True                  # a box near the goal

ROBOT_RADIUS = 0.20                        # m
SAFETY_MARGIN = 0.15                       # m, extra room for localisation error


def inflate(occ: np.ndarray, radius_m: float) -> np.ndarray:
    """Grow obstacles by the robot radius so the planner can treat the robot as a point."""
    r = math.ceil(radius_m / RES)
    out = occ.copy()
    rows, cols = np.nonzero(occ)
    for dr in range(-r, r + 1):
        for dc in range(-r, r + 1):
            if dr * dr + dc * dc <= r * r:
                rr = np.clip(rows + dr, 0, N - 1)
                cc = np.clip(cols + dc, 0, N - 1)
                out[rr, cc] = True
    return out


def astar(occ: np.ndarray, start: tuple[int, int], goal: tuple[int, int]) -> list[tuple[int, int]]:
    """8-connected A* with the octile-distance heuristic. Cells are (row, col)."""
    moves = [(-1, 0, 1.0), (1, 0, 1.0), (0, -1, 1.0), (0, 1, 1.0),
             (-1, -1, math.sqrt(2)), (-1, 1, math.sqrt(2)), (1, -1, math.sqrt(2)), (1, 1, math.sqrt(2))]

    def h(c: tuple[int, int]) -> float:
        dr, dc = abs(c[0] - goal[0]), abs(c[1] - goal[1])
        return (dr + dc) + (math.sqrt(2) - 2) * min(dr, dc)

    g = {start: 0.0}
    parent = {start: None}
    open_heap = [(h(start), 0.0, start)]
    closed = set()
    while open_heap:
        _, g_cur, cur = heapq.heappop(open_heap)
        if cur in closed:
            continue
        if cur == goal:
            path = [cur]
            while parent[path[-1]] is not None:
                path.append(parent[path[-1]])
            print(f"A*: expanded {len(closed)} cells, path cost {g_cur * RES:.2f} m")
            return path[::-1]
        closed.add(cur)
        for dr, dc, cost in moves:
            nb = (cur[0] + dr, cur[1] + dc)
            if not (0 <= nb[0] < N and 0 <= nb[1] < N) or occ[nb]:
                continue
            if dr and dc and (occ[cur[0] + dr, cur[1]] or occ[cur[0], cur[1] + dc]):
                continue                    # no cutting corners past an obstacle
            g_new = g_cur + cost
            if g_new < g.get(nb, math.inf):
                g[nb] = g_new
                parent[nb] = cur
                heapq.heappush(open_heap, (g_new + h(nb), g_new, nb))
    raise RuntimeError("no path")


def to_cell(x: float, y: float) -> tuple[int, int]:
    return int(y / RES), int(x / RES)


def to_xy(cell: tuple[int, int]) -> np.ndarray:
    return np.array([(cell[1] + 0.5) * RES, (cell[0] + 0.5) * RES])


start_xy, goal_xy = (1.0, 1.0), (9.0, 9.0)
occ = inflate(grid, ROBOT_RADIUS + SAFETY_MARGIN)
cells = astar(occ, to_cell(*start_xy), to_cell(*goal_xy))
path = np.array([to_xy(c) for c in cells])

# ---------- 2. Robot, sensors and the EKF ----------
DT = 0.05                  # control period (s)
WHEEL_BASE = 0.30          # m
V_MAX = 0.5                # m/s
LOOKAHEAD = 0.5            # m, pure pursuit
ODO_SIGMA = 0.01           # wheel distance noise, m per sqrt(m) travelled
RIGHT_WHEEL_SCALE = 1.005  # true right wheel is 0.5 % larger than the odometry assumes
FIX_SIGMA = 0.10           # position fix noise (m), 1 sigma per axis
FIX_EVERY = 10             # one fix every 10 steps = 0.5 s


def motion(state: np.ndarray, d_l: float, d_r: float) -> np.ndarray:
    """Midpoint integration of a differential drive for wheel travels d_l, d_r."""
    x, y, th = state
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    return np.array([x + ds * math.cos(th + dth / 2), y + ds * math.sin(th + dth / 2), th + dth])


def ekf_predict(mu: np.ndarray, P: np.ndarray, d_l: float, d_r: float) -> tuple[np.ndarray, np.ndarray]:
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    a = mu[2] + dth / 2
    F = np.array([[1, 0, -ds * math.sin(a)],
                  [0, 1, ds * math.cos(a)],
                  [0, 0, 1]])
    # Jacobian of the motion with respect to (d_l, d_r)
    G = np.array([[0.5 * math.cos(a) + ds * math.sin(a) / (2 * WHEEL_BASE), 0.5 * math.cos(a) - ds * math.sin(a) / (2 * WHEEL_BASE)],
                  [0.5 * math.sin(a) - ds * math.cos(a) / (2 * WHEEL_BASE), 0.5 * math.sin(a) + ds * math.cos(a) / (2 * WHEEL_BASE)],
                  [-1 / WHEEL_BASE, 1 / WHEEL_BASE]])
    M = np.diag([ODO_SIGMA ** 2 * abs(d_l) + 1e-6, ODO_SIGMA ** 2 * abs(d_r) + 1e-6])
    return motion(mu, d_l, d_r), F @ P @ F.T + G @ M @ G.T


def ekf_update(mu: np.ndarray, P: np.ndarray, z: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
    H = np.array([[1.0, 0, 0], [0, 1.0, 0]])
    R = np.eye(2) * FIX_SIGMA ** 2
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    mu = mu + K @ (z - H @ mu)
    P = (np.eye(3) - K @ H) @ P
    return mu, P


def pure_pursuit(pose: np.ndarray, path: np.ndarray, idx: int) -> tuple[float, float, int]:
    """Return (v, omega, nearest index) steering towards a point LOOKAHEAD metres along the path."""
    window = path[idx:idx + 40]
    idx = idx + int(np.argmin(np.linalg.norm(window - pose[:2], axis=1)))
    target = path[-1]
    for p in path[idx:]:
        if np.linalg.norm(p - pose[:2]) >= LOOKAHEAD:
            target = p
            break
    dx, dy = target - pose[:2]
    c, s = math.cos(pose[2]), math.sin(pose[2])
    x_r, y_r = c * dx + s * dy, -s * dx + c * dy          # target in the robot frame
    curvature = 2 * y_r / max(x_r ** 2 + y_r ** 2, 1e-9)
    v = V_MAX / (1 + 1.5 * abs(curvature))               # slow down in tight turns
    return v, v * curvature, idx


# ---------- 3. Closed loop: control on the EKF estimate ----------
start_heading = math.atan2(*(path[5] - path[0])[::-1])
truth = np.array([*start_xy, start_heading])
mu, P = truth.copy(), np.diag([0.01, 0.01, 0.01])
dead = truth.copy()                                       # odometry only, for comparison
idx, k = 0, 0
log = []
while np.linalg.norm(truth[:2] - goal_xy) > 0.15 and k < 4000:
    v, w, idx = pure_pursuit(mu, path, idx)
    d_l_cmd, d_r_cmd = (v - w * WHEEL_BASE / 2) * DT, (v + w * WHEEL_BASE / 2) * DT
    truth = motion(truth, d_l_cmd, d_r_cmd * RIGHT_WHEEL_SCALE)   # what the robot really did
    d_l_meas = d_l_cmd + rng.normal(0, ODO_SIGMA * math.sqrt(abs(d_l_cmd)))
    d_r_meas = d_r_cmd + rng.normal(0, ODO_SIGMA * math.sqrt(abs(d_r_cmd)))
    dead = motion(dead, d_l_meas, d_r_meas)
    mu, P = ekf_predict(mu, P, d_l_meas, d_r_meas)
    if k % FIX_EVERY == 0:
        z = truth[:2] + rng.normal(0, FIX_SIGMA, 2)
        mu, P = ekf_update(mu, P, z)
    k += 1
    in_2sigma = (truth[:2] - mu[:2]) @ np.linalg.inv(P[:2, :2]) @ (truth[:2] - mu[:2]) < 5.99
    log.append((k * DT, np.linalg.norm(truth[:2] - mu[:2]), np.linalg.norm(truth[:2] - dead[:2]), in_2sigma,
                *truth[:2]))

log = np.array(log)
t, e_ekf, e_dead = log[:, 0], log[:, 1], log[:, 2]
occupied_xy = np.argwhere(grid)[:, ::-1] * RES + RES / 2               # centres of occupied cells
gap = min(np.min(np.linalg.norm(occupied_xy - p, axis=1)) for p in log[:, 4:6]) - RES / 2 - ROBOT_RADIUS
print(f"reached goal: {np.linalg.norm(truth[:2] - goal_xy) <= 0.15} in {t[-1]:.1f} s, {k} steps")
print(f"smallest gap between the robot's edge and an obstacle: {gap:.3f} m")
print(f"EKF position error:      mean {e_ekf.mean():.3f} m, rms {np.sqrt((e_ekf ** 2).mean()):.3f} m, max {e_ekf.max():.3f} m")
print(f"odometry-only error:     mean {e_dead.mean():.3f} m, rms {np.sqrt((e_dead ** 2).mean()):.3f} m, max {e_dead.max():.3f} m, final {e_dead[-1]:.3f} m")
print(f"truth inside EKF 95 % ellipse: {100 * log[:, 3].mean():.1f} % of steps")
print("planned path, every 6th cell (x,y in m):")
print(" ".join(f"{x:.2f},{y:.2f}" for x, y in [*path[::6], path[-1]]))
print("every 2 s: t (s), EKF error (m), odometry-only error (m), true x (m), true y (m)")
for i in range(39, len(t), 40):
    print(f"{t[i]:.1f} {e_ekf[i]:.3f} {e_dead[i]:.3f} {log[i, 4]:.2f} {log[i, 5]:.2f}")

The closed loop is the while loop in section 3. Its order matters. The controller reads the estimate. The true robot moves with the commanded wheel travel, with the right wheel scaled. The encoders then report the commanded travel plus noise. The filter predicts from those encoder readings and, on every tenth step, updates with a fix. The truth is used only to generate the fix and to score the estimate.

Run

python -m pip install numpy
python astar_ekf.py

It runs in a fraction of a second.

Test

This is the output from our run. With the seed fixed at 7 you should see the same numbers. A different NumPy version can change the random stream and the last digits.

A*: expanded 4304 cells, path cost 20.15 m
reached goal: True in 69.8 s, 1395 steps
smallest gap between the robot's edge and an obstacle: 0.082 m
EKF position error:      mean 0.051 m, rms 0.057 m, max 0.151 m
odometry-only error:     mean 1.922 m, rms 2.520 m, max 5.639 m, final 5.639 m
truth inside EKF 95 % ellipse: 95.3 % of steps
planned path, every 6th cell (x,y in m):
1.05,1.05 1.15,1.65 1.15,2.25 1.15,2.85 1.15,3.45 1.15,4.05 1.55,4.65 2.15,5.25 2.55,5.85 2.55,6.45 2.95,6.95 3.55,6.85 4.15,6.25 4.75,5.65 4.85,5.05 5.05,4.45 5.35,3.85 5.65,3.25 6.25,3.05 6.75,3.45 6.75,4.05 7.25,4.65 7.85,5.25 8.45,5.85 9.05,6.45 9.45,7.05 9.45,7.65 9.45,8.25 9.25,8.85 9.05,9.05
every 2 s: t (s), EKF error (m), odometry-only error (m), true x (m), true y (m)
2.0 0.049 0.019 1.15 1.77
4.0 0.048 0.032 1.20 2.58
6.0 0.068 0.033 1.20 3.37
8.0 0.053 0.063 1.22 4.03
10.0 0.027 0.103 1.39 4.47
12.0 0.098 0.227 1.84 5.14
14.0 0.146 0.396 2.26 5.72
16.0 0.054 0.573 2.49 6.34
18.0 0.073 0.695 2.66 6.74
20.0 0.053 0.792 2.92 6.96
22.0 0.046 0.865 3.19 7.03
24.0 0.075 0.911 3.52 6.96
26.0 0.063 0.950 3.95 6.67
28.0 0.034 1.028 4.45 6.11
30.0 0.049 1.121 4.73 5.66
32.0 0.072 1.204 4.88 5.06
34.0 0.091 1.376 5.08 4.61
36.0 0.066 1.640 5.37 4.15
38.0 0.021 1.872 5.53 3.59
40.0 0.012 2.072 5.71 3.25
42.0 0.024 2.254 5.96 3.09
44.0 0.049 2.417 6.24 3.05
46.0 0.022 2.525 6.49 3.14
48.0 0.036 2.567 6.67 3.36
50.0 0.059 2.537 6.75 3.71
52.0 0.029 2.514 6.83 4.17
54.0 0.106 2.698 7.14 4.65
56.0 0.054 3.072 7.63 5.18
58.0 0.030 3.672 8.31 5.79
60.0 0.078 4.194 8.81 6.35
62.0 0.046 4.685 9.24 6.89
64.0 0.104 4.935 9.37 7.27
66.0 0.061 5.266 9.39 8.01
68.0 0.048 5.522 9.35 8.53

The robot reaches the goal and keeps at least 0.082 m between its edge and every obstacle. The EKF position error has a mean of 0.051 m and never exceeds 0.151 m. Odometry alone does well for the first 6 s, better than the filter in fact, because over a few metres the wheels are more accurate than a 0.1 m fix. After that its heading error grows, and the position error grows with it, to 5.6 m by the goal. The truth falls inside the EKF’s 95 % ellipse on 95.3 % of steps, so the covariance is a fair description of the real error, even though the filter does not model the wheel-size error. Frequent fixes keep that bias from building up.

If the robot does not reach the goal, check the A* line first. A path cost far longer than 20 m means the map or the inflation radius has changed. A smallest gap below zero means the robot’s footprint overlapped an obstacle at some point.

On a real robot

Every part of this program has a direct counterpart on hardware, and each counterpart brings its own problems.

• Encoders. You get tick counts, not metres. Wheel travel is ticks / ticks_per_rev × 2π × wheel radius. Calibrate the wheel radius and the wheel base by driving a measured straight line and a measured number of turns on the spot. The 0.5 % error modelled here is the kind of error that calibration leaves behind.
• Wheel slip. On a slippery floor or during hard acceleration the wheels turn without the robot moving. The EKF has no way to know. Limit acceleration, and give the filter a larger M during manoeuvres.
• A gyro. Most mobile robots add an IMU yaw-rate gyro, because heading error is what makes odometry drift. Treat the gyro rate as a second input to the predict step or as a measurement of θ.
• Fixes arrive late. A real fix has a timestamp that is older than the current pose. Either buffer past states and apply the update at the right time, or keep the delay small compared with the robot’s speed.
• The map is not known in advance. Here the grid is given. On a real robot it comes from SLAM, and the “position fix” is often the SLAM pose itself.

The block below reads encoder counts from a motor controller over a serial port and runs the EKF predict step on them. It needs a physical robot, the pyserial package and firmware that prints E <left_ticks> <right_ticks> at a fixed rate. That protocol is an example, not a standard. The constants are placeholders for your robot.

# needs-hardware
import math

import numpy as np
import serial  # pyserial

TICKS_PER_REV = 2048          # encoder counts per wheel revolution, from your motor's datasheet
WHEEL_RADIUS = 0.035          # m, measure it
WHEEL_BASE = 0.30             # m, calibrate it
ODO_SIGMA = 0.01


def motion(state, d_l, d_r):
    x, y, th = state
    ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
    return np.array([x + ds * math.cos(th + dth / 2), y + ds * math.sin(th + dth / 2), th + dth])


def odometry_loop(port: str = "/dev/ttyACM0") -> None:
    mu, P = np.zeros(3), np.diag([0.01, 0.01, 0.01])
    last = None
    with serial.Serial(port, 115200, timeout=1) as link:
        while True:
            parts = link.readline().decode(errors="ignore").split()
            if len(parts) != 3 or parts[0] != "E":
                continue
            ticks = np.array([int(parts[1]), int(parts[2])])
            if last is not None:
                d_l, d_r = (ticks - last) / TICKS_PER_REV * 2 * math.pi * WHEEL_RADIUS
                ds, dth = (d_r + d_l) / 2, (d_r - d_l) / WHEEL_BASE
                a = mu[2] + dth / 2
                F = np.array([[1, 0, -ds * math.sin(a)], [0, 1, ds * math.cos(a)], [0, 0, 1]])
                G = np.array([[0.5 * math.cos(a) + ds * math.sin(a) / (2 * WHEEL_BASE), 0.5 * math.cos(a) - ds * math.sin(a) / (2 * WHEEL_BASE)],
                              [0.5 * math.sin(a) - ds * math.cos(a) / (2 * WHEEL_BASE), 0.5 * math.sin(a) + ds * math.cos(a) / (2 * WHEEL_BASE)],
                              [-1 / WHEEL_BASE, 1 / WHEEL_BASE]])
                M = np.diag([ODO_SIGMA ** 2 * abs(d_l) + 1e-6, ODO_SIGMA ** 2 * abs(d_r) + 1e-6])
                mu, P = motion(mu, d_l, d_r), F @ P @ F.T + G @ M @ G.T
                print(f"x={mu[0]:.3f} y={mu[1]:.3f} th={mu[2]:.3f} sigma_xy={math.sqrt(P[0, 0] + P[1, 1]):.3f}")
            last = ticks

Signed tick counters wrap around on most controllers, so handle overflow in the difference before you trust long runs.

Things to try

1. Remove the safety margin. Set SAFETY_MARGIN = 0.0. In our run the path got shorter (18.95 m) and the smallest gap became -0.084 m: the robot clipped a wall. The estimate was no worse. The plan left no room for it to be wrong.
2. Fewer fixes. Set FIX_EVERY = 100 (one fix every 5 s). The EKF RMS error rose from 0.057 m to 0.139 m in our run, and the robot touched an obstacle (gap -0.113 m) even with the margin.
3. Perfect wheels. Set RIGHT_WHEEL_SCALE = 1.0. Odometry alone still drifted to 3.7 m at the goal from random noise alone. The EKF barely changed.
4. Noisier fixes. Set FIX_SIGMA = 0.30. The EKF RMS error rose to 0.140 m. Then try a larger margin to restore a positive gap.
5. A dishonest filter. Make the filter believe the wheels are 10 times better than they are by using ODO_SIGMA / 10 inside ekf_predict only. In our run the 95 % ellipse figure fell from 95.3 % to 88.7 %, while the RMS error rose only to 0.066 m. The error barely changed, but the filter now claims more certainty than it has.
6. Smooth the path. A* paths on a grid zigzag. Drop every grid point that lies on a straight line between its neighbours, or shortcut with line-of-sight checks on the inflated grid, and compare the drive time.

Summary

• Inflate obstacles by the robot radius plus a margin for localisation error, then plan for a point. The margin is what keeps a correct estimate with 0.1 m of noise from hitting a wall.
• A* with the octile heuristic on an 8-connected grid returns a shortest grid path. Block diagonal steps that squeeze past corners.
• Pure pursuit steers along the arc to a point a fixed distance ahead. Curvature is 2 y_r / d².
• An EKF that predicts with noisy wheel odometry and updates with position fixes keeps the error near the fix noise. Here it stayed under 0.151 m over a 70 s drive while odometry alone drifted 5.6 m.
• Check filter consistency, not only error: the truth should fall inside the 95 % ellipse on about 95 % of steps.

Background: REP 105 for the map and odom frames, REP 103 for units and axis conventions, and the state estimation and motion planning topic hubs.