Tim Bailey, Juan Nieto, Jose Guivant, Michael Stevens and Eduardo Nebot

Consistency of the EKF-SLAM Algorithm

IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006


 

Description

The EKF-SLAM algorithm is the most common implementation of recursive localisation and mapping. It has been around for nearly 20 years, but has not been closely examined in terms of estimate consistency. It is well known that it fails in large environments, but why? This paper looks at the root-cause for failure and how to overcome it.
 

Abstract

This paper presents an analysis of the extended Kalman filter formulation of simultaneous localisation and mapping (EKF-SLAM). We show that the algorithm produces very optimistic estimates once the “true” uncertainty in vehicle heading exceeds a limit. This failure is subtle and cannot, in general, be detected without ground-truth, although a very inconsistent filter may exhibit observable symptoms, such as disproportionately large jumps in the vehicle pose update. Conventional solutions — adding stabilising noise, using an iterated EKF or unscented filter, etc — do not improve the situation. However, if “small” heading uncertainty is maintained, EKF-SLAM exhibits consistent behaviour over an extended time-period. Although the uncertainty estimate slowly becomes optimistic, inconsistency can be mitigated indefinitely by applying tactics such as batch updates or stabilising noise. The manageable degradation of small heading variance SLAM indicates the efficacy of submap methods for large-scale maps.

Download

Full paper [pdf] (719 kb, 7 pages)

Addendum

The behaviour of the EKF-SLAM algorithm is still far from fully understood and characterised. The following is a list of open problems that I think might be interesting.

  • Test different motion models. My empirical experiments used the "bicycle kinematic model", which assumes pure rolling motion. Are the symptoms of jagged vehicle path due to this very constrained model? Will they still occur for more general models (eg, models that estimate slip angles [1], etc)? I have some preliminary results which indicate that the same symptoms are present for any motion model.
  • Perform analytical evaluation. Previous work on EKF-SLAM consistency has examined empirical behaviour [2,3,4] and analytical properties of linearised models [5]. Noone has yet compared EKF-SLAM to the true (or ideal) Bayesian filter; that is, a full non-linear analysis. If a full analytical solution is not possible, perhaps an empirical approximation to the ideal Bayesian estimate is feasible using, say, Monte Carlo methods or mixture models. Such an experiment might only require a few time-steps to show the divergence in shape of the "true" and "linearised" probability densities.
  • Examine the effects of process model and observation model linearisation separately. My published results simply looked at the overall effect of linearisation. Quite clearly, from the stationary vehicle tests, it is the observation model linearisation that causes the worst degradation (via information gain). However, linearisation of the process model also influences the estimate. I did an experiment where I linearised the observation model about the "true" state while linearising the process model about the "estimated" state. The results were very strange. The estimated covariance was reduced (indicating inconsistency) but the true errors were also reduced in proportion. A Monte Carlo analysis produced an average NEES within consistent bounds!! Why? What does this mean?
  • Try other fixed linearisations. My "ideal Jacobian" results linearised everything about the true state. However, perhaps "true state" linearisation is unnecessary and what really matters is simply "consistent linearisation". What happens if EKF-SLAM is run with fixed linearisations that are not the true states? How does this effect the estimated covariances? For example, we could run a nominal EKF-SLAM, record the final state estimates, then rerun SLAM using those final states as the linearisation points. (Of course, the vehicle state linearisations might present a problem for this scenario; perhaps use true states for the vehicle.)
  • Understand Figure 6(b). In my paper, I show the average NEES for two landmarks. The shape of the plot is very peculiar; it is (almost) within the NEES bounds but does not appear to be noisy enough. (Compare it to the other plots.) Why is it so?

References

  1. S.J. Julier and H.F. Durrant-Whyte., On The Role of Process Models in Autonomous Land Vehicle Navigation Systems, IEEE Transactions on Robotics and Automation, vol 19, no 1, 2003.
  2. S.J. Julier and J.K. Uhlmann., A counter example to the theory of simultaneous localization and map building. In IEEE International Conference on Robotics and Automation, pages 4238–4243, 2001.
  3. J.A. Castellanos, J. Neira, and J.D. Tardos., Limits to the consistency of EKF-based SLAM. In IFAC Symposium on Intelligent Autonomous Vehicles, 2004.
  4. T. Bailey, J. Nieto, J. Guivant, M. Stevens, and E. Nebot., Consistency of the EKF-SLAM algorithm. In IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006.
  5. S. Huang and G. Dissanayake., Convergence analysis for extended Kalman filter based SLAM. In IEEE International Conference on Robotics and Automation, 2006.

[Tim's Homepage]