Add euclidean distance funciton for simple nav