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.youkuaiyun.com/qq_24133491/article/details/79056761 *

2. Improved trilateration algorithm

当选取的三个锚点共线时，或选取的三个锚点不共线但三圈不相交或不相交时，由于距离估计误差，传统的三边测量法无法搜索出可行解。
改进的三边测量法的基本原理如下：
1）构建误差函数如下，则误差函数最小时即可得到目标节点位置

（x,y)是未知点,（ai,bi)，di分别是锚点的位置和半径，定义
gi(x,y)关于x和y的导数如下
根据泰勒展开，可得

因此，误差函数可改写成
为了使得f(x,y)的值最小，可使得f(x,y)对x的导数为0，且f(x,y)对y的导数为0。
令

则可得
令
（注：这个公式有点错误，仅仅这个公式中的S改写成且仅仅这个公式中的T改写成
）

可得
上述式子可以表达为

值得注意的是，由于进行泰勒展开，高阶部分被忽视了，所以会有误差，为了进一步提高精度，可以将上式改写成迭代的形式，即

迭代终止条件设置为
（注：这里作者没有介绍迭代公式的由来，结合文章的内容，本人做了一些推测，仅供参考，也欢迎大家批评指正。结合迭代终止条件可知，当此时的迭代结果与上一次的迭代结果非常相近迭代终止，我们可以得到最后的结果Xm=Xm-1,Ym=Ym-1；结合前文的求偏导公式可知，最后得到的值必然满足Xm=X’,Ym=Y’。因此得到Xm=Xm-1=X’,Ym=Ym-1=Ym-1，由于Xm=alfa(X’,Y’)，Ym=beta(X’,Y’)，所以可以用Xm-1取代公式中的X’，Ym-1取代公式中的Y’，固有最终的迭代公式Xm=alfa(X’,Y’)=alfa(Xm-1,Ym-1)，Ym=alfa(X’,Y’)=beta(Xm-1,Ym-1)）
在论文中，作者还添加了一些说明
上述思想主要来自于论文
B. Yang, L. Guo, R. Guo, M. Zhao, and T. Zhao, “A novel
trilateration algorithm for rssi-based indoor localization,”
IEEE Sensors, vol. 20, no. 14, pp. 8164 – 8172, 2020.