Concurrent Estimation of Robot Pose and Landmark Locations in Underwater Robot

This paper describes a concurrent mapping and localization(CML) for an underwater robot where mapping indicates location estimation of landmarks. The proposed method extends Kalman filter approach of CML in 2 dimensional area to that in 3 dimensional underwater environment. The method uses measurement of range and bearing from a robot and the landmarks. The method uses linearized relationship between the robot motion velocity and locations of robot and landmarks. Also, the measurements are assumed to be linearly dependent on the robot motion. Simulation results show the feasibility of the method. The method can improve the IMU based navigation to confine the location error into a limited range and prevent the growth of error with time.