Fixed issues regarding trilateration