A Square Root Unscented Kalman Filter for visual monoSLAM

Abstract

This paper introduces a square root unscented Kalman filter (SRUKF) solution to the problem of performing visual simultaneous localization and mapping (SLAM) using a single camera. Several authors have proposed the conventional UKF for SLAM to improve the handling of non-linearities compared with the more widely used EKF, but at the expense increasing computational complexity from O(N <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sup> ) to O(N <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">3</sup> ) in the map size, making it unattractive for video-rate application. Van der Merwe and Wan's general SRUKF delivers identical results to a general UKF along with computational savings, but remains O(N <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">3</sup> ) overall. This paper shows how the SRUKF for the SLAM problem can be re-posed with O(N <sup xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">2</sup> ) complexity, matching that of the EKF. The paper also shows how the method of inverse depth feature initialization developed by Montiel et al. for the EKF can be reformulated to work with the SRUKF. Experimental results confirm that the SRUKF and the UKF produce identical estimates, and that the SRUKF is more consistent than the EKF. Although the complexity is the same, the SRUKF remains more expensive to compute.