Accurate SLAM With Application For Aerial Path Planning