ZhanLi
/
UWBIns
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
UWBIns/lib/multilateration.m

82 lines
3.0 KiB
Matlab
Raw Normal View History

first commit
2025-04-16 20:15:33 +08:00
function pos = multilateration(sv_pos, pos, pr, dim)
%% <20><>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD>߲<EFBFBD><DFB2><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǩλ<C7A9><CEBB>
% M x N: M:ά<><CEAC> 2 OR 3 N:<3A><>վ<EFBFBD><D5BE><EFBFBD><EFBFBD>
% sv_pos: <20><>վλ<D5BE><CEBB> M x N
% pos: M x 1
% pr: α<><CEB1> N x 1
% dim : 2 or 3 : 2: <20><>ά<EFBFBD><CEAC>λ 3: <20><>ά<EFBFBD><CEAC>λ
B1 = 0.1; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>λ<EFBFBD>ø<EFBFBD><C3B8><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD>ڴ<EFBFBD>ֵʱֹͣ<CDA3><D6B9>
END_LOOP = 100; % <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD>в<D0B2><EEA3A8>Ϊ<EFBFBD>ϴ<EFBFBD>ֵ<EFBFBD><D6B5>
sv_num = size(sv_pos, 2); % <20><>վ<EFBFBD><D5BE><EFBFBD><EFBFBD>
max_retry = 5; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
lpos = pos; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD>λ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>ڵ<EFBFBD><DAB5><EFBFBD>ʧ<EFBFBD><CAA7>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>
% <20><><EFBFBD><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><D5BE><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>˳<EFBFBD><CBB3><EFBFBD>
% <20><>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% 2D<32><44>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><33><EFBFBD>ǹ<EFBFBD><C7B9>߻<EFBFBD>վ<EFBFBD><D5BE>
% 3D<33><44>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>4<EFBFBD><34><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><D5BE>
if sv_num < 3
return
end
while (END_LOOP > B1 && max_retry > 0) % ѭ<><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD>ø<EFBFBD><C3B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵB1<42><31>δ<EFBFBD><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <20><><EFBFBD>õ<EFBFBD>ǰλ<C7B0><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>վ<EFBFBD><D5BE>ŷ<EFBFBD>Ͼ<EFBFBD><CFBE><EFBFBD>
r = vecnorm(sv_pos - pos);
% <20><><EFBFBD><EFBFBD>H<EFBFBD><48><EFBFBD><EFBFBD>
H = (sv_pos - pos) ./ r; % <20><>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> ./<2F>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>ӦԪ<D3A6><D4AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
if dim == 2
H = [H [0 0 -1]']; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2Dģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶<EFBFBD>Լ<EFBFBD><D4BC>
end
H =-H'; % ת<>ò<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>H <20><>α<EFBFBD><CEB1><EFBFBD><EFBFBD>λ<EFBFBD>õ<EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ſɱȾ<C9B1><C8BE>󣩣<EFBFBD><F3A3A9A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><D4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
% <20><><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD><EFBFBD>dp
dp = (pr - r)'; % α<><CEB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD> = <20><><EFBFBD><EFBFBD>ֵ - Ԥ<><D4A4>ֵ
if dim == 2
dp = [dp; 0]; % <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2Dģʽ<C4A3><CABD><EFBFBD><EFBFBD><EFBFBD>Ӹ߶Ȳв<C8B2>Լ<EFBFBD><D4BC>
end
% <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD>루λ<EBA3A8><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
delta = (H'*H)^(-1)*H'*dp; % <20><>С<EFBFBD><D0A1><EFBFBD>˽<EFBFBD><CBBD><EFBFBD>
%<25><><EFBFBD><EFBFBD><EFBFBD>в<D0B2><EEA3AC><EFBFBD>жϲв<CFB2><D0B2><EFBFBD>С<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
% <20>в<EFBFBD><D0B2><EFBFBD><EFBFBD>ǡ<EFBFBD><C7A1><EFBFBD><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5>ȥ<EFBFBD><C8A5><EFBFBD><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD>IJ<EFBFBD><C4B2>࣬<EFBFBD><E0A3AC><EFBFBD><EFBFBD><EFBFBD>㵱ǰ<E3B5B1><C7B0><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD><EFBFBD>
END_LOOP = norm(delta); % ŷ<><C5B7><EFBFBD><EFBFBD><EFBFBD>÷<EFBFBD><C3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ij<EFBFBD><C4B3>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE><EFBFBD>θ<EFBFBD><CEB8>µķ<C2B5><C4B7>ȡ<EFBFBD>
%<25><><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
pos = pos + delta;
max_retry = max_retry - 1; % <20><><EFBFBD><EFBFBD>ʣ<EFBFBD><CAA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
%<25><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>λʧ<CEBB><CAA7>
if(max_retry == 0 && END_LOOP > 10)
pos = lpos; % <20>ͻ<EFBFBD><CDBB>ع<EFBFBD><D8B9><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>ε<EFBFBD><CEB5><EFBFBD><EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD>
return;
end
end
end
%
% % <20><>С<EFBFBD><D0A1><EFBFBD>˷<EFBFBD><CBB7><EFBFBD><EFBFBD>߲<EFBFBD><DFB2><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