GDOP计算方法（几何精度因子）

假设一共有 $\mathrm{k}$ 个基站，定义 $(\mathrm{x}, \mathrm{y}, \mathrm{z})$ 为每使用k个基站解算出的标签的位置, $\left(A x_{k}, A y_{k}, A z_{k}\right)$ 表示第k个基站的坐标, $d_{k}$ 表示标签到第K个基站的测量距离。
GDOP 通过下式得出:

$$\begin{gathered} f_{\mathrm{k} 1}=\frac{\mathrm{x}-A x_{k}}{d_{k}}, f_{k 2}=\frac{\mathrm{y}-A y_{k}}{d_{k}}, f_{k 3}=\frac{\mathrm{z}-A z_{k}}{d_{k}} \\ G=\left[\begin{array}{cccc} f_{11} & f_{12} & f_{13} & 1 \\ f_{21} & f_{22} & f_{23} & 1 \\ \vdots & \vdots & & \\ f_{k 1} & f_{k 2} & f_{k 3} & 1 \end{array}\right] \\ \text { GDOP }=\sqrt{\operatorname{tr}\left(G^{T} G\right)^{-1}} \end{gathered}$$

$\mathrm{G}$ 是距离测量值相对于位置量的偏导数构成的矩阵(Jacobian）

代码如下：
def get_toa_gdop(bs_list, xyz):
"""
compute the gdop of the toa
:param bs_list: the base station coordinate
:param xyz: the location coordinate of the tag
:return: the gdop value
"""

bs_n = len(bs_list)
if bs_n < 3:
    return None
h1 = np.mat(np.zeros((bs_n, 3)))

for i in range(0, len(bs_list)):
    ri = np.sqrt((bs_list[i][0] - xyz[0]) ** 2 +
                 (bs_list[i][1] - xyz[1]) ** 2)
    if ri == 0:
        h1[i, 0] = 0
        h1[i, 1] = 0
        h1[i, 2] = 1

    else:
        h1[i, 0] = (bs_list[i][0] - xyz[0]) / ri
        h1[i, 1] = (bs_list[i][1] - xyz[1]) / ri
        h1[i, 2] = 1

m_tmp = h1.T * h1
gdop_value = None

try:
    g = m_tmp.I
    gdop_value = np.sqrt(g[0, 0] + g[1, 1])
finally:
    return gdop_value