Factor Graph-Based Smoothing Without Matrix Inversion for Highly Precise Localization

Abstract

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.

A Review of Device-Free Indoor Positioning for Home-Based Care of the Aged: Techniques and Technologies

“…The algorithm can effectively use the surrounding reference nodes to assist the end nodes to achieve higher accuracy positioning. Chauchat et al [90] used the algorithm to solve the problem of error accumulation formed during the iteration and the nonlinear optimization problems at hand in navigation.…”

Machine Learning Assists IoT Localization: A Review of Current Challenges and Future Trends

“…Xiong et al [ 67 ] presented a statistical iterative model based on factor graphs to replace the loss with under an impulsive noise for ToA-based localization. The authors of [ 68 ] looked at simultaneous localization and mapping (SLAM) for self-driving cars and proposed a Kalman filter (factor-graph-based solution) to improve the localization accuracy.…”

Simultaneous Localization and Mapping (SLAM) and Data Fusion in Unmanned Aerial Vehicles: Recent Advances and Challenges

“…S 12 and S 21 may be sparse or dense, depending on the specific observation data. The S matrix computation takes advantage of the sparsity [158]. Suppose there are two sensor datasets (D 1 , D 2 ) and five landmarks (L 1 , L 2 , L 3 , L 4 , L 5 ) in a scene.…”