Skills - EKF SLAM, Unsupervised Learning, Odometry, ROS, TF, URDF, C++, Unit Testing, CMake
This repository builds feature-based EKF SLAM on Turtlebot3 from scratch. The demo below shows the algorithm in action (2x speed).
- The pink path shows the odometer estimated path.
- The green path is the groundtruth.
- Yello path is the SLAM result.
- Blue landmarks are groundtruth.
- Green landmarks are measurements.
- Indigo landmarks show where the SLAM algorithm thinks their positions are.
The figure below shows the result of the landmark detection algorithm using a 2D laser scanner.
This system has the following major components:
- A 2D Lie Group library for differential drive robots with complete unit testing
- A waypoint following feedback controller
- Turtlebot3 URDF built from scratch for Gazebo simulation
- Gazebo plugins to control the robot and return the groundtruth data for evaluation
- An odometer that estimates robot states based on encoder reading
- Turtlebot3 interface that controls the motors with given velocity command
- Feature detection algorithm that identifies landmarks using a 2D laser scanner
- EKF SLAM algorithm that estimates robots states
Six packages are built in this project.
nuturtle_descriptiondevelops Turtlebot3 URDF, and visualizes the wheeled robot in
nuturtle_gazeboincludes Gazebo plugins to simulate the robot in
nuturtle_robotimplements the Turtlebot3 interface, and includes the test node for the odometer on the real robot
nuturtle_slamconsists of the feature detection algorithm and the EKF SLAM algorithm
tsimimplements the waypoints following feedback controller
rigid2dis the 2D Lie Group library, including SO(2), SE(2) calculations, the odometer, and the fake encoder
The video below shows the testing of the odometer in the real world.
Landmarks in the environment are cylinders. Therefore, the feature detection pipeline includes points clustering, circle fitting, and circle classification. The 2D laser scanner results are clustered into groups based on a distance threshold. Clusters with fewer than 4 points are then discarded to enhance the robustness of the circle fitting algorithm.
A circle fitting algorithm  was implemented to find the position and radius of landmarks. Circles identified with too large radius are removed. A circle classification algorithm  was also applied to help avoid false positives.
An extended-Kalman-filter is implemented. Kalman filter has two steps, a prediction step, and a correction step. The prediction step takes the motion commands into account to predict the robot's position in the current time step. The correction step then compares the robot's actual observations with the predicted observations to correct the robot state. The observations, in this case, are all the detected cylindrical landmark positions. Because the model is nonlinear, the extended Kalman filter is used to linearize the dynamics locally by a Taylor expansion.
- Data association is currently assumed to be known. SLAM with unknown data association can be achieved by calculating the Mahalanobis Distance
 A. Al-Sharadqah and N. Chernov, Error Analysis for Circle Fitting Algorithms, Electronic Journal of Statistics (2009), Volume 3 p 886-911
 J. Xavier et al., Fast line, arc/circle and leg detection from laser scan data in a Player driver, ICRA 2005