Added sphere and got trilateration working