Modified fast-SLAM for 2D mapping and 3D localization

Fast Simultaneous Localization and Mapping (SLAM) algorithm is capable of real-time implementation due to logarithmic time complexity which results in decrease of computational cost. In this algorithm state vector of a robot merely includes planar location of the robot and its angle to the horizontal plane. It has fewer components comparing to state vector in extended Kalman filter method which consists of location of all environmental features. In existing methods for implementing this algorithm, robot movement is considered to be totally in planar movement; while if moving on a slope changes the pitch angle of the robot, it causes errors in the algorithm. Correcting these errors will lead to a precise 2D mapping and 3D localization. This paper details the modification added to conventional Fast-Slam algorithm to accommodate this requirement by using an IMU. Simulation and experimental results shows the effectiveness of such modification.