基于伪逆的三点法测距，MATLAB源代码（MATLAB函数）

程序介绍

这是一个函数，输入距离和已知点位置，输出未知点的位置。
已知点三个，未知点一个，已知点和未知点的距离也是有三个。

完整源代码

function [p_out] = triposition(R_calcu,baseP)
% p = [5,5;10,9;15,12;20,22;25,3]; %real location
baseX_ = baseP(:,1);
baseY_ = baseP(:,2);
baseZ_ = baseP(:,3);
H = [
    baseX_(2)-baseX_(1),baseY_(2)-baseY_(1),baseZ_(2)-baseZ_(1);
    baseX_(3)-baseX_(1),baseY_(3)-baseY_(1),baseZ_(3)-baseZ_(1)];
    for i=1
    % HX=a
    a = 0.5*[
        baseX_(2).^2+baseY_(2).^2+baseZ_(2).^2-R_calcu(i,2).^2-baseX_(1).^2-baseY_(1).^2-baseZ_(1).^2+R_calcu(i,1).^2;
        baseX_(3).^2+baseY_(3).^2+baseZ_(3).^2-R_calcu(i,3).^2-baseX_(1).^2-baseY_(1).^2-baseZ_(1).^2+R_calcu(i,1).^2];
    p_out(i,:) = (pinv(H)*a)';

    end
end

代码测试

未完待续