We consider the problem of localizing a manned, semi-autonomous, or autonomous vehicle in the environment using information coming from the vehicle's sensors, a problem known as navigation or simultaneous localization and mapping (SLAM) depending on the context. To infer knowledge from sensors' measurements, while drawing on a priori knowledge about the vehicle's dynamics, modern approaches solve an optimization problem to compute the most likely trajectory given all past observations, an approach known as smoothing. Improving smoothing solvers is an active field of research in the SLAM community. Most work is focused on reducing computation load by inverting the involved linear system while preserving its sparsity. The present paper raises an issue which, to the knowledge of the authors, has not been addressed yet: standard smoothing solvers require explicitly using the inverse of sensor noise covariance matrices. This means the parameters that reflect the noise magnitude must be sufficiently large for the smoother to properly function. When matrices are close to singular, which is the case when using high precision modern inertial measurement units (IMU), numerical issues necessarily arise, especially with 32-bits implementation demanded by most industrial aerospace applications. We discuss these issues and propose a solution that builds upon the Kalman filter to improve smoothing algorithms. We then leverage the results to devise a localization algorithm based on fusion of IMU and vision sensors. Successful real experiments using an actual car equipped with a tactical grade high performance IMU and a LiDAR illustrate the relevance of the approach to the field of autonomous vehicles.

中文翻译：

---

基于因子图的平滑无矩阵求逆，用于高精度定位

我们考虑使用来自车辆传感器的信息在环境中定位有人驾驶、半自主或自主车辆的问题，这个问题被称为导航或同步定位和映射 (SLAM)，具体取决于上下文。为了从传感器的测量推断知识，同时利用关于车辆动力学的先验知识，现代方法解决了一个优化问题，以计算给定所有过去观察的最可能的轨迹，这种方法称为平滑。改进平滑求解器是 SLAM 社区中一个活跃的研究领域。大多数工作都集中在通过反转所涉及的线性系统同时保持其稀疏性来减少计算负载。本文提出了一个问题，据作者所知，尚未解决：标准平滑求解器需要明确使用传感器噪声协方差矩阵的逆矩阵。这意味着反映噪声幅度的参数必须足够大，平滑器才能正常运行。当矩阵接近奇异矩阵时（使用高精度现代惯性测量单元 (IMU) 时就是这种情况），必然会出现数值问题，尤其是大多数工业航空航天应用要求的 32 位实现。我们讨论了这些问题并提出了一种基于卡尔曼滤波器的解决方案，以改进平滑算法。然后，我们利用结果设计基于 IMU 和视觉传感器融合的定位算法。