Distance calculation working very well, MatLab script added