Fully implemented Trilateration

Now working, woohoo!