Experimental Study of Iterated Kalman Filters for Simultaneous Localization and Mapping of Autonomous Mobile Robots

In this paper, we investigate the role of iteration in Kalman filters family for improvement of the estimation accuracy of states in simultaneous localization and mapping (SLAM). The linearized error propagation existing in Kalman filters family can result in large errors and inconsistency in the SLAM problem. One approach to alleviate this situation is the use of iteration in extended Kalman filter (EKF) and sigma point Kalman filter (SPKF) based SLAM. The main contribution is to present that the iterated versions of Kalman filters can increase consistency and robustness of these filters against linear error propagation. Experimental results are presented to validate this improvement of state estimate convergence through repetitive linearization of the nonlinear observation model in EKF-SLAM and SPKF-SLAM algorithms.