matlab chan算法定位,MATLAB实现基于Chan氏算法的三维TDOA定位

% 功能：基于chan算法的TDOA三维定位

function [zp] = Chan_3(Noise,MS)

%基站数目

BSN = 7;

%基站位置,每一列为一个基站位置

BS = [0, 2*sqrt(3), -2*sqrt(3), sqrt(3), -sqrt(3), -sqrt(3), sqrt(3);

0, 0, 0, 3, 3, -3, -3;

0, 0, 0, 0, 2, 2, 0];

%无噪声情况下BS到MS的距离

for i = 1:BSN

R0(i) = sqrt((BS(1,i) - MS(1))^2 + (BS(2,i) - MS(2))^2 + (BS(3,i) - MS(3))^2);

end

%噪声方差

c = 3*10^5;

%有噪声情况下BSi到MS的距离与BS1到MS的距离差，实际由TDOA*c求得

for i = 1:BSN-1

R(i) = R0(i+1) - R0(1) +c*Noise(i,1);

end

%% 第一次WLS

%k = x^2+y^2+z^2

for i =1:BSN

k(i) = BS(1,i)^2 + BS(2,i)^2 + BS(3,i)^2;

end

% h

for i = 1:BSN-1

h(i) = 0.5*(R(i)^2 - k(i+1) + k(1));

end

% Ga

for i = 1:BSN-1