Proposal: EKF-SLAM (Extended Kalman Filter - Simultaneous Localization and Mapping) #64
Description
Description
EKF-SLAM (Extended Kalman Filter based Simultaneous Localization and Mapping) is a foundational algorithm that enables a mobile robot to simultaneously build a map of unknown landmarks in its environment while localizing itself within that map in real time. Unlike running localization and mapping as separate modules, SLAM jointly estimates the robot's pose and all landmark positions in a single augmented state vector, capturing the critical correlations between them. As the robot re-observes landmarks, the uncertainty in both the robot's pose and the map collapses, a property known as loop closure. EKF-SLAM extends the repository's existing EKF localization framework by augmenting the state vector with landmark positions and updating the full covariance matrix upon each observation. It is the classical entry point to SLAM.
Plan for Implementation
1. State Vector Augmentation
2. Motion Model (Prediction Step)
3. Observation Model (Update Step)
4. Data Association
5. Landmark Management
6. Loop Closure Demonstration
7. Visualization
Expected Outcomes
- Real-time EKF-SLAM simulation with landmark-based mapping in 2D environments
- Demonstration of covariance convergence as landmarks are re-observed (loop closure)
- GIF animation showing robot exploring, mapping landmarks, and refining estimates over time
- Comparative analysis with standalone EKF localization (known map) to show the benefit and cost of joint estimation
- Performance metrics: landmark position RMSE, robot pose RMSE, state vector dimensionality growth, computation time per step
- Demonstration of data association robustness under varying sensor noise levels
- Unit tests for state augmentation, prediction step, update step, data association, and landmark initialization
Execution Structure and Pre-requisites
1. Pre-requisites
NumPy, Matplotlib, SciPy
2. File Structure
| File | Purpose |
|---|---|
ekf_slam.py |
Main EKF-SLAM algorithm (predict, update, augment) |
observation_model.py |
Range-bearing sensor simulation and Jacobians |
motion_model.py |
Odometry/velocity motion model and Jacobians |
data_association.py |
Nearest-neighbor matching with Mahalanobis distance |
landmark_manager.py |
State vector growth and new landmark initialization |
visualization.py |
Animated plotting of trajectory, landmarks, and covariance ellipses |
3. Parameters
Sensor noise (range σ_r, bearing σ_φ), process noise (linear σ_v, angular σ_ω), Mahalanobis gate threshold, maximum sensor range, number of landmarks, simulation duration
4. Test Scenarios
- Sparse landmark field with circular robot path (classic loop closure)
- Dense landmark field with random exploration
- High sensor noise to stress data association
- Long corridor with revisitation (demonstrating correlation-based updates)
5. Advantages
- Jointly optimal estimation of robot pose and map
- Loop closure dramatically improves global consistency
- Builds directly on existing EKF localization code in the repository
6. Integration
- Extends the existing
localization/extended_kalman_filtermodule - Can serve as the mapping backend for global path planners (e.g., A*, Dijkstra) by providing an incrementally built landmark map
References
- Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics, Chapter 10: EKF-SLAM