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
   Ga(i,1) = -BS(1,i+1);
   Ga(i,2) = -BS(2,i+1);
   Ga(i,3) = -BS(3,i+1);
   Ga(i,4) = -R(i);
end

%Q为TDOA系统的协方差矩阵
Q = cov(R);
%za,距离较远时
za1 = pinv(Ga'*pinv(Q)*Ga)*Ga'*pinv(Q)*h';

%% 第二次WLS
%h2
X1 = BS(1,1);
Y1 = BS(2,1);
Z1 = BS(3,1);
h2 = [
      (za1(1,1) - X1)^2;
      (za1(2,1) - Y1)^2;
      (za1(3,1) - Z1)^2;
       za1(4,1)^2
      ];
  
%Ga2
Ga2 = [1,0,0;0,1,0;0,0,1;1,1,1];

%B2
B2 = [
      za1(1,1)-X1,0,0,0;
      0,za1(2,1)-Y1,0,0;
      0,0,za1(3,1)-Z1,0;
      0,0,0,za1(4,1)
      ];
%za2
za2 = pinv( Ga2' * pinv(B2) * Ga' * pinv(Q) * Ga * pinv(B2) * Ga2) * (Ga2' * pinv(B2) * Ga' * pinv(Q) * Ga * pinv(B2)) * h2;

%zp
zp(1,1) = abs(za2(1,1))^0.5+X1;
zp(2,1) = abs(za2(2,1))^0.5+Y1;
zp(3,1) = abs(za2(3,1))^0.5+Z1;
end

% 功能：基于chan算法的TDOA三维定位的MSE曲线
clear;
clc;
account_test =1000;
Counter_Size=account_test;
Zp_mean1=zeros(2,6);
Zp_mse1=zeros(1,6);
M=7;
k=1;
c = 3*10^5;                      % 单位km
MS = [1,2.5,1];

for Noise_db = -16:2:-6                 % 信道所受到的噪声干扰
    Sigma = 10^(Noise_db./10)/c; 
    
    for m = 1:1:Counter_Size%%%%%%%%%%%%%%%%%%%%cishu 
        for i = 1:1:M-1
            Noise(i, 1) = gngauss(Sigma);
        end
          [ zp ] = Chan_3(Noise,MS);
          Zp(:,m) = zp;
    end 

   Zp_all(:,1) = 0;
   Zp_mse_all = 0;
   for i = 1:1:Counter_Size
      Zp_all = Zp_all + Zp( :, i);
      Zp_mse_all = Zp_mse_all + (Zp(1,i) - MS(1,1))^2 + (Zp(2,i) - MS(1,2))^2 + (Zp(3,i) - MS(1,3))^2;
   end
   Zp_mean= Zp_all / Counter_Size ;   % 计算chan所得的均值
   Zp_mean1(1:1:3,k) =Zp_mean;

   Zp_mse  = Zp_mse_all / Counter_Size ;  % 计算chan所得的均方误差MSE
   Zp_mse1(1,k)=Zp_mse;
   
   
   k=k+1;

end
Zp_mean1
Zp_mse1
plot(-16:2:-6,Zp_mse1,'bo--')
xlabel('10lg(cσ)/dB')
ylabel('均方误差MSE/km')
legend('Chan算法')

测试结果