SLAM Algorithm Design for Autonomous Vehicle

To address the high computational complexity of iterative Extended Kalman Filters caused by multiple iterations, a Simultaneous Localization and Mapping algorithm based on single-iteration Invariant Extended Kalman Filter is proposed. By leveraging the properties of Lie groups and Lie algebras, the algorithm linearizes only rotational errors, significantly reducing linearization errors during the estimation process. The single-iteration update maintains high estimation accuracy while substantially improving computational efficiency. To mitigate Z-axis drift caused by cumulative errors, a sliding window Bundle Adjustment optimization is applied to poses in a voxel-based map, resulting in more accurate pose estimates. Experimental results demonstrate that the proposed method maintains high accuracy while significantly improving computational speed. Moreover, it effectively resolves Z-axis cumulative error issues, enhancing both system efficiency and stability.