Localization: EKF SLAM

[kunj1302]

Summary

EKF-SLAM Component for 2D Landmark-Based Mapping

• Implements a clear Extended Kalman Filter (EKF) for joint estimation of:
  • Robot pose: [x, y, yaw]
  • Landmark positions: [l_x, l_y, ...]
• Uses standard textbook EKF prediction and update equations.
• Explicit motion and observation models:
  • predict_robot_state
  • observe_landmark
  • build_H_matrix
• Includes:
  • Angle normalization
  • Jacobian clamping for numerical stability
• Data association handled via nearest-neighbor matching with a Mahalanobis distance gate.
• Prevents duplicate matches within the same timestep using a simple landmark reuse guard.

---

Landmark Management

• New landmarks are initialized directly from range–bearing observations and current robot pose.
• State vector and covariance matrix are augmented in a single, transparent location.
• Cross-time duplicate detection:
  • Skips adding new landmarks if inferred positions lie within duplicate_position_threshold of existing landmarks.
  • Keeps the map compact and reduces spurious landmark duplication.

---

Visualization for Learning SLAM Behavior

• Robot follows a circular trajectory (one lap plus slight overshoot) to naturally revisit areas and demonstrate loop closure.
• Landmarks are randomly distributed with a minimum spacing constraint to maintain clarity and realism.
• Visualization includes:
  • True vehicle (blue)
  • Estimated vehicle (green)
  • Estimated landmarks
  • Pose uncertainty circle
• Final output provides a side-by-side comparison of true vs. estimated maps for qualitative evaluation of mapping accuracy.

---

Test Plan

Run the EKF-SLAM Simulation End-to-End

1. Execute:

   python ekf_slam.py

[ShisatoYano]

Motion model of the vehicle has already been implemented in the following state class.
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/blob/main/src/components/state/state.py
Please refactor your code by using the above existing motion model.

[ShisatoYano]

Please implement a sensor model to observe landmarks refer to the following existing sensor models code.
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/tree/main/src/components/sensors

[ShisatoYano]

Please implement a landmark class refer to the existing obstacle class.
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/tree/main/src/components/obstacle

[ShisatoYano]

This simulation code does not follow the existing simulation codes. Please refactor the implementation refer to these existing codes,
https://github.com/ShisatoYano/AutonomousVehicleControlBeginnersGuide/tree/main/src/simulations