An optimization solution to simultaneous localization and map building

This paper presents an optimization solution for the simultaneous localization and map building (SLAM) problem. The full covariance solution based on extended Kalman filter (EKF) requires update time quadratic in the number of landmarks in the map. This paper reconstructs system state vector and system models. Covariance matrix consists of a symmetrical submatrix and an anti-symmetrical submatrix. An optimization solution is proposed based on this property. The computation requirement is reduced without any approximation during covariance matrix update. The optimization solution is consistent and convergent theoretically and realistically. The experiment compares the performance of optimization solution with the full covariance solution. All these techniques have been implemented on our mobile robot ATRVII equipped with 2D laser rangefinder SICK.