Demo

Overview

This project builds feature-based EKF SLAM on a TurtleBot3 from scratch. Five ROS 2 packages make up the system:

• turtlelib — Pure C++ library for SE(2) transforms, differential drive kinematics, and the EKF. No ROS dependency; fully unit-tested with Catch2.
• nusim — 2D physics simulator with Gaussian wheel noise, wheel slip, lidar ray-casting, and collision detection.
• nuturtle_description — Multi-colour TurtleBot3 URDF with a parametric diff_params.yaml for wheel radius, track width, and collision geometry.
• nuturtle_control — Turtle interface node, odometry node, and a circle-driving node.
• nuslam — Landmark detection pipeline and EKF SLAM node.

Algorithms

Odometry

The odometry node integrates wheel encoder deltas using the constant-curvature arc model from the DiffDrive class. It publishes nav_msgs/Odometry and broadcasts the odom → base_footprint TF, with an initial_pose service to reset the origin.

Landmark Detection

Landmarks are cylinders detected from 2D lidar scans in three stages:

1. Clustering — consecutive scan points within a distance threshold form a cluster; clusters with fewer than 4 points are discarded.
2. Circle fitting — the Hyper algebraic fit finds the centre and radius of each cluster. Clusters with implausible radii are rejected.
3. Arc classification — uses the inscribed angle theorem: for a true circular arc, the angle ∠P₁PP₂ is nearly constant across all points P. Clusters whose mean falls between 90°–135° with standard deviation below 0.15 rad are classified as circles.

EKF SLAM

The EKF maintains a joint state vector [θ, x, y, mx₁, my₁, …, mxN, myN] and a single covariance matrix over robot pose and all landmark positions simultaneously.

• Prediction — wheel encoder deltas propagate through the nonlinear motion model; the Jacobian is computed analytically.
• Correction — each detected landmark is matched to a known map entry by world-frame Euclidean distance, then used to correct the full state via a range-bearing Jacobian.
• Initialisation — on first sighting, a landmark is placed using the inverse measurement model and the EKF correction is skipped for that frame, avoiding a degenerate update with uninformative covariance.

TF Tree

Click to view full size