EKF SLAM：TurtleBot3 实体机器人实现EKF SLAM on a Real TurtleBot3

Real Robot Demo

The robot drives a circuit and returns to its starting position. The green robot is the SLAM estimate, the blue robot is raw odometry.

Overview

This project, built for Northwestern University’s ME495 Sensing, Navigation and Machine Learning for Robotics (Winter 2025), implements a full EKF SLAM pipeline on a real TurtleBot3 — including landmark detection, odometry, and a two-machine ROS 2 architecture.

Key Results:

SLAM reduces position error from 65.7 cm (odometry) to 1 mm — a 657× improvement.

How It Works

The system fuses odometry with LiDAR landmark observations to simultaneously estimate the robot’s pose and landmark positions as it drives.

Landmark Detection: LiDAR Clustering and Circle Fitting

LiDAR scan data is processed to identify cylindrical landmarks in the environment:

1. Clustering: Adjacent scan points separated by more than an angular gap threshold are split into different clusters
2. Circle fitting: Umeyama circle fitting is applied to each cluster to estimate center $(x_c, y_c)$ and radius $r$
3. Filtering: Clusters with radii or sizes outside expected ranges are rejected (e.g., wall reflections)

The fitted circle centers provide relative landmark positions, which feed directly into the EKF update step.

Extended Kalman Filter (EKF)

The EKF maintains a joint state vector $\mathbf{x} = [q^T, m^T]^T$, where $q = (\theta, x, y)$ is the robot pose and $m$ is the list of landmark coordinates.

Prediction step (using odometry):

Update step (using landmark observations):

Unknown Data Association

Every time a landmark is observed, the system must decide whether it matches a known landmark or is new. This project uses Mahalanobis distance for matching:

If the minimum Mahalanobis distance falls below a threshold, the observation is associated with that landmark. Otherwise, it is initialized as a new landmark and added to the map. This allows the system to operate without knowing the number or positions of landmarks in advance.

Simulation

Known Data Association (Simulation)

Color coding:

• Red: Ground truth trajectory
• Green: SLAM estimated trajectory
• Blue: Raw odometry trajectory
• Yellow circles: True landmark positions
• Red circles: SLAM-estimated landmark positions

Unknown Data Association (Simulation)

Simulation runs in the nusim virtual environment with configurable walls and cylindrical obstacles, publishing noisy encoder data and a fake LiDAR scan.

System Architecture

Two-Machine ROS 2 Deployment

The system runs distributed across two machines:

TurtleBot3 (onboard)                   PC (remote)
├── Hardware drivers                   ├── EKF SLAM node
├── Landmark detection (circle fit)    ├── RViz visualization
└── wheel_cmd / joint_states           └── Global pose estimate
         └──────── WiFi ────────────────┘

On the TurtleBot:

ros2 launch nuslam turtlebot_bringup.launch.xml

On the PC:

ros2 launch nuslam pc_bringup.launch.xml

Package Structure

turtlelib: Core Geometry Library

turtlelib is a custom C++ library used throughout the system, providing:

• SE(2) rigid body transforms: Composition and inversion of rotation matrices and translation vectors
• Twist/Wrench operations: Transformation of velocity twists between frames
• Differential drive kinematics: Forward/inverse kinematics, wheel speed to body velocity mapping

Hardware

Software Stack

Acknowledgement

This project was completed as the final assignment for ME495 Sensing, Navigation and Machine Learning for Robotics at Northwestern University, under the instruction of Professor Matthew Elwin. The course provided the foundational framework and hardware infrastructure that made this work possible.