Simultaneous localization and map building with linear system model

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 two identical submatrix and a negative symmetrical submatrix. An optimization solution is proposed based on this property. The computation requirement is reduced without any approximation. The optimization solution is consistent and convergent theoretically and realistically. Another approximation solution is proposed to reduce computation requirement much more. The experiment compares the performance of optimization and approximation solution with the full covariance solution. All these techniques have been implemented on our mobile robot ATRVII equipped with 2D laser rangefinder SICK.