Square Root Iterated Kalman Filter for Bearing-Only SLAM

This paper presents an undelayed solution to the bearing-only simultaneous localization and mapping problem (SLAM). We employ a square-root iterated Kalman filter for nonlinear state estimation. The proposed technique incorporates a modified Kalman update that is equivalent to a variable-step iterative Gauss-Newton method, and is numerically stable because it maintains a square-root decomposition of the covariance matrix. Although many existing bearing only algorithms focus on proper initialization of landmark locations, our method allows for arbitrary initialization along the initial measurement ray without sacrificing map accuracy. This is desirable because we require only one filter and the state dimension of that filter need not include numerous temporary hypotheses. For this reason, the proposed algorithm is more computationally efficient than other methods. We demonstrate the feasibility of this approach in simulation and with experiments on mobile robots.