Implemented an EKF SLAM algorithm from scratch using C++ and ROS 2 for a turtlebot3 robot. A simulation environment was developed in Rviz2 to test the algorithm.

Overview

This project involves the implementation of an Extended Kalman Filter (EKF) Simultaneous Localization and Mapping (SLAM) algorithm from scratch using C++ and ROS 2. The algorithm is tested in a simulation environment in Rviz2 using a turtlebot3 robot. The robot is equipped with a LIDAR sensor that provides range and bearing measurements to landmarks in the environment. The EKF SLAM algorithm estimates the robot’s position and the position of the landmarks in the environment using these sensor measurements. The estimated robot position is compared to the ground truth position to evaluate the performance of the algorithm.

• Red robot represents the ground truth
• Blue robot represents the odometry
• Green robot represents the estimated position of the robot using the EKF SLAM algorithm.
• Green landmarks represents the estimated position of the obstacles using the EKF SLAM algorithm.
• Yellow obstacles represent the simulated sensor data

Algorithm description

The EKF SLAM algorithm consists of two main steps: the prediction step and the update step. In the prediction step, the algorithm predicts the robot’s position based on the odometry data. In the update step, the algorithm updates the robot’s position and the position of the landmarks in the environment based on the sensor measurements. The algorithm uses the Extended Kalman Filter to estimate the robot’s position and the position of the landmarks in the environment.

Here is the algorithm in a nutshell:

1. Update the current state estimate using the odometry data
2. Update the estimated state from re-observing landmarks.
3. Add new landmarks to the current state.

Implementation

The project is made up of several components:

nuturtle_description: Contains the URDF description and visulization code for the turtlebot3 robot.

nusim: The nusim package provides a simulated robot environment for visualization of turtlebot3 robots in Rviz2. It is capable of creating stationary walls and tracking the position of a robot. The arena walls and obstacles display as markers in Rviz2. It offers services like teleport which allows the user to move the robot to a desired pose and reset to reset the simulation to the initial conditions.

nuturtle_control: The package provides operations responsible to control the turtlebot in real world and simulation. It includes three nodes : Circle ,Turtle_control, Odometry. Circle is responsible to make the robot move in a circle at a set velocity and radius. Turtle_control is responsible of converting twsit messages to wheel commands. The odometry node is responsible for providing odometry calculations.

nuslam: This packge implements a Feature-Based Kalman Filter SLAM.

It also contains a turtlelib library which is a 2D geoemtry library responsible for handling transformations in SE(2) and contains classes and functions to calculate the kinematics of a differential drive robot.

Results

SLAM with collision

The picture below shows the robot’s path when it encounters a collision. The red robot and the green robot are estimated to be at the same position. The blue robot representing the odometry is far from the ground truth, as seen in the picture. The EKF SLAM algorithm is able to estimate the position of the robot even when the odometry is not reliable.

Simultaneous Localization and Mapping (SLAM) with collision

The following video shows the EKF SLAM algorithm in action when the robot encounters a collision. The visualization shows the robot’s path, the estimated position of the robot, and the estimated position of the landmarks in the environment. Noise and wheel slip are introduced to simulate real-world conditions. The EKF SLAM algorithm is able to estimate the robot’s position and the position of the landmarks in the environment accurately. This is demonstrated by the green robot and the green landmarks closely following the red robot and the yellow obstacles, respectively.