New localization algorithm

Quick but not efficient in case of noise.
There is a NaN result if the position is exactly at the middle between
two beacons.