假设一共有 \(\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