An implementation of SLAM with extended Kalman filter
Simultaneous localization and mapping (SLAM): part II