22 lines
268 B
Matlab
22 lines
268 B
Matlab
% 迭代式多边测距
|
|
|
|
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
|
|
|
|
|