TDOA_CHAN三维定位（仅供参考）

前言

根据syy_1797提供的二维条件下进行TDOA定位的程序，运用到三维定位中，修改了细节 ( 通过校验先验信息 ) 使得定位模糊现象减少。我也是初学者，有什么不足之处请大家多多批评指正。

---

一、TDOA_CHAN定位解算的原理

参考CSDN博主syy_1797的无源定位入门系列

二、具体代码

1.主程序

代码如下：

clear all; 
clc; 

BS1=[2,2,2];
BS2=[2,-2,2];
BS3=[2,2,-2];
BS4=[2,-2,-2];
BS5=[-2,2,2];
BS6=[-2,-2,2];
BS7=[-2,2,-2];
BS8=[-2,-2,-2];
MS=[300,220,260]; 
A=[BS1;BS2;BS3;BS4;BS5;BS6;BS7;BS8];

std_var=[1e-9]; %TDOA测量值精度范围
number=10000; 
c=3e8;

for j=1:length(std_var)  
    num=0;
    [m,~]=size(A);
    Q=ones(m-1)*0.5*std_var(j)^2+diag(ones(m-1,1)*0.5*std_var(j)^2);
    result=0;
    for i=1:number 
          r1=A-ones(8,1)*MS;
          r2=(sum(r1.^2,2)).^(1/2); 
          r=r2(2:end,:)-ones(7,1)*r2(1,:)+std_var(j)*randn(7,1);
          [~,~,~,result_far]=TDOA_Location(r,A,Q,c); 
          result=result+result_far;
    end 
    result=result/number;
end

figure(1);
plot3(MS(:,1),MS(:,2),MS(:,3),'r*');
hold on;
plot3(result(1,:),result(2,:),result(3,:),'b--o');
hold on;
plot3(A(:,1),A(:,2),A(:,3),'g*');
hold on;
legend('真实位置','定位位置','卫星位置');
xlabel('X m');
ylabel('Y m');
zlabel('Z m');

2.定位解算函数

代码如下：

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

%仅针对二维目标定位场景
%  输入参数:
%            r(N-1×1): TDOA距离差测量值，N为基站数
%            bs(N×2): 基站的坐标,第一列为X,第二列为Y;参考基站坐标位于第一行
%  输出参数: 
%            POS1(2X1):定位结果1
%            POS2(2X1):定位结果2
%            POS3(2X1):定位结果3
%            POS4(2X1):定位结果4
%c=3e8;       %光速
WLS2_near_out=zeros(3,1);
WLS2_far_out=zeros(3,1);

N = size(bs,1);
K = bs(:,1).^2 + bs(:,2).^2 + bs(:,3).^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) bs(2:N,3)-bs(1,3) r]; %Ga矩阵

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

%% 第一次WL结果（近场模型算法，精确模型）
W1=Za1(1)*ones(N-1,1);
W2=Za1(2)*ones(N-1,1);
W3=Za1(3)*ones(N-1,1);
Ba=diag(sqrt(((W1-bs(2:N,1)).^2)+((W2-bs(2:N,2)).^2)+((W3-bs(2:N,3)).^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:3);    %第一步WLS近场模型结果


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

Gb = [1 0 0;0 1 0;0 0 1;1 1 1];          %Ga'矩阵
Bb = diag([Za2(1)-bs(1,1), Za2(2)-bs(1,2),Za2(3)-bs(1,3), norm(Za2(1:3)-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)-bs(1,3))^2;(Za2(4))^2]; %h'矩阵  
Zb1= inv(Gb.'*inv(Fb)*Gb)*Gb.'*inv(Fb)*h; 

WLS2_near(:,1)=[sqrt(Zb1(1))+bs(1,1)';sqrt(Zb1(2))+bs(1,2)';sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,2)=[sqrt(Zb1(1))+bs(1,1)';-sqrt(Zb1(2))+bs(1,2)';sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,3)=[sqrt(Zb1(1))+bs(1,1)';sqrt(Zb1(2))+bs(1,2)';-sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,4)=[sqrt(Zb1(1))+bs(1,1)';-sqrt(Zb1(2))+bs(1,2)';-sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,5)=[-sqrt(Zb1(1))+bs(1,1)';sqrt(Zb1(2))+bs(1,2)';sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,6)=[-sqrt(Zb1(1))+bs(1,1)';-sqrt(Zb1(2))+bs(1,2)';sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,7)=[-sqrt(Zb1(1))+bs(1,1)';sqrt(Zb1(2))+bs(1,2)';-sqrt(Zb1(3))+bs(1,3)'];
WLS2_near(:,8)=[-sqrt(Zb1(1))+bs(1,1)';-sqrt(Zb1(2))+bs(1,2)';-sqrt(Zb1(3))+bs(1,3)'];

p=size(r)+1;

for i=1:p 
    for j=2:p
    Rems(j-1,i)=sqrt((bs(j,1) - WLS2_near(1,i))^2 + (bs(j,2) - WLS2_near(2,i))^2 + (bs(j,3) - WLS2_near(3,i))^2)-sqrt((bs(1,1) - WLS2_near(1,i))^2 + (bs(1,2) - WLS2_near(2,i))^2 + (bs(1,3) - WLS2_near(3,i))^2);
    end
end

for i=1:p
    num=0;
    for j=2:p
        if sign(Rems(j-1,i))*sign(r(j-1,1))==1
            num=num+1;
        end
    end
    if num>=(p-1)
        WLS2_near_out=WLS2_near(:,i);
    end
end

%% 第二次WLS结果（远场模型算法，粗糙模型） 
Bb = diag([Za1(1)-bs(1,1), Za1(2)-bs(1,2), Za1(3)-bs(1,3), norm(Za1(1:3)-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(:,1)=[sqrt(Zb2(1))+bs(1,1)';sqrt(Zb2(2))+bs(1,2)';sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,2)=[sqrt(Zb2(1))+bs(1,1)';-sqrt(Zb2(2))+bs(1,2)';sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,3)=[sqrt(Zb2(1))+bs(1,1)';sqrt(Zb2(2))+bs(1,2)';-sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,4)=[sqrt(Zb2(1))+bs(1,1)';-sqrt(Zb2(2))+bs(1,2)';-sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,5)=[-sqrt(Zb2(1))+bs(1,1)';sqrt(Zb2(2))+bs(1,2)';sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,6)=[-sqrt(Zb2(1))+bs(1,1)';-sqrt(Zb2(2))+bs(1,2)';sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,7)=[-sqrt(Zb2(1))+bs(1,1)';sqrt(Zb2(2))+bs(1,2)';-sqrt(Zb2(3))+bs(1,3)'];
WLS2_far(:,8)=[-sqrt(Zb2(1))+bs(1,1)';-sqrt(Zb2(2))+bs(1,2)';-sqrt(Zb2(3))+bs(1,3)'];

for i=1:p 
    for j=2:p
    Rems1(j-1,i)=sqrt((bs(j,1) - WLS2_far(1,i))^2 + (bs(j,2) - WLS2_far(2,i))^2 + (bs(j,3) - WLS2_far(3,i))^2)-sqrt((bs(1,1) - WLS2_far(1,i))^2 + (bs(1,2) - WLS2_far(2,i))^2 + (bs(1,3) - WLS2_far(3,i))^2);
    end
end

for i=1:p 
    num1=0;
    for j=2:p
        if sign(Rems1(j-1,i))*sign(r(j-1,1))==1
            num1=num1+1;
        end
    end
    if num1>=(p-1)
        WLS2_far_out=WLS2_far(:,i);
    end
end


%% 输出结果
POS1=WLS1_near;
POS2=WLS1_far;
POS3=WLS2_near_out;
POS4=WLS2_far_out;

---

结果如下：