Convergence and Consistency Analysis for a 3-D Invariant-EKF SLAM

Abstract

Abstract-In this paper, we investigate the convergence and consistency properties of an Invariant-Extended Kalman Filter (RI-EKF) based Simultaneous Localization and Mapping (SLAM) algorithm. Basic convergence properties of this algorithm are proven. These proofs do not require the restrictive assumption that the Jacobians of the motion and observation models need to be evaluated at the ground truth. It is also shown that the output of RI-EKF is invariant under any stochastic rigid body transformation in contrast to SO(3) based EKF SLAM algorithm (SO(3)-EKF) that is only invariant under deterministic rigid body transformation. Implications of these invariance properties on the consistency of the estimator are also discussed. Monte Carlo simulation results demonstrate that RI-EKF outperforms SO(3)-EKF, Robocentric-EKF and the "First Estimates Jacobian" EKF, for 3D point feature based SLAM.

Contact-aided invariant extended Kalman filtering for robot state estimation

“…This invariance is referred to as the symmetries of the system [5]. This work led to the development of the InEKF [18,8,10,11], with successful applications and promising results in SLAM [8,86] and aided INSs [21,4,5,8,83]. Similar to the above mentioned EKFs on matrix Lie groups, the state is again represented as a matrix Lie group and the noise as a concentrated Gaussian on the group.…”

“…This approach was taken by Bloesch et al [15] when developing a quaternion-based extended Kalman filter (QEKF) that combines inertial, contact, and kinematic data to estimate the robot's base pose, velocity, and a number of contact states. In this article, we expand upon these ideas to develop an invariant extended Kalman filter (InEKF) that has improved convergence and consistency properties allowing for a more robust observer that is suitable for long-term autonomy.The theory of invariant observer design is based on the estimation error being invariant under the action of a matrix Lie group [1,20], which has recently led to the development of the InEKF 1 [18,8,10,11] with successful applications and promising results in simultaneous localization and mapping [8,86] and aided inertial navigation systems [4,5,8,83]. The invariance of the estimation error with respect to a Lie group action is referred to as a symmetry of the system [5].…”

Visual-Inertial Navigation: A Concise Review

“…We have introduced the optimal state constraint (OSC)-EKF [54,55] that first optimally extracts all the information contained in the visual measurements about the relative camera poses in a sliding window and then uses these inferred relative-pose measurements in the EKF update. The (right) invariant Kalman filter [56] was recently employed to improve filter consistency [25,57,58,59,60], as well as the (iterated) EKF that was also used for VINS in robocentric formulations [22,61,62,63]. On the other hand, in the EKF framework, different geometric features besides points have also been exploited to improve VINS performance, for example, line features used in [64,65,66,67,68] and plane features in [69,70,71,72].…”

An Efficient Schmidt-EKF for 3D Visual-Inertial SLAM

“…While SLAM estimators -by jointly estimating the location of the sensor platform and the features in the surrounding environment -are able to easily incorporate loop closure constraints to bound localization errors and have attracted much research attention in the past three decades [8,1,6,3], there are also significant research efforts devoted to open-loop VIO systems (e.g., [30,13,14,22,17,50,37,49,53,5,2,15,40]). For example, a hybrid MSCKF/SLAM estimator was developed for VIO [23], which retains features that can be continuously tracked beyond the sliding window in the state as SLAM features while removing them when they get lost.…”