Extended Kalman Filter SLAM from Scratch

Overview

In this project, I implemented Extended Kalman Filter Simultaneous Localization and Mapping (EKF SLAM) from scratch on a TurtleBot3 in simulation. In the videos below, three robots are displayed:

• Red - Ground truth
• Blue - Odometry
• Green - EKF SLAM estimate

Cylindrical obstacles are also displayed:

• Red - Ground truth
• Blue - Detected landmarks after performing clustering and circle fitting (shown in the second video)
• Green - Obstacles as detected by EKF SLAM

The following sections briefly explain the details of the project.

Simulator

To test different components of the project such as EKF SLAM algorithm and landmark detection, I used RViz as a visualization tool and created a simulated robot environment in a ROS 2 node. The simulation was used to mimic the movement of the robot and included cylindrical obstacles to test the landmark detection algorithm based on LiDAR data. Additionally, I implemented collision detection between the robot and the obstacles in the environment. The videos above are examples of this simulation.

Library

A library was developed in C++ to assist with various calculations that involve 2D rigid body transformations, differential drive kinematics, EKF algorithm, and circle fitting algorithm.

EKF SLAM

The main component of this project was to implement EKF SLAM, which consists of two steps: prediction and update. In the prediction step, new robot state and covariance are predicted iteratively using odometry estimates. In the update step, the predicted state is adjusted iteratively based on newly detected landmarks. This process involves computing the theoretical measurement, which is calculated from the predicted state, and the Kalman gain, which considers both the expected and actual locations of the landmark. The gain is then used to update the state and covariance accordingly. More details of the EKF SLAM algorithm can be found here.

Landmark Detection

To detect cylindrical obstacles using laser scan data, unsupervised learning (clustering) and supervised learning (circular regression) were employed. Initially, laser scanner points were grouped together based on a certain distance threshold, and any cluster with less than four points was disregarded. This step ensured that only clusters that represented real landmarks were retained. The subsequent task involved using a circle fitting algorithm to determine the best fit circle for each cluster. Finally, radius filtering was performed to classify the clusters into “circle” or “not circle.” If the radius of the circle that fit the cluster was too big or too small, then that cluster was discarded.