% 功能：基于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算法')
测试结果
