Optimal mobile robot pose estimation using geometrical maps

We propose a weighted least-squares (WLS) algorithm for optimal pose estimation of mobile robots using geometrical maps as environment models. Pose estimation is achieved from feature correspondences in a nonlinear framework without linearization. The proposed WLS approach yields optimal estimates in the least-squares sense, is applicable to heterogeneous geometrical features decomposed in points and lines, and has an O(N) computation time.