A higher order Rao-Blackwellized particle filter for monocular vSLAM

Abstract

This paper generalizes the traditional formulation of Rao-Blackwellized particle filter (RBPF) by incorporating a higher order state variable and a modified undelayed initialization scheme to solve the 3D monocular visual SLAM problem (vSLAM). In the proposed approach, velocity has been included in the state variables so that filtering progresses based on sampling from velocity distribution, not the displacement. The new sampling posterior has been obtained with respect to observations, control inputs and the robot path. The proper importance weight for resampling has been derived in this paper. To solve the bearing-only problem, the proposed approach features a modified initialization scheme that uses an inverse depth of the landmarks. The proposed higher order RBPF approach has been compared to the traditional (lower order) RBPF approach for proof of concept through a tangible simulation routine. The results of the numerical simulation indicate the superiority of the higher order RBPF in certain conditions e.g., high parallax angles.