带气压高度的三点法定位函数（MATLAB，含源代码）

程序介绍

输入待定位节点与各个锚节点的距离、锚节点位置、，输出待测点位置

程序源码

function [p_out] = triposition_Ver2(dairheight,R_calcu,baseP)
% Ver2：输入添加气压高度值用于校正距离
% Evand/2024年3月19日
% airghight 由气压高度计算的锚节点-待测节点的相对高度，按行向量排布
% R_calcu   距离，每个时刻的若干个距离按行向量排布
% baseP     锚点位置，每个锚点坐标按行形式排列
[baseX_,baseY_,baseZ_] = deal(baseP(:,1),baseP(:,2),baseP(:,3));
height = sum(W*(baseZ_-dairheight'))/trace(W); %加权计算的气压高度（精度较差仅用于校正R_calcu）
%% 根据高度修正R_calcu
R_calcu_correct = (R_calcu.^2-height^2).^0.5;
%% WLS方法
H = [
    baseX_(2)-baseX_(1),baseY_(2)-baseY_(1);
    baseX_(3)-baseX_(1),baseY_(3)-baseY_(1);
    baseX_(4)-baseX_(1),baseY_(4)-baseY_(1)];
for i=1
    % HX=a
    a = 0.5*[
        baseX_(2).^2+baseY_(2).^2-R_calcu_correct(i,2).^2-baseX_(1).^2-baseY_(1).^2+R_calcu_correct(i,1).^2;
        baseX_(3).^2+baseY_(3).^2-R_calcu_correct(i,3).^2-baseX_(1).^2-baseY_(1).^2+R_calcu_correct(i,1).^2;
        baseX_(4).^2+baseY_(4).^2-R_calcu_correct(i,4).^2-baseX_(1).^2-baseY_(1).^2+R_calcu_correct(i,1).^2];
    %     p_out(i,:) = (pinv(H)*a)'; %伪逆求法
%         (H'*H)^(-1)*H'*a;
        X(:,i) = (H'*H)^(-1)*H'*a; %左逆求法
end
p_out = [X;height]'; %【注意】输出的x和y精度较高，高度精度不高，在有气压高度的情况下，不建议使用这个高度
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

输出结果

【注】输出的x和y精度较高，高度精度不高，在有气压高度的情况下，不建议使用这个高度