Simultaneous Localization and Map Building with Modified System State

The full covariance solution to simultaneous localization and map building based on extended Kalmanfilter requires update time quadratic in the number of landmarks in the map. In order to improve thecomputational efficiency, this paper reorganizes system state vector and system models. The state of mobile robotis redefined and represented indirectly. The higher dimensional system models and covariance matrix can berepresented with two lower dimensional submatrices. An optimization solution is proposed based on thisproperty. The computational requirement and memory requirement are decreased by half. The covariance matrixis fully updated without any approximation during estimation. The optimization solution is consistent andconvergent theoretically and realistically. The experiment also compares the performance of optimization solutionwith the full covariance solution. All these techniques have been implemented on mobile robot ATRVII equippedwith 2D laser rangefinder SICK