Convert pose-only EKF + occupancy grid into full EKF-SLAM (joint pose + landmark/map estimation)

[AlexBarbi]

Summary

Currently, per the estimation/ module, the EKF is used exclusively for pose estimation (prediction from odometry, scan-matching pseudo-measurement update, and loop-closure correction), while the map is built separately as a passively-updated occupancy grid (log-odds, Bresenham ray tracing) used only for collision checking.

This issue proposes converting this into a true EKF-SLAM formulation, where the map (landmarks) is estimated jointly with the robot pose in a single augmented state vector with full pose-landmark and landmark-landmark covariance, rather than treated as a separate, decoupled data structure.

Motivation

• The current design treats localization and mapping as independent problems, which forfeits the core benefit of SLAM: using observed landmarks to correct drift and using the correlated uncertainty between pose and landmarks to improve both.
• Loop-closure currently only corrects the pose; in a real EKF-SLAM it should also tighten the uncertainty of all landmarks observed along the closed loop.
• A joint formulation would give more principled uncertainty estimates for landmarks used in navigation/APF obstacle avoidance and in relative peer measurements (V2V), rather than relying on an occupancy grid with no probabilistic linkage to the pose covariance.

Proposed Changes

1. State augmentation: Extend the EKF state vector x = [x_r, y_r, θ_r] to x = [x_r, y_r, θ_r, x_l1, y_l1, ..., x_lN, y_lN], with the corresponding block-structured covariance matrix Σ.
2. Landmark extraction: Add a feature-extraction front-end (e.g. corner/line detection or clustering) on the LiDAR scan to produce landmark observations, replacing (or complementing) the raw grid-based scan matcher currently used for the pseudo-measurement update.
3. Data association: Implement nearest-neighbor / Mahalanobis-distance gating (or JCBB) to associate new observations with existing landmarks vs. spawning new ones.
4. EKF update step: Replace/extend the current scan-matching correction with the standard EKF-SLAM measurement update (range-bearing or point-based Jacobians H) that updates both robot pose and landmark estimates jointly.
5. Loop-closure integration: Update loop-closure so that, on a valid match, the correction propagates through the full joint covariance (correcting past/linked landmarks), not just the current pose.
6. Occupancy grid role: Keep the occupancy grid as a secondary, derived product (for collision checking / APF), but populate or refine it from the EKF-SLAM landmark map rather than as a fully independent estimator.
7. Multi-robot / V2V compatibility: Verify that peer-relative measurements folded into the EKF (per the V2V cooperative localization module) remain consistent with the augmented state size, and that per-robot landmark maps stay computationally tractable at swarm scale (this is likely the main risk — full EKF-SLAM covariance grows O(N²) with landmark count).

Acceptance Criteria

• EKF state vector includes landmark positions with joint pose-landmark covariance.
• Landmark extraction + data association implemented and tested on the existing LiDAR sensor model.
• Loop-closure correction demonstrably reduces landmark uncertainty, not just pose uncertainty (add to test/ with a metric similar to the existing |Σ| / RMSE plots in docs/media/ekf_slam.png).
• Occupancy grid still available for collision checking, now derived from / consistent with the estimated landmark map.
• No regression in real-time performance for the swarm scenario (test.test_simulation), or a documented tradeoff if landmark count needs capping/pruning.
• README EKF-SLAM section updated to reflect the new joint estimation architecture.