SLAM aided Inertial Navigation System