Observability-based rules for designing consistent EKF SLAM estimators

Guoquan P. Huang, Anastasios I. Mourikis, Stergios Roumeliotis

Research output: Contribution to journalArticle

86 Citations (Scopus)

Abstract

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.

Original languageEnglish (US)
Pages (from-to)502-528
Number of pages27
JournalInternational Journal of Robotics Research
Volume29
Issue number5
DOIs
StatePublished - Apr 1 2010

Fingerprint

Simultaneous Localization and Mapping
Observability
Extended Kalman filters
Kalman Filter
Estimator
Linearization
Inconsistency
Estimate
Subspace
Higher Dimensions
State Space
Paradigm
Filter
Minimise

Keywords

  • Estimator inconsistency
  • Extended Kalman filter
  • Linearization errors
  • Nonlinear estimation
  • Observability
  • Simultaneous localization and mapping

Cite this

Observability-based rules for designing consistent EKF SLAM estimators. / Huang, Guoquan P.; Mourikis, Anastasios I.; Roumeliotis, Stergios.

In: International Journal of Robotics Research, Vol. 29, No. 5, 01.04.2010, p. 502-528.

Research output: Contribution to journalArticle

@article{6a94b61e78c0439091076b387c861f0f,
title = "Observability-based rules for designing consistent EKF SLAM estimators",
abstract = "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.",
keywords = "Estimator inconsistency, Extended Kalman filter, Linearization errors, Nonlinear estimation, Observability, Simultaneous localization and mapping",
author = "Huang, {Guoquan P.} and Mourikis, {Anastasios I.} and Stergios Roumeliotis",
year = "2010",
month = "4",
day = "1",
doi = "10.1177/0278364909353640",
language = "English (US)",
volume = "29",
pages = "502--528",
journal = "International Journal of Robotics Research",
issn = "0278-3649",
publisher = "SAGE Publications Inc.",
number = "5",

}

TY - JOUR

T1 - Observability-based rules for designing consistent EKF SLAM estimators

AU - Huang, Guoquan P.

AU - Mourikis, Anastasios I.

AU - Roumeliotis, Stergios

PY - 2010/4/1

Y1 - 2010/4/1

N2 - 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.

AB - 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.

KW - Estimator inconsistency

KW - Extended Kalman filter

KW - Linearization errors

KW - Nonlinear estimation

KW - Observability

KW - Simultaneous localization and mapping

UR - http://www.scopus.com/inward/record.url?scp=77951109530&partnerID=8YFLogxK

UR - http://www.scopus.com/inward/citedby.url?scp=77951109530&partnerID=8YFLogxK

U2 - 10.1177/0278364909353640

DO - 10.1177/0278364909353640

M3 - Article

VL - 29

SP - 502

EP - 528

JO - International Journal of Robotics Research

JF - International Journal of Robotics Research

SN - 0278-3649

IS - 5

ER -