SLAM: Extended Kalman Filter with Landmark Detection

A C++ library was meticulously crafted to streamline calculations involving 2D rigid body transformations, differential drive kinematics and other dead reckoning algorithms. This library serves as a crucial utility throughout the project. Central to the project's functionality, dedicated helper libraries were engineered from scratch in C++ to adeptly manage calculations associated with odometry, Extended Kalman Filter calculations, and various other tasks. These libraries are readily accessible within the turtlelib directory of the repository.

The project's core focused on applying the extended Kalman filter (EKF) to achieve feature-based simultaneous localization and mapping (SLAM) – EKF-SLAM. This algorithm involved a three-step process: initialization, prediction, and update. During each timestep, the algorithm leveraged both odometry and sensor measurements to estimate the robot's state and landmarks' positions. The prediction phase entailed refining the state estimate and propagating uncertainty via a linearized state transition model. Subsequently, the update step entailed the computation of theoretical measurements, the Kalman gain, and subsequent state and covariance updates. This central component was implemented meticulously. It encompassed two pivotal steps: prediction and update. The prediction phase iteratively anticipated new robot states and covariance, based on odometry inputs. In contrast, the update step progressively adjusted the predicted state, emphasizing newly identified landmarks. This intricate process calculated theoretical measurements from the projected state, with the Kalman gain crucially considering both anticipated and actual landmark locations. The gain played a pivotal role in fine-tuning the state and covariance to enhance accuracy and refine mapping.

Learn more about Extended Kalman Filter algorithm here.

For the detection of cylindrical landmarks utilizing laser scan data, a comprehensive approach amalgamating unsupervised and supervised learning was deployed. This process involved two distinct stages: clustering and circle fitting. In the initial phase, laser scanner points were judiciously grouped based on a predetermined distance threshold. Notably, clusters containing fewer than four points were excluded, ensuring only significant clusters representing genuine landmarks were retained. Following this step, a circle fitting algorithm was harnessed to deduce the optimal-fit circle for each cluster. Subsequently, a critical aspect involved radius filtering, which effectively categorized clusters as either "circle" or "not circle." Any cluster with a circle fit radius deemed excessively large or small was dismissed, thus enhancing the accuracy of landmark identification. Overall, this approach fused the power of unsupervised and supervised learning methods, offering a robust methodology for identifying and categorizing cylindrical landmarks using laser scan data.