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.

[1]  Gamini Dissanayake,et al.  Convergence and Consistency Analysis for Extended Kalman Filter Based SLAM , 2007, IEEE Transactions on Robotics.

[2]  José A. Castellanos,et al.  Robocentric map joining: Improving the consistency of EKF-SLAM , 2007, Robotics Auton. Syst..

[3]  Javier Ibanez Guzman,et al.  On the Observability and Observability Analysis of SLAM , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[4]  Carl D. Meyer,et al.  Matrix Analysis and Applied Linear Algebra , 2000 .

[5]  Gamini Dissanayake,et al.  Convergence analysis for extended Kalman filter based SLAM , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[6]  Juan Andrade-Cetto,et al.  The effects of partial observability when building fully correlated maps , 2005, IEEE Transactions on Robotics.

[7]  J. A. Castellanos,et al.  Limits to the consistency of EKF-based SLAM , 2004 .

[8]  Stergios I. Roumeliotis,et al.  Analysis and improvement of the consistency of extended Kalman filter based SLAM , 2008, 2008 IEEE International Conference on Robotics and Automation.

[9]  J. C. Hung,et al.  Local observability matrix and its application to observability analyses , 1990, [Proceedings] IECON '90: 16th Annual Conference of IEEE Industrial Electronics Society.

[10]  Juan Andrade-Cetto,et al.  The effects of partial observability in SLAM , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[11]  Eduardo Mario Nebot,et al.  Consistency of the EKF-SLAM Algorithm , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[12]  Stergios I. Roumeliotis,et al.  Generalized Analysis and Improvement of the Consistency of EKF-based SLAM , 2008 .

[13]  Thia Kirubarajan,et al.  Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software , 2001 .

[14]  A. Krener,et al.  Nonlinear controllability and observability , 1977 .

[15]  Jeffrey K. Uhlmann,et al.  A counter example to the theory of simultaneous localization and map building , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).