无源定位入门（一）TDOA（4）TDOA定位模块代码

TDOA定位模块代码

function [POS1,POS2,POS3,POS4] = TDOA_Location(r,bs,Q,c)
%% TDOA定位定位Chan算法
%*********************************************************
%  CHAN算法,假设移动台与各基站位置较近,需进行三四WLS计算

%仅针对二维目标定位场景
%  输入参数:
%            r(N-1×1): TDOA距离差测量值，N为基站数
%            Pbs(N×2): 基站的坐标,第一列为X,第二列为Y;参考基站坐标位于第一行
%  输出参数: 
%            POS1(2X1):定位结果1
%            POS2(2X1):定位结果2
%            POS3(2X1):定位结果3
%            POS4(2X1):定位结果4
%c=3e8;       %光速
N = size(bs,1);
K = bs(:,1).^2 + bs(:,2).^2;   %K1和Ki,i=2,...,M
ha = 0.5*(r.^2-K(2:N)+K(1));   %h矩阵
Ga = -[bs(2:N,1)-bs(1,1) bs(2:N,2)-bs(1,2) r]; %Ga矩阵

%% 第一次WLS结果（远场模型算法，粗糙模型）
Za1 = inv(Ga.'*inv(Q)*Ga)*Ga.'*inv(Q)*ha;
WLS1_far=Za1(1:2);     %第一步WLS远场模型结果

%% 第一次WL结果（近场模型算法，精确模型）
W1=Za1(1)*ones(N-1,1);
W2=Za1(2)*ones(N-1,1);
Ba=diag(sqrt(((W1-bs(2:N,1)).^2)+((W2-bs(2:N,2)).^2)));%B矩阵，用Za1结果来估计  
Fa = c^2*Ba*Q*Ba;      %F=c^2*B*Q*B矩阵

Za2 = inv(Ga.'*inv(Fa)*Ga)*Ga.'*inv(Fa)*ha;
WLS1_near=Za2(1:2);    %第一步WLS近场模型结果

%% 第二次WLS结果（近场模型算法，精确模型）
W1=Za2(1)*ones(N-1,1);
W2=Za2(2)*ones(N-1,1);
Ba=diag(sqrt(((W1-bs(2:N,1)).^2)+((W2-bs(2:N,2)).^2)));%更新B矩阵，用Za2结果来估计  
Fa = c^2*Ba*Q*Ba;            %更新F矩阵

Gb = [1 0;0 1;1 1];          %Ga'矩阵
Bb = diag([Za2(1)-bs(1,1), Za2(2)-bs(1,2), norm(Za2(1:2)-bs(1,:)')]); %B'矩阵 
cov_Za=inv(Ga.'*inv(Fa)*Ga); %cov(Za)
Fb = 4*Bb*cov_Za*Bb;         %F'=4B'cov(za)B'
h=[(Za2(1)-bs(1,1))^2;(Za2(2)-bs(1,2))^2;(Za2(3))^2]; %h'矩阵  
Zb1= inv(Gb.'*inv(Fb)*Gb)*Gb.'*inv(Fb)*h;   
WLS2_near=sqrt(Zb1)+bs(1,:)';%第二步WLS近场模型结果

%% 第二次WLS结果（远场模型算法，粗糙模型） 
Bb = diag([Za1(1)-bs(1,1), Za1(2)-bs(1,2), norm(Za1(1:2)-bs(1,:)')]); %B'矩阵  
Zb2= inv(Gb.'*inv(Bb)*Ga.'*inv(Q)*Ga*inv(Bb)*Gb)*Gb.'*inv(Bb)*Ga.'*inv(Q)*Ga*inv(Bb)*h;    
WLS2_far=sqrt(Zb2)+bs(1,:)';                %第二步WLS远场模型结果

%% 输出结果
POS1=WLS1_near;
POS2=WLS1_far;
POS3=WLS2_near;
POS4=WLS2_far;