VFH* Algorithm in Matlab