ZhaiShuaiShuai/UWBIns
1
0
Fork 0
forked from ZhanLi/UWBIns
Code Pull Requests Activity
UWBIns/lib/multilateration.m

82 lines
3.0 KiB
Mathematica
Raw Normal View History

first commit
2025-04-16 20:15:33 +08:00
function pos = multilateration(sv_pos, pos, pr, dim)
%% <EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩλ<EFBFBD><EFBFBD>
% M x N: M:ά<EFBFBD><EFBFBD> 2 OR 3 N:<EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% sv_pos: <EFBFBD><EFBFBD>վλ<EFBFBD><EFBFBD> M x N
% pos: M x 1
% pr: α<EFBFBD><EFBFBD> N x 1
% dim : 2 or 3 : 2: <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD>λ 3: <EFBFBD><EFBFBD>ά<EFBFBD><EFBFBD>λ
B1 = 0.1; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>λ<EFBFBD>ø<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ڴ<EFBFBD>ֵʱֹͣ<EFBFBD><EFBFBD>
END_LOOP = 100; % <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD>Ϊ<EFBFBD>ϴ<EFBFBD>ֵ<EFBFBD><EFBFBD>
sv_num = size(sv_pos, 2); % <EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
max_retry = 5; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lpos = pos; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD>λ<EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><EFBFBD><EFBFBD>˳<EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD>Ϊ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% 2D<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><EFBFBD>߻<EFBFBD>վ<EFBFBD><EFBFBD>
% 3D<EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>4<EFBFBD><EFBFBD><EFBFBD>ǹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD>
if sv_num < 3
return
end
while (END_LOOP > B1 && max_retry > 0) % ѭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ø<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵB1<EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD>ǰλ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><EFBFBD>ŷ<EFBFBD>Ͼ<EFBFBD><EFBFBD><EFBFBD>
r = vecnorm(sv_pos - pos);
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD>H<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
H = (sv_pos - pos) ./ r; % <EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ./<EFBFBD>ǽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ӦԪ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if dim == 2
H = [H [0 0 -1]']; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2Dģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>Լ<EFBFBD><EFBFBD>
end
H =-H'; % ת<EFBFBD>ò<EFBFBD>ȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>H <EFBFBD><EFBFBD>α<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>õ<EFBFBD>ƫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ſɱȾ<EFBFBD><EFBFBD>󣩣<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>dp
dp = (pr - r)'; % α<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> = <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ - Ԥ<EFBFBD><EFBFBD>ֵ
if dim == 2
dp = [dp; 0]; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2Dģʽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӹ߶Ȳв<EFBFBD>Լ<EFBFBD><EFBFBD>
end
% <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD>루λ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
delta = (H'*H)^(-1)*H'*dp; % <EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˽<EFBFBD><EFBFBD><EFBFBD>
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><EFBFBD><EFBFBD>жϲв<EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>Ƿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹͣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <EFBFBD>в<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD>ȥ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><EFBFBD><EFBFBD>IJ<EFBFBD><EFBFBD>࣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㵱ǰ<EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
END_LOOP = norm(delta); % ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><EFBFBD>ȣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><EFBFBD><EFBFBD>θ<EFBFBD><EFBFBD>µķ<EFBFBD><EFBFBD>ȡ<EFBFBD>
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><EFBFBD>
pos = pos + delta;
max_retry = max_retry - 1; % <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʧ<EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><EFBFBD><EFBFBD><EFBFBD>λʧ<EFBFBD><EFBFBD>
if(max_retry == 0 && END_LOOP > 10)
pos = lpos; % <EFBFBD>ͻ<EFBFBD><EFBFBD>ع<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD>
return;
end
end
end
%
% % <EFBFBD><EFBFBD>С<EFBFBD><EFBFBD><EFBFBD>˷<EFBFBD><EFBFBD><EFBFBD><EFBFBD>߲<EFBFBD><EFBFBD><EFBFBD>
%
% function pos = ch_multilateration(anchor_pos, pos, pr)
%
% pr = pr(1:size(anchor_pos, 2));
%
% b = vecnorm(anchor_pos).^(2) - pr.^(2);
% b = b(1:end-1) - b(end);
% b = b';
%
% A = anchor_pos - anchor_pos(:,end);
% A = A(:,1:end-1)'*2;
%
% pos = (A'*A)^(-1)*A'*b;
%
%
% end
Copy Permalink