Visual-inertial slam using invariant extended Kalman filterfor autonomous navigation

Abstract

The study is devoted to the development and application of visual-inertial SLAM based on the iterated extended Kalman filter (IEKF) for autonomous navigation tasks of mobile platforms. Classical approaches based on the extended Kalman filter (EKF) have significant limitations: single linearization errors in irreversible states lead to the phenomenon of false observability. This causes artificial narrowing of covariance matrices and exponential accumulation of drift during complex maneuvers. To solve these problems, the work mathematically substantiates and implements a comprehensive approach that combines the inverse depth parameterization (IDP) for instantaneous initialization of landmarks and the apparatus of the theory of SE2(3) Lie matrix groups. The transformation of the measurement update stage into a nonlinear optimization problem allows the system to maintain strict geometric consistency. To validate the method, a computer simulation environment with three scenarios was developed: a closed loop, a city block with sharp turns, and a figure-eight trajectory. The results of a series of experiments confirm that the proposed IEKF-SLAM provides, on average, 3.5 times higher accuracy of trajectory construction according to the absolute error (ATE) metric. The algorithm almost completely eliminates yaw angle drift compared to the classic EKF-SLAM (the deviation is < 3° versus ~ 25°). It is also proven that the invariant filter is able to effectively use a larger number of visual landmarks (80–100 points) without losing consistency. Despite an increase in computational costs by approximately 30%, the algorithm retains the ability to operate stably in real time, which makes it an optimal solution for modern autonomous systems.