Improved obstacle detection algorithm