Guoquan Huang

Observability-based Rules for Designing Consistent EKF SLAM Estimators

In this work, we study the inconsistency problem of extended Kalman filter (EKF)-based simultaneous localization and mapping (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 an observable subspace of dimension higher than that of the actual, non-linear, 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 (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.

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

In this paper, we investigate the consistency of extended Kalman filter (EKF)-based cooperative localization (CL) from the perspective of observability. We analytically show that the error-state system model employed in the standard EKF-based CL always has an observable subspace of higher dimension than that of the actual nonlinear CL system. This results in unjustified reduction of the EKF covariance estimates in directions of the state space where no information is available, and thus leads to inconsistency. To address this problem, we adopt an observability-based methodology for designing consistent estimators in which the linearization points are selected to ensure a linearized system model with observable subspace of correct dimension. In particular, we propose two novel observability-constrained (OC)-EKF estimators that are instances of this paradigm. In the first, termed OC-EKF 1.0, the filter Jacobians are calculated using the prior state estimates as the linearization points. In the second, termed OC-EKF 2.0, 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. The proposed OC-EKFs have been tested in simulation and experimentally, and have been shown to significantly outperform the standard EKF in terms of both accuracy and consistency.

Consistent sparsification for graph optimization

In a standard pose-graph formulation of simultaneous localization and mapping (SLAM), due to the continuously increasing numbers of nodes (states) and edges (measurements), the graph may grow prohibitively too large for long-term navigation. This motivates us to systematically reduce the pose graph amenable to available processing and memory resources. In particular, in this paper we introduce a consistent graph sparsification scheme: (i) sparsifying nodes via marginalization of old nodes, while retaining all the information (consistent relative constraints) - which is conveyed in the discarded measurements - about the remaining nodes after marginalization; and (ii) sparsifying edges by formulating and solving a consistent ℓ1-regularized minimization problem, which automatically promotes the sparsity of the graph. The proposed approach is validated on both synthetic and real data.

A First-Estimates Jacobian EKF for Improving SLAM Consistency

In this work, we study the inconsistency of EKF-based SLAM from the perspective of observability. We analytically prove that when the Jacobians of the system 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. Furthermore, a new “First-Estimates Jacobian” (FEJ) EKF is proposed to improve the estimator’s consistency during SLAM. The proposed algorithm performs better in terms of consistency, because when the filter Jacobians are calculated using the first-ever available estimates for each state variable, the error-state system model has an observable subspace of the same dimension as the underlying nonlinear SLAM system. The theoretical analysis is validated through both simulations and experiments.

A Quadratic-Complexity Observability-Constrained Unscented Kalman Filter for SLAM

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic computational complexity in the number of states and the inconsistency of the state estimates. To address the first issue, we introduce a new sampling strategy for the UKF, which has constant computational complexity. As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF)-based SLAM, i.e., quadratic in the size of the state vector. Furthermore, we investigate the inconsistency issue by analyzing the observability properties of the linear-regression-based model employed by the UKF. Based on this analysis, we propose a new algorithm, termed observability-constrained (OC)-UKF, which ensures the unobservable subspace of the UKFs linear-regression-based system model is of the same dimension as that of the nonlinear SLAM system. This results in substantial improvement in the accuracy and consistency of the state estimates. The superior performance of the OC-UKF over other state-of-the-art SLAM algorithms is validated by both Monte-Carlo simulations and real-world experiments.

An observability-constrained sliding window filter for SLAM

A sliding window filter (SWF) is an appealing smoothing algorithm for nonlinear estimation problems such as simultaneous localization and mapping (SLAM), since it is resource-adaptive by controlling the size of the sliding window, and can better address the nonlinearity of the problem by relinearizing available measurements. However, due to the marginalization employed to discard old states from the sliding window, the standard SWF has different parameter observability properties from the optimal batch maximum-a-posterior (MAP) estimator. Specifically, the nullspace of the Fisher information matrix (or Hessian) has lower dimension than that of the batch MAP estimator. This implies that the standard SWF acquires spurious information, which can lead to inconsistency. To address this problem, we propose an observability-constrained (OC)-SWF where the linearization points are selected so as to ensure the correct dimension of the nullspace of the Hessian, as well as minimize the linearization errors. We present both Monte Carlo simulations and real-world experimental results which show that the OC-SWFs performance is superior to the standard SWF, in terms of both accuracy and consistency.

Communication-constrained multi-AUV cooperative SLAM

Multi-robot deployments have the potential for completing tasks more efficiently. For example, in simultaneous localization and mapping (SLAM), robots can better localize themselves and the map if they can share measurements of each other (direct encounters) and of commonly observed parts of the map (indirect encounters). However, performance is contingent on the quality of the communications channel. In the underwater scenario, communicating over any appreciable distance is achieved using acoustics which is low-bandwidth, slow, and unreliable, making cooperative operations very challenging. In this paper, we present a framework for cooperative SLAM (C-SLAM) for multiple autonomous underwater vehicles (AUVs) communicating only through acoustics. We develop a novel graph-based C-SLAM algorithm that is able to (optimally) generate communication packets whose size scales linearly with the number of observed features since the last successful transmission, constantly with the number of vehicles in the collective, and does not grow with time even the case of dropped packets, which are common. As a result, AUVs can bound their localization error without the need for pre-installed beacons or surfacing for GPS fixes during navigation, leading to significant reduction in time required to complete missions. The proposed algorithm is validated through realistic marine vehicle and acoustic communication simulations.

On the complexity and consistency of UKF-based SLAM

This paper addresses two key limitations of the unscented Kalman filter (UKF) when applied to the simultaneous localization and mapping (SLAM) problem: the cubic, in the number of states, computational complexity, and the inconsistency of the state estimates. In particular, we introduce a new sampling strategy that minimizes the linearization error and whose computational complexity is constant (i.e., independent of the size of the state vector). As a result, the overall computational complexity of UKF-based SLAM becomes of the same order as that of the extended Kalman filter (EKF) when applied to SLAM. Furthermore, we investigate the observability properties of the linear-regression-based model employed by the UKF, and propose a new algorithm, termed the Observability-Constrained (OC)-UKF, that improves the consistency of the state estimates. The superior performance of the OC-UKF compared to the standard UKF and its robustness to large linearization errors are validated by extensive simulations.

On the consistency of multi-robot cooperative localization.

In this paper, we investigate the consistency of extended Kalman filter (EKF)-based cooperative localization (CL) from the perspective of observability. To the best of our knowledge, this is the first work that analytically shows that the error-state system model employed in the standard EKF-based CL always has an observable subspace of higher dimension than that of the actual nonlinear CL system. This results in unjustified reduction of the EKF covariance estimates in directions of the state space where no information is available, and thus leads to inconsistency. To address this problem, we adopt an observabilitybased methodology for designing consistent estimators and propose a novel Observability-Constrained (OC)-EKF. In contrast to the standard EKF-CL, the linearization points of the OC-EKF are selected so as to ensure that the dimension of the observable subspace remains the same as that of the original (nonlinear) system. The proposed OC-EKF has been tested in simulation and experimentally, and has been shown to significantly outperform the standard EKF in terms of both accuracy and consistency.

Towards consistent visual-inertial navigation

Visual-inertial navigation systems (VINS) have prevailed in various applications, in part because of the complementary sensing capabilities and decreasing costs as well as sizes. While many of the current VINS algorithms undergo inconsistent estimation, in this paper we introduce a new extended Kalman filter (EKF)-based approach towards consistent estimates. To this end, we impose both state-transition and obervability constraints in computing EKF Jacobians so that the resulting linearized system can best approximate the underlying nonlinear system. Specifically, we enforce the propagation Jacobian to obey the semigroup property, thus being an appropriate state-transition matrix. This is achieved by parametrizing the orientation error state in the global, instead of local, frame of reference, and then evaluating the Jacobian at the propagated, instead of the updated, state estimates. Moreover, the EKF linearized system ensures correct observability by projecting the most-accurate measurement Jacobian onto the observable subspace so that no spurious information is gained. The proposed algorithm is validated by both Monte-Carlo simulation and real-world experimental tests.