EKF SLAM for TurtleBot3

Overview

This project is a full ROS2 SLAM stack for a TurtleBot3 navigating around cylindrical landmarks. It combines a custom 2D simulator, wheel-encoder odometry, lidar-based landmark detection, and an Extended Kalman Filter that estimates both the robot pose and landmark map online. In RViz, the red robot is simulation ground truth, the blue robot is raw odometry, and the green robot is the SLAM-corrected estimate.

System Architecture

The stack is split into reusable layers: a non-ROS math library, robot control and odometry nodes, a simulator, and SLAM nodes. Each layer communicates over normal ROS2 topics so the same logic can run in simulation or on the real TurtleBot3.

• turtlelib: C++ library for SE(2) transforms, 2D geometry, and differential-drive forward/inverse kinematics.
• nusim: Simulator that publishes ground truth, encoder readings, fake landmark observations, lidar scans, arena walls, and cylindrical obstacles.
• nuturtle_control: Converts velocity commands to motor commands, converts encoder ticks to joint states, and computes odometry.
• nuslam: Detects landmarks from lidar and runs EKF SLAM with either known or unknown data association.

cmd_vel
  |
  v
turtle_control --> wheel_cmd --> nusim / TurtleBot hardware
  ^                                      |
  |                                      v
joint_states <-- sensor_data <-- wheel encoders
  |
  v
odometry --> /odom --> EKF predict

/scan --> landmarks --> detected cylinders --> EKF update
                                      |
                                      v
                             map -> odom TF + landmark map

Robot Motion and Odometry

The robot is modeled as a differential-drive system. Wheel commands are converted into left/right wheel velocities, encoder ticks are converted back into wheel angles, and forward kinematics integrates those wheel angle changes into odometry. This creates the blue robot estimate in RViz.

left_encoder, right_encoder
  |
  v
wheel angles in radians
  |
  v
delta left/right wheel motion
  |
  v
body twist: [omega, vx, 0]
  |
  v
integrate SE(2) transform
  |
  v
odom -> base_footprint

Landmark Detection from Lidar

The landmark detector turns raw LaserScan ranges into cylindrical landmark detections. It first converts scan rays into Cartesian points in the scan frame, clusters neighboring points, fits circles to candidate clusters, and rejects bad fits before publishing the landmark positions to SLAM.

• Wrap-around clustering handles objects split across the beginning and end of the laser scan.
• Circle fitting uses an algebraic regression method backed by Armadillo linear algebra.
• Filters reject clusters that are too small, too large, too noisy, or not circular enough.

LaserScan ranges
  |
  v
polar -> Cartesian points
  |
  v
cluster consecutive nearby points
  |
  v
fit circle to each cluster
  |
  v
filter by radius, RMSE, and circularity
  |
  v
publish detected landmark centers

Sensor Strategy Comparison

Fake Sensor Landmarks

The simulator can publish landmark observations with known IDs. This is useful for validating the EKF math because it removes the data-association problem and lets the filter focus only on prediction and correction.

Lidar Detected Landmarks

The real SLAM pipeline uses only detected landmark positions from the laser scanner. This is harder because the robot has to decide whether a detection is a previously seen landmark or a new one.

EKF SLAM Loop

The EKF state stores the robot pose followed by each landmark position: [x, y, theta, m1_x, m1_y, m2_x, m2_y, ...]. Odometry drives the predict step, and landmark observations drive the update step.

1. 1Prediction keeps the estimate moving even when landmarks are not visible.
2. 2Correction pulls the estimate back toward observed landmarks when detections arrive.
3. 3Map-to-odom TF expresses the difference between drifting odometry and the corrected SLAM pose.

For every odometry update:
  1. Compute odom-frame delta from the previous odom pose.
  2. Rotate that delta into the robot body frame.
  3. Predict the robot pose with the motion model.
  4. Grow covariance based on movement uncertainty.

For every landmark observation:
  1. Transform the observation into the current map frame.
  2. Match it to the nearest known landmark or create a new one.
  3. Compute expected observation from the current EKF state.
  4. Apply the Kalman update to correct robot pose and landmark map.
  5. Publish map -> odom so the green robot shows the corrected pose.

Data Association

Known Data Association

Known data association uses simulator-provided landmark IDs. This version is the controlled experiment: if the EKF still behaves badly here, the issue is likely in the filter math, motion model, covariance, or TF publishing rather than the perception system.

Unknown Data Association

Unknown data association projects each detected landmark into the current map frame and compares it against the existing map. If the nearest landmark is within a distance threshold, the observation updates that landmark. Otherwise, the EKF initializes a new landmark.

detected landmark in robot frame
  |
  v
transform into map frame using current SLAM pose
  |
  v
compare distance to all known landmarks
  |
  +-- nearest distance <= threshold --> update existing landmark
  |
  +-- nearest distance > threshold  --> initialize new landmark

Simulation Results

In simulation, SLAM strongly outperformed wheel odometry after driving around the landmark field and returning near the start. The blue odometry robot drifted visibly, while the green SLAM robot stayed locked to the landmark map.

• Ground truth final pose: x=0.037 m, y=0.827 m, theta=0.369 rad.
• Odometry final pose: x=-0.380 m, y=1.194 m, theta=0.368 rad.
• SLAM final pose: x=0.037 m, y=0.824 m, theta=0.368 rad.
• Position error: odometry 0.556 m vs SLAM 0.003 m.

Odometry vs SLAM

The result is a direct comparison between dead reckoning and landmark-corrected localization. Odometry gives a smooth short-term estimate, but SLAM wins over longer paths because it can use landmarks to remove accumulated drift.

Real Robot Deployment

The same SLAM pipeline was deployed on a physical TurtleBot3. The robot-side launch brought up the hardware interface, wheel control, odometry, laser driver, landmark detector, and SLAM. The PC-side launch handled RViz and teleoperation.

• Real robot odometry final pose: x=0.741 m, y=-1.224 m, theta=-0.879 rad.
• Real robot SLAM final pose: x=1.023 m, y=-0.539 m, theta=1.254 rad.
• SLAM returned closer to the origin than raw odometry on the real robot run: 1.16 m vs 1.43 m.

TurtleBot:
  hardware interface
  laser driver
  turtle_control
  odometry
  landmarks
  slam_uda

Workstation:
  RViz
  teleop or circle command source

Key Technical Challenges

• Maintaining a consistent TF tree across ground truth, odometry, and corrected SLAM frames.
• Handling wheel-slip and encoder drift while keeping the EKF process noise stable.
• Tuning landmark clustering and circle-fit thresholds so lidar detections work in both simulation and real-world scans.
• Choosing a simple but effective unknown data association strategy that avoids repeatedly creating duplicate landmarks.
• Keeping the green SLAM robot visually stable while still applying meaningful map corrections.

Quickstart

# Landmark detection only
ros2 launch nuslam landmark_detect.launch.xml robot:=nusim

# EKF SLAM with unknown data association in simulation
ros2 launch nuslam unknown_data_assoc.launch.xml cmd_src:=teleop

# Real robot split deployment
ros2 launch nuslam turtlebot_bringup.launch.xml
ros2 launch nuslam pc_bringup.launch.xml cmd_src:=teleop