Mobile robot algorithms: localization, planning & learning

Overview

Three connected projects from an AI and ML for robotics course, each tackling one piece of the autonomy stack on a differential-drive robot. All are validated on the University of Toronto Institute for Aerospace Studies Multi-Robot Cooperative Localization and Mapping (UTIAS MRCLAM) single-robot dataset.

---

Unscented Kalman Filter (UKF) localization

Source: github.com/flojule/UKF

The UKF fuses body twist commands (prediction) with range-and-bearing measurements to known landmarks (correction) to maintain a pose estimate $(x, y, \theta)$.

• Predict: a unicycle motion model propagates sigma points forward, accounting for wheel-velocity process noise.
• Correct: visible landmark measurements pull the estimate toward ground truth, weighted by sensor noise.

Dead reckoning diverges quickly from orientation drift; the UKF tracks ground truth across the run (avg. position error 0.107 m, avg. bearing error 0.049 rad).

---

A* path planning and motion control

Source: github.com/flojule/search-a-star

A* search on a grid map plus a proportional motion controller that drives the robot along the resulting waypoints.

Path planning. A* uses a Chebyshev distance heuristic (admissible for 8-directional movement). Tie-breaking on fewest direction changes plus octile distance produces cleaner paths. Two grid resolutions compared: coarse (1 m/cell) and fine (0.1 m/cell with the robot footprint inflated around landmarks). An online variant replans from scratch as the robot reveals neighbors — coarse grids work better here due to fewer waypoints and direction changes.

Motion controller. Proportional control on linear velocity $v = K_v \cdot d$ and angular velocity $\omega = K_\omega \cdot \Delta\theta$, with $v$ scaled by $\cos(\Delta\theta)$ to avoid moving away from the target while turning. A collision-avoidance layer reorients on occupied next-cells. Best gains from a sweep: $K_v = 2$, $K_\omega = 5$ — 24 s completion, low overshoot.

---

Learned motion model with Locally Weighted Linear Regression (LWLR)

Source: github.com/flojule/LWLR

LWLR replaces the analytical unicycle motion model with a data-driven one. Given a control input $(v, \omega)$ and the previous pose, it predicts the resulting pose change by solving a weighted least-squares problem in which each training sample is weighted by similarity to the query in control space.

This non-parametric approach captures systematic effects — wheel slip, asymmetry — that the closed-form unicycle model cannot represent. Evaluated against the analytical model on held-out odometry sequences.