22 lines
268 B
Matlab
22 lines
268 B
Matlab
|
% <20><><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD>߲<EFBFBD><DFB2><EFBFBD>
|
|||
|
|
|
|||
|
|
function p = triangulate(anchor_pos, p, pr)
|
|||
|
|
|
|||
|
|
% <20><>վ<EFBFBD><D5BE><EFBFBD><EFBFBD>
|
|||
|
|
n = size(anchor_pos, 2);
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD>õ<EFBFBD>ǰλ<C7B0><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD>ľ<EFBFBD><C4BE><EFBFBD>
|
|||
|
|
r = vecnorm(anchor_pos - p);
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD>H<EFBFBD><48><EFBFBD><EFBFBD>
|
|||
|
|
H = (anchor_pos - p) ./ r;
|
|||
|
|
H =-H';
|
|||
|
|
|
|||
|
|
% <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
p = p + (H'*H)^(-1)*H'*(pr - r)';
|
|||
|
|
|
|||
|
|
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
|