forked from ZhanLi/UWBIns
22 lines
268 B
Mathematica
22 lines
268 B
Mathematica
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
|
|||
|
|
function p = triangulate(anchor_pos, p, pr)
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
n = size(anchor_pos, 2);
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ǰλ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD>ľ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
r = vecnorm(anchor_pos - p);
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>H<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
H = (anchor_pos - p) ./ r;
|
|||
|
|
H =-H';
|
|||
|
|
|
|||
|
|
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
p = p + (H'*H)^(-1)*H'*(pr - r)';
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|