Trilateration

The ground truth code is done. Lets merge!