% 迭代式多边测距
function p = triangulate(anchor_pos, p, pr)
% 基站个数
n = size(anchor_pos, 2);
% 获得当前位置与各个基站的距离
r = vecnorm(anchor_pos - p);
% 求得H矩阵
H = (anchor_pos - p) ./ r;
H =-H';
% 迭代用户距离
p = p + (H'*H)^(-1)*H'*(pr - r)';
end