Stochastic State Estimation for Simultaneous Localization and Map Building in Mobile Robotics

摘要

We have shown that full correlation of the map model in the Kalman Filter based approach to Simultaneous Localization and Map Building hinders full observability of the state estimate. A unit norm eigenvalue for the matrix F-KH makes the state error estimate converge to a non zero mean constant bounded value in the linear case SLAM. Marginal stability of such partially observable system produces also at least one psd solution to the steady state Riccati equation for the covariance error, provided the initial conditions of P are also psd. Partial observability makes the final map dependant on the initial observations. This situation can easily be remedied either by anchoring the map to the first landmark observed, or by having an external sensor that sees the vehicle at all times. Suboptimal techniques to improve the speed of the algorithm include covariance inflation methods to diagonalize the state error covariance matrix. These techniques may lead to instability if pseudo-noise is added in a higher state dimensionality than what can be observed. We propose in this Chapter to diagonalize only the map part of the state error covariance, thus guaranteeing convergence of P, and at the same time obtaining an O(N) algorithm.