Project Title & GitHub Link

Extended Kalman Filter (EKF) SLAM from Scratch

Project

Intro

This project showcases an implementation of Extended Kalman Filter (EKF) SLAM from scratch, using C++. The SLAM pipeline is designed as a series of ROS packages, allowing for simulation in Rviz and compatibility with the Turtlebot3 platform, utilizing its onboard LiDAR sensor.

Turtlebot3 Kinematics:

The project incorporates the differential drive forward and inverse kinematics necessary for the Turtlebot3's movement. The robot's motion is visualized in Rviz, where a world with cylindrical obstacles is created for the Turtlebot3 to detect and navigate. All parameters related to the obstacles and the robot are easily configurable through ROS parameters.

SLAM Pipeline:

1. LiDAR Data Clustering: The SLAM pipeline starts by clustering the LiDAR distance data based on proximity. This step helps identify distinct objects in the environment.
2. Circle Fitting: The points within each cluster are then fitted to a circle using a Gaussian process regression algorithm. This approach allows for accurate detection of cylindrical obstacles in the scene. The location and radius of the fitted circle are used to classify the object as an “obstacle” or “not obstacle.”
3. Data Association: To determine whether a detected obstacle corresponds to a previously discovered one or should be considered as a new measurement, the Mahalanobis distance is calculated between the current measurement and the existing obstacles in the Kalman Filter.
4. Extended Kalman Filter: The EKF utilizes the Turtlebot3’s velocity to predict its future state. The Kalman gains, covariance matrix, and state vector are then updated accordingly based on the associated measurements.

Robustness to Noise

When noise is introduced to the LiDAR data, wheel velocity, and wheel slip, the implemented EKF SLAM algorithm demonstrates superior performance in estimating the robot's state compared to using odometry alone. In the accompanying video, the green robot represents the SLAM's estimate, the blue robot represents the odometry-based estimate, and the red robot represents the actual position of the Turtlebot3.

Conclusion:

This project highlights the effectiveness of implementing an Extended Kalman Filter (EKF) SLAM algorithm for robust localization and mapping in the presence of sensor noise and wheel slip. The integration with ROS and compatibility with the Turtlebot3 platform demonstrate the practicality and adaptability of the implemented solution.

For more details and access to the source code, please visit the project's GitHub repository.