原理图片：

源码：

%**********************************************************************

%功能：    利用Bancroft算法实现2维和3维定位结果及测量偏差

%作者：何成文（中科院测地所）20180520

%参考文献：

%       [1].一种全球定位系统接收机单点定位的新算法及数值试验[J].

%参数说明：

%   L：基站坐标-m基站*n坐标

%   d：对应基站的距离观测值-列向量

%   m：2维定位 or 3维定位

%   P：未知点解算结果-列向量

%   b：观测值偏差-列向量

% 代码调试成功！20180520

%**********************************************************************

function [P,b]=Bancroft_cwhe(L,d,m)

N=length(L(:,1));                                %基站数目！

if  m==2

    h=zeros(3,1);

    M=diag([1 1 -1]);

    L=L(:,1:2);

    B=[L d];                                   　　%参考文献[1]中公式（8）的实现！

    tau=ones(N,1);                              　%参考文献[1]中公式（8）的实现！

    aerfa=zeros(N,1);                            %参考文献[1]中公式（8）的实现！

    for i=1:N

        aerfa(i,1)=L(i,1)*L(i,1)+L(i,2)*L(i,2)-d(i,1)*d(i,1);

    end

    aerfa=aerfa/2;

    wuyon=inv(B'*B)*B';                         　　　　%参考文献[1]中公式（12）的实现！

    A=lorentz_bancro(wuyon*tau,wuyon*tau);        %参考文献[1]中公式（12）的实现！

    wuyon2=lorentz_bancro(wuyon*tau,wuyon*aerfa);

    BB=2*[wuyon2-1];                           　　　　　%参考文献[1]中公式（12）的实现！

    C=lorentz_bancro(wuyon*aerfa,wuyon*tau);       %参考文献[1]中公式（12）的实现！

    Nmuda1=[-BB+sqrt(BB^2-4*A*C)]/(2*A);

    Nmuda2=[-BB-sqrt(BB^2-4*A*C)]/(2*A);

    Nmuda=max([Nmuda1 Nmuda2]);

    h=M*inv(B'*B)*(Nmuda1*B'*tau+B'*aerfa);%获取定位结果和偏差

    P=[h(1) h(2)]';

    b=h(3);

elseif m==3                            %三维定位！

    h=zeros(4,1);

    M=diag([1 1 1 -1]);

    L=L(:,1:3);

    B=[L d];                           　%参考文献[1]中公式（8）的实现！

    tau=ones(N,1);                      %参考文献[1]中公式（8）的实现！

    aerfa=zeros(N,1);                   %参考文献[1]中公式（8）的实现！

    for i=1:N

        aerfa(i,1)=L(i,1)*L(i,1)+L(i,2)*L(i,2)-d(i,1)*d(i,1);

    end

    aerfa=aerfa/2;

    wuyon=inv(B'*B)*B';                      　　　　　　 %参考文献[1]中公式（12）的实现！

    A=lorentz_bancro(wuyon*tau,wuyon*tau);      　　%参考文献[1]中公式（12）的实现！

    BB=2*[lorentz_bancro(wuyon*tau,wuyon*aerfa)-1];%参考文献[1]中公式（12）的实现！

    C=lorentz_bancro(wuyon*aerfa,wuyon*tau);     　 %参考文献[1]中公式（12）的实现！

    Nmuda1=[-BB+sqrt(BB^2-4*A*C)]/(2*A);

    Nmuda2=[-BB-sqrt(BB^2-4*A*C)]/(2*A);

    Nmuda=max([Nmuda1 Nmuda2]);

    h=M*inv(B'*B)*(Nmuda*B'*tau+B'*aerfa);        %获取定位结果和偏差

    P=[h(1) h(2) h(3)]';

    b=h(4);

end

%*******************************************代码调试成功！20180520

结论：应用于UWB室内定位技术方面，精度不高！

PDF文档下载链接：026-Bancroft算法原理20180520.pdf