Reorganize renata's code.
The qIntersection only works with at least 3 readings.
Implements the contractor by pinger's angle, but need to implement atan2 at Ibex. Not tested yet.