Observability-based Rules for Designing Consistent EKF SLAM Estimators

Abstract

Abstract-In this work, we study the inconsistency problem of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the process and measurement models are evaluated at the latest state estimates during every time step, the linearized error-state system employed in the EKF has observable subspace of dimension higher than that of the actual, nonlinear, SLAM system. As a result, the covariance estimates of the EKF undergo reduction in directions of the state space where no information is available, which is a primary cause of the inconsistency. Based on these theoretical results, we propose a general framework for improving the consistency of EKF-based SLAM. In this framework, the EKF linearization points are selected in a way that ensures that the resulting linearized system model has an observable subspace of appropriate dimension. We describe two algorithms that are instances of this paradigm. In the first, termed Observability Constrained EKF (OC-EKF), the linearization points are selected so as to minimize their expected errors (i.e., the difference between the linearization point and the true state) under the observability constraints. In the second, the filter Jacobians are calculated using the first-ever available estimates for all state variables. This latter approach is termed First-Estimates Jacobian (FEJ)-EKF. The proposed algorithms have been tested both in simulation and experimentally, and are shown to significantly outperform the standard EKF both in terms of accuracy and consistency.

Towards Consistent Vision-Aided Inertial Navigation

“…Due to linearization errors, the number of unobservable directions is reduced in a standard EKF-based VINS approach, allowing the estimator to gain spurious information and leading to inconsistency. What we present is a significant, nontrivial extension of our previous work on mitigating inconsistency in 2D robot localization problems [12]. This is due in part to the higher-dimensional state of the 3D VINS system as compared to 2D localization (15 elements vs. 3), as well as more complex motion and measurement models.…”

“…The work by Huang et al [11,12,13] was the first to identify this connection for several 2D localization problems (i.e., simultaneous localization and mapping, cooperative localization). The authors showed that, for these problems, a mismatch exists between the number of unobservable directions of the true nonlinear system and the linearized system used for estimation purposes.…”

Observability-based consistent EKF estimators for multi-robot cooperative localization

“…However, to the best of our knowledge, prior to [15], no work has analytically examined the consistency of CL. In contrast, recent research has focused on the consistency of EKF-based simultaneous localization and mapping (SLAM) (see [24][25][26][27][28][29][30][31]), showing that the computed state estimates tend to be inconsistent. Specifically, Julier and Uhlmann [24] first observed that when a stationary robot measures the relative position of a new landmark multiple times, the estimated variance of the robot's orientation becomes smaller.…”

“…In our previous work [29][30][31], we conducted a theoretical analysis of the EKF-SLAM inconsistency, and identified as a fundamental cause the mismatch between the dimensions of the observable subspaces of the linearized system, employed by the EKF, and the underlying nonlinear system. Furthermore, we introduced the first estimates Jacobian (FEJ)-EKF and observability-constrained (OC)-EKF, which significantly outperform the standard EKF and the robocentric mapping algorithm [25], in terms of both accuracy and consistency.…”

A critique of current developments in simultaneous localization and mapping

“…11,12 This insight resulted in a number of observability-constrained EKF SLAM algorithms which significantly improve the SLAM consistency. 13 SLAM can also be formulated as a parameter estimation problem where all the robot poses from where the measurements were taken together with all the observed features are treated as a set of unknown constant parameters that need to be estimated. An objective function based on maximum likelihood of the parameter estimates can be formulated by using robot odometry measurements and robot-to-feature observation information to relate the unknown parameters.…”