基于TDOA定位算法源代码

function Estimation=Chan_TDOA_3D（BSNumXXbReal_msP）
c=3e8;
Rb = sqrt（（Real_ms（1） - Xb（1））^2+（Real_ms（2） - Xb（2））^2+（Real_ms（3）-Xb（3））^2）;%移动台到基站的真实距离
%Q = eye（BSNum）*（delta0^2）;
Q=（eye（BSNum）+ones（BSNum））/2;
%Q=chol（Q_a）;
%P= normrnd（0delta01BSNum）;%产生TDOA测量误差
Kb = sum（Xb.^2）;
R = zeros（BSNum1）;
for i=1:BSNum                %产生TDOA测量值
    R（i） = -Rb+sqrt（（Real_ms（1）- X（i1））^2+（Real_ms（2） - X（i2））^2+（Real_ms（3）-X（i3））^2）+P（i）;
end
Pbs = [Xb;X];
N = size（Pbs1）;
K = zeros（1N）;
K = Pbs（:1）.^2 + Pbs（:2）.^2+Pbs（:3）.^2;
ha = 0.5*（R.^2-K（2:N）+K（1））;
Ga = -[Pbs（2:N1）-Xb（1） Pbs（2:N2）-Xb（2） Pbs（2:N3）-Xb（3） R];

        %计算第一次WLS估计结果（远距算法）
Za=inv（Ga.‘*inv（Q）*Ga）*Ga.‘*inv（Q）*ha;
for i=1:BSNum
    Ba（ii）=sqrt（（Za（1）-X（i1））^2