Trilateration algorithm and improved trilateration algorithm

1. Trilateration algorithm

基于测距的定位技术仿真——三边测量法 (Trilateration algorithm)
基本原理示意图如下所示：
1）已知三点位置（x1,y1)，(x2,y2)，(x3,y3)；
2）已知未知点（x0,y0)到三点距离d1，d2，d3；
3）以d1，d2，d3为半径作三个圆，根据勾股定理，有
(x1-x0)^2 + (y1-y0)^2 =d1^2 (1)
(x2-x0)^2 + (y2-y0)^2 =d2^2 (2)
(x3-x0)^2 + (y3-y0)^2 =d3^2 (3)
联立上述三个公式必然可以求解出x0,y0。
（注：可以直接公式求解，直接算出x0,y0的表达式，或者也可以利用MATLAB构造关于x0,y0的方程组求解。）
*上述内容主要来自于https://blog.csdn.net/qq_24133491/article/details/79056761 *

2. Improved trilateration algorithm

当选取的三个锚点共线时，或选取的三个锚点不共线但三圈不相交或不相交时，由于距离估计误差，传统的三边测量法无法搜索出可