In this paper, we propose a novel robocentric formulation of visual-inertial navigation systems (VINS)within a multi-state constraint Kalman filter (MSCKF)framework and develop an efficient, lightweight, robocentric visual-inertial odometry (R-VIO)algorithm for consistent localization in challenging environments using only monocular vision. The key idea of the proposed approach is to deliberately reformulate the 3D VINS with respect to a moving local frame (i.e., robocentric), rather than a fixed global frame of reference as in the standard world-centric VINS, and instead utilize high-accuracy relative motion estimates for global pose update. As an immediate advantage of using this robocentric formulation, the proposed R-VIO can start from an arbitrary pose, without the need to align its orientation with the global gravity vector. More importantly, we analytically show that the proposed robocentric EKF-based VINS does not undergo the observability mismatch issue as in the standard world-centric frameworks which was identified as the main cause of inconsistency of estimation. The proposed R-VIO is extensively tested through both Monte Carlo simulations and real-world experiments using different sensor platforms in different environments and shown to achieve competitive performance with the state-of-the-art VINS algorithms in terms of consistency, accuracy and efficiency.
In particular, there are two main approaches for tightly-coupled state estimation, i.e., the optimization-based and the EKF-based. Typically, bundle adjustment (BA) @cite_8 is employed by the former that is to estimate all the states involved in all of the available measurements by solving a nonlinear least-squares problem (e.g., @cite_10 @cite_33 ). As the relinearization of nonlinear measurement models is carried out at each iteration, this would incur higher computational cost as compared to the EKF-based methods (e.g., @cite_27 @cite_40 ). However, as what was mentioned before, the standard EKF-based VINS suffers from the estimation inconsistency primarily caused by the observability mismatch due to EKF linearization (e.g., @cite_28 @cite_3 ). Recently, @cite_20 @cite_17 introduced an EKF-based VINS solution using a robocentric formulation, which, however, follows the VI-SLAM framework and employs the iterated EKF update in a direct fashion. In contrast to that, inspired by the robocentric mapping that improves the EKF consistency in the 2D SLAM @cite_24 , in this paper we propose a robocentric formulation within the sliding window filter-based VIO framework and perform the observability analysis of the EKF-based robocentric VINS to theoretically support the consistency improvement of the proposed R-VIO algorithm.
@cite_33 @cite_8 @cite_28 @cite_3 @cite_24 @cite_40 @cite_27 @cite_10 @cite_20 @cite_17
2056358962 2800595980 956540729 2592781794
In this paper, we focus on the problem of motion tracking in unknown environments using visual and inertial sensors. We term this estimation task visual-inertial odometry (VIO), in analogy to the well-known visual-odometry problem. We present a detailed study of extended Kalman filter (EKF)-based VIO algorithms, by comparing both their theoretical properties and empirical performance. We show that an EKF formulation where the state vector comprises a sliding window of poses (the multi-state-constraint Kalman filter (MSCKF)) attains better accuracy, consistency, and computational efficiency than the simultaneous localization and mapping (SLAM) formulation of the EKF, in which the state vector contains the current pose and the features seen by the camera. Moreover, we prove that both types of EKF approaches are inconsistent, due to the way in which Jacobians are computed. Specifically, we show that the observability properties of the EKF's linearized system models do not match those of the underlying system, which causes the filters to underestimate the uncertainty in the state estimates. Based on our analysis, we propose a novel, real-time EKF-based VIO algorithm, which achieves consistent estimation by (i) ensuring the correct observability properties of its linearized system model, and (ii) performing online estimation of the camera-to-inertial measurement unit (IMU) calibration parameters. This algorithm, which we term MSCKF 2.0, is shown to achieve accuracy and consistency higher than even an iterative, sliding-window fixed-lag smoother, in both Monte Carlo simulations and real-world testing. In this paper, we propose a novel robocentric formulation of the visual-inertial navigation system (VINS) within a sliding-window filtering framework and design an efficient, lightweight, robocentric visual-inertial odometry (R-VIO) algorithm for consistent motion tracking even in challenging environments using only a monocular camera and a 6-axis IMU. The key idea is to deliberately reformulate the VINS with respect to a moving local frame, rather than a fixed global frame of reference as in the standard world-centric VINS, in order to obtain relative motion estimates of higher accuracy for updating global poses. As an immediate advantage of this robocentric formulation, the proposed R-VIO can start from an arbitrary pose, without the need to align the initial orientation with the global gravitational direction. More importantly, we analytically show that the linearized robocentric VINS does not undergo the observability mismatch issue as in the standard world-centric counterpart which was identified in the literature as the main cause of estimation inconsistency. Additionally, we investigate in-depth the special motions that degrade the performance in the world-centric formulation and show that such degenerate cases can be easily compensated in the proposed robocentric formulation, without resorting to additional sensors as in the world-centric formulation, thus leading to better robustness. The proposed R-VIO algorithm has been extensively tested through both Monte Carlo simulations and real-world experiments with different sensor platforms navigating in different environments, and shown to achieve better (or competitive at least) performance than the state-of-the-art VINS, in terms of consistency, accuracy and efficiency. In this paper, we study estimator inconsistency in Vision-aided Inertial Navigation Systems (VINS). We show that standard (linearized) estimation approaches, such as the Extended Kalman Filter (EKF), can fundamentally alter the system observability properties, in terms of the number and structure of the unobservable directions. This in turn allows the influx of spurious information, leading to inconsistency. To address this issue, we propose an Observability-Constrained VINS (OC-VINS) methodology that explicitly adheres to the observability properties of the true system.We apply our approach to the Multi-State Constraint Kalman Filter (MSC-KF), and provide both simulation and experimental validation of the effectiveness of our method for improving estimator consistency. The main contribution of this paper is an invariant extended Kalman filter (EKF) for visual inertial navigation systems (VINS). It is demonstrated that the conventional EKF based VINS is not invariant under the stochastic unobservable transformation, associated with a translation and a rotation about the gravitational direction. This can lead to inconsistent state estimates as the estimator does not obey a fundamental property of the physical system. To address this issue, we use a novel uncertainty representation to derive a Right Invariant error extended Kalman filter (RIEKF-VINS) that preserves this invariance property. RIEKF-VINS is then adapted to the multi-state constraint Kalman filter framework to obtain a consistent state estimator. Both Monte Carlo simulations and real-world experiments are used to validate the proposed method.