Learn SLAM

A 2-D EKF-SLAM system built from scratch using C++, Python, ROS2, and RViz2.

Built as a learning project to understand SLAM internals by implementing every step — motion prediction, landmark extraction, data association, Kalman update — explicitly.

---

Algorithm

EKF-SLAM (Extended Kalman Filter SLAM) with point landmarks.

Component	What it does
State vector	[x_r, y_r, θ_r, x_L0, y_L0, ..., x_LN, y_LN]ᵀ — robot pose + all landmark positions in one vector
Prediction	Differential-drive dead-reckoning model with analytical Jacobian
Landmark extraction	Jump-distance clustering on LiDAR scan → point landmark centroids
Data association	Mahalanobis-distance nearest-neighbour with χ² gating
Update	Standard EKF innovation/gain/correction cycle with bearing angle wrapping
New landmarks	Inverse observation model + covariance augmentation

Full mathematical derivation: docs/ekf_math.md

---

Package Structure

slam_ws/src/
├── slam_interfaces/   Custom ROS2 messages (Landmark, LandmarkArray) and services (ResetSlam)
├── slam_ekf/          C++ EKF-SLAM core: ekf_slam, landmark_extractor, data_association, slam_node
├── slam_simulator/    Python: simulated differential-drive robot + 2-D LiDAR
└── slam_bringup/      Python: launch files, YAML config, RViz config, keyboard teleop

System architecture and topic/TF reference: docs/architecture.md

---

Prerequisites

# ROS2 Humble (Ubuntu 22.04) or later
source /opt/ros/humble/setup.bash

# Eigen3 for C++ linear algebra
sudo apt install libeigen3-dev

# xterm for the keyboard teleop pop-up window
sudo apt install xterm

---

Build

cd ~/Repos/Learn-SLAM/slam_ws
colcon build --symlink-install
source install/setup.bash

---

Run

# Full system: simulator + EKF-SLAM + RViz2 + keyboard teleop
ros2 launch slam_bringup slam_full.launch.py

# Simulator only (check /scan and /odom without EKF)
ros2 launch slam_bringup sim_only.launch.py

# EKF only (for rosbag replay)
ros2 launch slam_bringup ekf_only.launch.py

Keyboard controls (in the xterm window):

Key	Action
W / S	Speed up / slow down
A / D	Turn left / right
Space	Full stop
Q	Quit teleop

---

What You'll See in RViz2

Element	Description
Orange squares	Live 2-D LiDAR scan
Blue arrow + ellipse	EKF estimated robot pose with covariance
Blue line	EKF trajectory
Green cylinders	Estimated landmark map (grows over time)
Light green ellipses	1-σ covariance around each landmark
Orange spheres	Extracted landmark observations (current scan)

---

Useful Commands

# Print live landmark estimates
ros2 topic echo /slam/landmarks

# Reset the SLAM state (clear map, restart from origin)
ros2 service call /slam/reset slam_interfaces/srv/ResetSlam

# Record a bag file
ros2 bag record /scan /odom /ground_truth/odom -o my_run

# Replay through EKF only
ros2 launch slam_bringup ekf_only.launch.py &
ros2 bag play my_run

---

Tuning Parameters

Edit slam_ws/src/slam_bringup/config/ekf_params.yaml:

Parameter	Default	Effect
sigma_v	0.05 m/s	Trust in odometry linear velocity
sigma_omega	0.02 rad/s	Trust in odometry angular velocity
sigma_range	0.02 m	LiDAR range noise level
sigma_bearing	0.01 rad	LiDAR bearing noise level
mahalanobis_threshold	5.99	Data association gate (chi² 95%)
jump_threshold	0.30 m	Range gap to split scan clusters
max_landmarks	50	Cap on map size

Larger sigma_v/sigma_omega → algorithm trusts sensors more over odometry. Smaller mahalanobis_threshold → more conservative association (fewer false matches, more new landmarks).