Multi-state Constraint Kalman Filter Research Articles

EKF-Based Visual Inertial Navigation Using Sliding Window Nonlinear Optimization

In this paper, we present a hybrid visual inertial navigation algorithm for an autonomous and intelligent vehicle that combines the multi-state constraint Kalman filter (MSCKF) with the nonlinear visual-inertial graph optimization. The MSCKF is a well-known visual inertial odometry (VIO) method that performs the fusion between an inertial measurement unit (IMU) and the image measurements within a sliding window. The MSCKF computes the re-projection errors from the camera measurements and the states in the sliding window. During this process, the structure-only estimation is performed without exploiting the full information over the window, like the relative interstate motion constraints and their uncertainties. The key contribution of this paper is combination of the filtering and non-linear optimization method for VIO, and the design of a novel measurement model that exploits all of the measurements and information available within the sliding window. The local visual-inertial optimization is performed using pre-integrated IMU measurements and camera measurements. It infers the probabilistically optimal relative pose constraints. These local optimal constraints are used to estimate the global states under the MSCKF framework. The proposed local-optimal-multi-state constraint Kalman filter is validated using a simulation data set, as well as publicly available real-world data sets generated from real-world urban driving experiments.

Monocular Visual-Inertial Odometry with an Unbiased Linear System Model and Robust Feature Tracking Front-End.

The research field of visual-inertial odometry has entered a mature stage in recent years. However, unneglectable problems still exist. Tradeoffs have to be made between high accuracy and low computation for users. In addition, notation confusion exists in quaternion descriptions of rotation; although not fatal, this may results in unnecessary difficulties in understanding for researchers. In this paper, we develop a visual-inertial odometry which gives consideration to both precision and computation. The proposed algorithm is a filter-based solution that utilizes the framework of the noted multi-state constraint Kalman filter. To dispel notation confusion, we deduced the error state transition equation from scratch, using the more cognitive Hamilton notation of quaternion. We further come up with a fully linear closed-form formulation that is readily implemented. As the filter-based back-end is vulnerable to feature matching outliers, a descriptor-assisted optical flow tracking front-end was developed to cope with the issue. This modification only requires negligible additional computation. In addition, an initialization procedure is implemented, which automatically selects static data to initialize the filter state. Evaluations of proposed methods were done on a public, real-world dataset, and comparisons were made with state-of-the-art solutions. The experimental results show that the proposed solution is comparable in precision and demonstrates higher computation efficiency compared to the state-of-the-art.

Tightly-Coupled Visual-Inertial Localization and 3-D Rigid-Body Target Tracking

In this letter we present a novel method to perform target tracking of a moving rigid body utilizing an inertial measurement unit with cameras. A key contribution is the tightly-coupling of the target motion estimation within a visual-inertial navigation system (VINS), allowing for improved performance of both the processes. In particular, we build upon the standard multi-state constraint Kalman filter -based VINS and generalize it to incorporate three-dimensional (3-D) target tracking. Rather than representing the target object as a moving point particle (which is often the case in the literature), we instead utilize a dynamic, 3-D rigid-body model, wherein orientation, position, and their derivatives are estimated, as well as the structure of points on the object. We then leverage visual bearings to this set of features for target motion estimation, rather than requiring continuous observation of a single representative point over the tracking period. Moreover, we propose three motion models which capture most commonly-seen tracking scenarios in practice such as UAVs, fixed-wing aircraft, and ground vehicles over changing slopes and perform an observability analysis with geometric interpretation, providing insights into parameter initialization, and modes of estimation drift. The proposed estimator is validated with both Monte-Carlo simulations and real-world experiments where it is shown to offer accurate performance even for challenging trajectories that do not completely fit the selected model.

Tight Fusion of a Monocular Camera, MEMS-IMU, and Single-Frequency Multi-GNSS RTK for Precise Navigation in GNSS-Challenged Environments

Precise position, velocity, and attitude is essential for self-driving cars and unmanned aerial vehicles (UAVs). The integration of global navigation satellite system (GNSS) real-time kinematics (RTK) and inertial measurement units (IMUs) is able to provide high-accuracy navigation solutions in open-sky conditions, but the accuracy will be degraded severely in GNSS-challenged environments, especially integrated with the low-cost microelectromechanical system (MEMS) IMUs. In order to navigate in GNSS-denied environments, the visual–inertial system has been widely adopted due to its complementary characteristics, but it suffers from error accumulation. In this contribution, we tightly integrate the raw measurements from the single-frequency multi-GNSS RTK, MEMS-IMU, and monocular camera through the extended Kalman filter (EKF) to enhance the navigation performance in terms of accuracy, continuity, and availability. The visual measurement model from the well-known multistate constraint Kalman filter (MSCKF) is combined with the double-differenced GNSS measurement model to update the integration filter. A field vehicular experiment was carried out in GNSS-challenged environments to evaluate the performance of the proposed algorithm. Results indicate that both multi-GNSS and vision contribute significantly to the centimeter-level positioning availability in GNSS-challenged environments. Meanwhile, the velocity and attitude accuracy can be greatly improved by using the tightly-coupled multi-GNSS RTK/INS/Vision integration, especially for the yaw angle.

Vehicle Positioning in GNSS-Deprived Urban Areas by Stereo Visual-Inertial Odometry

Accurate and continuous positioning in global navigation satellite system (GNSS) deprived urban areas is crucial for autonomous vehicles and mobile mapping systems. To achieve this goal, we propose a stereo visual-inertial odometry approach using a multistate constraint Kalman filter (MSCKF). In contrast with the conventional MSCKF, in which an inertial navigation system (INS) propagates the vehicle motion, and then the propagation is corrected by measurements of salient features extracted from images of a single camera, we update the propagation with observations extracted from images of a stereo pair of cameras. This way, additional constraints across the stereo pairs of images are exploited to improve the pose estimation. Experimental results on several KITTI datasets show that the stereo MSCKF outperforms the mono one achieving an average positioning error of 0.9% of the trajectory length compared to 1.3% for the mono approach and 2.5% for INS-only integration. These results show that visual-inertial odometry has a promising potential for vehicle positioning in short periods of GNSS signal outage.

Consistent EKF-Based Visual-Inertial Odometry on Matrix Lie Group

In this paper, we present a novel visual-inertial navigation algorithm for low-cost and computationally constrained vehicle in global positioning system denied environments by modeling the state space as the matrix Lie group (LG), based on the recent theory of the invariant Kalman filter. The multistate constraint Kalman filter (MSCKF) is a well-known visual-inertial odometry algorithm that performs the fusion of the visual and inertial information by constraining each other through the stochastically cloned pose within a sliding window. However, conventional MSCKF (MSCKF-Conv) suffers from the inconsistent state estimates caused by the spurious gain along the unobservable directions, resulting in large estimation errors. To tackle this problem, we extend the concepts of the state and noise of the MSCKF from Euclidean space to matrix LG. We model the state of the MSCKF as the element of the specially customized matrix LG and use the noise uncertainty modeling with the corresponding Lie algebra. The detailed derivation and observability analysis of the proposed filter are provided to prove that the proposed filter is more consistent than the MSCKF-Conv. The proposed MSCKF on matrix LG naturally enforces the state vector to exist in the state space that maintains the unobservability characteristics without any artificial remedies. The performance of the proposed filter is validated through the Monte-Carlo simulation and the real-world experimental dataset.

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight

In recent years, vision-aided inertial odometry for state estimation has matured significantly. However, we still encounter challenges in terms of improving the computational efficiency and robustness of the underlying algorithms for applications in autonomous flight with microaerial vehicles, in which it is difficult to use high-quality sensors and powerful processors because of constraints on size and weight. In this letter, we present a filter-based stereo visual inertial odometry that uses the multistate constraint Kalman filter. Previous work on the stereo visual inertial odometry has resulted in solutions that are computationally expensive. We demonstrate that our stereo multistate constraint Kalman filter (S-MSCKF) is comparable to state-of-the-art monocular solutions in terms of computational cost, while providing significantly greater robustness. We evaluate our S-MSCKF algorithm and compare it with state-of-the-art methods including OKVIS, ROVIO, and VINS-MONO on both the EuRoC dataset and our own experimental datasets demonstrating fast autonomous flight with a maximum speed of 17.5 m/s in indoor and outdoor environments. Our implementation of the S-MSCKF is available at https://github.com/KumarRobotics/msckf_vio.

Pose estimation by Omnidirectional Visual-Inertial Odometry

In this paper, a novel approach to ego-motion estimation is proposed based on visual and inertial sensors, named Omnidirectional Visual-Inertial Odometry (OVIO). The proposed approach combines omnidirectional visual features with inertial measurements within the Multi-State Constraint Kalman Filter (MSCKF). In contrast with other visual inertial odometry methods that use visual features captured by perspective cameras, the proposed approach utilizes spherical images obtained by an omnidirectional camera to obtain more accurate estimates of the position and orientation of the camera. Because the standard perspective model is unsuitable for omnidirectional cameras, a measurement model on a plane tangent to the unit sphere rather than on the image plane is defined. The key hypothesis of OVIO is that a wider field of view allows the incorporation of more visual features from the surrounding environment, thereby improving the accuracy and robustness of the ego-motion estimation. Moreover, by using an omnidirectional camera, a situation where there is not enough texture is less likely to arise. Experimental evaluation of OVIO using synthetic and real video sequences captured by a fish-eye camera in both indoor and outdoor environments shows the superior performance of the proposed OVIO as compared to the MSCKF using a perspective camera in both positioning and attitude estimation.

Rapid and robust initialization for monocular visual inertial navigation within multi-state Kalman filter

Sensor-fusion based navigation attracts significant attentions for its robustness and accuracy in various applications. To achieve a versatile and efficient state estimation both indoor and outdoor, this paper presents an improved monocular visual inertial navigation architecture within the Multi-State Constraint Kalman Filter (MSCKF). In addition, to alleviate the initialization demands by appending enough stable poses in MSCKF, a rapid and robust Initialization MSCKF (I-MSCKF) navigation method is proposed in the paper. Based on the trifocal tensor and sigma-point filter, the initialization of the integrated navigation can be accomplished within three consecutive visual frames. Thus, the proposed I-MSCKF method can improve the navigation performance when suffered from shocks at the initial stage. Moreover, the sigma-point filter is applied at initial stage to improve the accuracy for state estimation. The state vector generated at initial stage from the proposed method is consistent with MSCKF, and thus a seamless transition can be achieved between the initialization and the subsequent navigation in I-MSCKF. Finally, the experimental results show that the proposed I-MSCKF method can improve the robustness and accuracy for monocular visual inertial navigations.

Monocular Visual Inertial Navigation for Mobile Robots using Uncertainty based Triangulation

Abstract In this paper, we present a robust multi-state constraint Kalman filter (MSCKF) for visual inertial navigation of mobile robots. We assume the hardware of a mobile robot consists of an inertial measurement unit (IMU) and a monocular camera. The MSCKF is a well-known visual inertial navigation algorithm which performs tightly-coupled fusion between IMU and camera measurements over a sliding window of camera poses, like fixed lag smoother. The conventional MSCKF calculates the residuals as the differences between camera measurements and the re-projected points from the triangulated 3D point, which is calculated by using camera measurements and the pose information over the sliding window. However, the uncertainties of camera poses and image measurements are not considered in this triangulation process. Our algorithm is enforced to estimate robust and precise results by providing a good linearization point related with a feature 3D position based on uncertainty based triangulation, which considers the uncertainties of all sources related with triangulation. The proposed algorithm is validated by the dataset, which is generated known trajectory and features, and real world experimental datasets.

Consistency Analysis and Improvement of Vision-aided Inertial Navigation

In this paper, we study estimator inconsistency in vision-aided inertial navigation systems (VINS) from the standpoint of system's observability. We postulate that a leading cause of inconsistency is the gain of spurious information along unobservable directions, which results in smaller uncertainties, larger estimation errors, and divergence. We develop an observability constrained VINS (OC-VINS), which explicitly enforces the unobservable directions of the system, hence preventing spurious information gain and reducing inconsistency. This framework is applicable to several variants of the VINS problem such as visual simultaneous localization and mapping (V-SLAM), as well as visual-inertial odometry using the multi-state constraint Kalman filter (MSC-KF). Our analysis, along with the proposed method to reduce inconsistency, are extensively validated with simulation trials and real-world experimentation.

High-precision, consistent EKF-based visual-inertial odometry

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.