trilateration algorithm test and bug fixes