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