Refactored trilateration based on GIS Stackoverflow Python snippet.
This is before I raised issue 1.