MATLAB实现基于Chan氏算法的二维TDOA定位

Chan算法主要用到的公式
第一次WLS


当移动台与基站距离较远时，可近似为

第二次WLS

当移动台与基站距离较远时，可近似为

最终移动台定位的结果为

function [X] = Chan2(BSN,BS,R)

%% 第一次WLS

 %k=X^2+Y^2
for i = 1:BSN                      %BSN为基站个数
    k(i) = BS(1,i)^2 + BS(2,i)^2;  %BS为基站坐标
end

%h = 1/2(Ri^2-ki+k1)
for i =1:BSN-1
    h(i) = 0.5*(R(i)^2 - k(i+1) + k(1));  %注意k(i+1)
end


%Ga = [Xi,Yi,Ri]
for i = 1:BSN-1
    Ga(i,1) = -BS(1,i+1);
    Ga(i,2) = -BS(2,i+1);
    Ga(i,3) = -R(i);
end

%Q为TDOA系统的协方差矩阵
Q = cov(R);

%MS与BS距离较远时
za = inv(Ga' * inv(Q) * Ga) * Ga' * inv(Q) * h'

%% 第二次WLS
%h'
X1 = BS(1,1);
Y1 = BS(2,1);
h2 = [
    (za(1,1) - X1)^2;
    (za(2,1) - Y1)^2;
     za(3,1)^2
      ];

%Ga'
Ga2 = [1,0;0,1;1,1];

%B'
B2 = [
      za(1,1)-X1,0,0;
      0,za(2,1)-Y1,0;
      0,0,za(3,1)
      ];
  
%za',距离较远时
za2 = inv( Ga2' * inv(B2) * Ga' * inv(Q) * Ga * inv(B2) * Ga2) * (Ga2' * inv(B2) * Ga' * inv(Q) * Ga * inv(B2)) * h2;

zp(1,1) = abs(za2(1,1))^0.5 + X1;
zp(2,1) = abs(za2(2,1))^0.5 + Y1;

X = zp;
 

end

TDOA定位测试代码

BSN = 4;%基站数目
%各个基站的位置，2*BSN的矩阵存储，每一列是一个坐标
BS = [0 , sqrt(3) , 0.5*sqrt(3) , -0.5*sqrt(3) , -sqrt(3) , -0.5*sqrt(3) , 0.5*sqrt(3); 
      0 ,       0 ,         1.5 ,         1.5  ,        0 ,        -1.5  ,        -1.5];
BS = BS(:,1:BSN);
BS = BS .* 50;

%MS的实际距离,为待测量值
MS = [30,30];

% R0为无噪声情况下各个BS与MS的距离
for i = 1:BSN
    R0(i) = sqrt((BS(1,i)-MS(1))^2 + (BS(2,i)-MS(2))^2);
end
%噪声方差
Noise = 1;
%R（i）,是加上噪声后，BSi与BS1到MS的距离差，实际中因由TDOA*c算的
for i = 1:BSN-1
    R(i) = R0(i+1) - R0(1) + Noise * randn(1);
end

X = Chan2(BSN,BS,R)

结果