MATLAB code for obstacle detection