EKF SLAM from Scratch

A full-featured 2D physics simulator that acts as a stand-in for the real robot, providing ground truth state while injecting realistic sensor imperfections.

• Gaussian input noise — Zero-mean noise is added to non-zero commanded wheel velocities, modeling motor imprecision.
• Wheel slip — Uniform random slip is applied independently to each wheel, causing the reported encoder positions to diverge from the true wheel motion.
• Collision detection — The robot is represented as a circle. On intersection with a cylindrical obstacle, the robot is pushed to a tangent position while the wheels continue spinning simulating the real behavior of a robot bumping into an obstacle.
• Simulated LiDAR — Ray-casting against obstacle cylinders and arena walls at 5 Hz, with configurable noise, range, and angular resolution matching the real TurtleBot 3 LiDAR sensor.
• Fake sensor — Publishes noisy relative (x, y) measurements of landmarks with unique IDs, used for known-data-association testing during SLAM development.

A three-stage pipeline converts raw 2D LiDAR scan data into detected landmark positions, enabling SLAM to run without pre-placed fiducials or sensor tags.

• Clustering — Consecutive scan points within a distance threshold are grouped into clusters. The scan wraps around, so clusters spanning the 0°/360° boundary are handled explicitly. Clusters with fewer than 3 points are discarded.
• Circle regression — The Hyper circle-fitting algorithm fits a circle to each cluster using algebraic least squares with SVD decomposition.
• Radius filtering — Fitted circles outside the expected obstacle radius range (1–10 cm) are rejected, preventing walls and other large features from being classified as landmarks.

The core SLAM algorithm maintains a joint state estimate of the robot pose and all landmark positions, fusing odometry predictions with landmark observations using an Extended Kalman Filter.

• Predict step — On each odometry update, the robot's pose estimate is propagated forward using the DiffDrive forward kinematics. The covariance matrix grows, reflecting increased uncertainty between sensor updates.
• Update step — Each detected landmark measurement triggers a linearized Kalman update. The Jacobian of the measurement model is computed analytically and used to correct both the robot pose and the landmark positions simultaneously.
• Known data association — Validated first using the simulator's fake_sensor topic, which provides landmark IDs directly. Driving into obstacles to corrupt odometry confirmed the filter's ability to recover correct landmark positions even under large drift.
• Unknown data association — Extended to real LiDAR data using Mahalanobis distance gating. New observations that fall outside the gate distance for all known landmarks are initialized as new landmarks in the map.
• ROS REP 105 compliance — Publishes a map → odom transform such that the full map → odom → base_footprint chain reflects the SLAM-corrected robot pose.