82 lines
3.0 KiB
Matlab
82 lines
3.0 KiB
Matlab
function pos = multilateration(sv_pos, pos, pr, dim)
|
||
%% 最小二乘法多边测距 解算标签位置
|
||
% M x N: M:维度 2 OR 3 N:基站个数
|
||
% sv_pos: 基站位置 M x N
|
||
% pos: M x 1
|
||
% pr: 伪距 N x 1
|
||
% dim : 2 or 3 : 2: 二维定位 3: 三维定位
|
||
|
||
B1 = 0.1; % 迭代收敛阈值（位置更新量小于此值时停止）
|
||
END_LOOP = 100; % 初始迭代残差（设为较大值）
|
||
sv_num = size(sv_pos, 2); % 基站数量
|
||
max_retry = 5; % 最大迭代次数
|
||
lpos = pos; % 保存上一次的位置，用于迭代失败时回退
|
||
|
||
% 设置约束，如果基站数量少于3，则直接退出。
|
||
% 因为解算位置需满足以下条件：
|
||
% 2D定位：至少3个非共线基站。
|
||
% 3D定位：至少4个非共面基站。
|
||
if sv_num < 3
|
||
return
|
||
end
|
||
|
||
|
||
while (END_LOOP > B1 && max_retry > 0) % 循环条件：位置更新量大于阈值B1且未超过最大迭代次数
|
||
% 获得当前位置与各个基站的欧氏距离
|
||
r = vecnorm(sv_pos - pos);
|
||
|
||
% 求得H矩阵
|
||
H = (sv_pos - pos) ./ r; % 求每个矩阵的单位方向向量， ./是将矩阵中每个对应元素逐个做除法运算
|
||
if dim == 2
|
||
H = [H [0 0 -1]']; % 如果是2D模式，添加虚拟高度约束
|
||
end
|
||
H =-H'; % 转置并取负，H 是伪距对位置的偏导数矩阵（雅可比矩阵），用于线性化非线性问题
|
||
|
||
% 构建残差向量dp
|
||
dp = (pr - r)'; % 伪距测量残差 = 测量值 - 预测值
|
||
if dim == 2
|
||
dp = [dp; 0]; % 如果是2D模式，添加高度残差约束
|
||
end
|
||
|
||
% 迭代用户距离（位置增量）
|
||
delta = (H'*H)^(-1)*H'*dp; % 最小二乘解算
|
||
|
||
%计算残差，以判断残差大小是否可以停止迭代
|
||
% 残差就是“测量值减去估计值”的差距，代表你当前估计的误差。
|
||
END_LOOP = norm(delta); % 欧几里得范数（向量的长度），表示这次更新的幅度。
|
||
|
||
%更新位置
|
||
pos = pos + delta;
|
||
max_retry = max_retry - 1; % 减少剩余迭代次数
|
||
|
||
%迭代失败 如果到达了最大迭代次数还没有收敛，则说明定位失败
|
||
if(max_retry == 0 && END_LOOP > 10)
|
||
pos = lpos; % 就会回滚到上一次迭代的结果
|
||
return;
|
||
end
|
||
|
||
end
|
||
|
||
end
|
||
|
||
|
||
%
|
||
% % 最小二乘法多边测距
|
||
%
|
||
% 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
|
||
|