Trilateration calculation