TDOA的Chan算法求解及MATLAB实现

在博文到达时间差测量目标位置（TDOA）定位一文中，我们推导了Chan算法的数学原理。Chan算法可将TDOA中的求解多个双曲线的非线性方程求解，化为带参数的伪线性方程，可大大降低运算量。但在求解参数$r_0$时，会存在无解，一个解，两个解三种情况。即Chan算法，存在不可定位和定位模糊的可能，在出现定位模糊时，需要我们引入新的时间差信息，对其中的一个伪解进行剔除。
仿真图如下：

下面给出Chan算法的MATLAB实现：

clear all; close all; clc;

stations = [0 0 0;-20 20 30;20 20 -40;20 10 -20;20 30 40];    %基站位置
c = 3e8; %光速

X = -100:100;  %生成目标位置
Y = 3*X.*sin(X);
N = length(X);
Locations = zeros(N,3);
tds = zeros(N,length(stations)-1);
for i = 1:N
    tds(i,:) = TD(stations*1e3,[X(i),Y(i),0]*1e3,c);
end

for i = 1:N
  Locations(i,:) = TDOA1(stations*1e3,tds(i,:),c)/1e3;
end

figure(1);
plot(X,Y,'r*');
hold on;
plot(Locations(:,1),Locations(:,2),'b--o');
legend('真实位置','定位位置');
xlabel('Km');
ylabel('Km');



%% 根据目标位置，生成观测样本（时差信息）
function [tds] = TD(stations,T,c)
N = length(stations)-1;
tds = zeros(1,N);
rs = zeros(1,N);
r0 = sqrt((T(1)-stations(1,1))^2+(T(2)-stations(1,2))^2+(T(3)-stations(1,3))^2);
for i = 1:N
    rs(i) = sqrt((T(1)-stations(i+1,1))^2+(T(2)-stations(i+1,2))^2+(T(3)-stations(i+1,3))^2);
    tds(1,i) = (rs(i)-r0)/c;
end
end


%% 根据时差，基站位置反推目标位置，即Chan算法的核心实现
function [location] = TDOA1(stations,tds,c)
    N = length(stations)-1;
    %%%%%%%%%%%%%%%
    A = zeros(N-1,3);
    for i = 1:N
        A(i,1) = stations(i+1,1)-stations(1,1);
        A(i,2) = stations(i+1,2)-stations(1,2); 
        A(i,3) = stations(i+1,3)-stations(1,3);
    end
    %%%%%%%%%%%%%%%%%%%
    B1 = (-tds*c)';  %[N-1,1],delta r
    D1 = zeros(N,1);
    for i = 1:N
        D1(i,1) = stations(i+1,1)^2+ stations(i+1,2)^2+ stations(i+1,3)^2;
    end
    L = (1/2)*(D1-(stations(1,1)^2+ stations(1,2)^2+ stations(1,2)^2)-(B1.^2));
    
    A2 = A(1:3,:);
    L2 = L(1:3,:);
    B2 = B1(1:3,:);
    
    a = det(A2);
    a1 = (det([B2,A2(:,2:3)]))/a;
    a2 = (det([A2(:,1),B2,A2(:,3)]))/a;
    a3 = (det([A2(:,1:2),B2]))/a;
    b1 = (det([L2,A2(:,2:3)]))/a;
    b2 = (det([A2(:,1),L2,A2(:,3)]))/a;
    b3 = (det([A2(:,1:2),L2]))/a;
    
    D = a1^2+a2^2+a3^2-1; %A
    c1 = (b1-stations(1,1));
    c2 = (b2-stations(1,2));
    c3 = (b3-stations(1,3));
    E = a1*c1+a2*c2+a3*c3; %B
    F =c1^2+c2^2+c3^2; %C
    
    G = sqrt(E^2-D*F);
    
    r01 =  (-E+G)/D;
    r02 = (-E-G)/D;
    location1 = [a1*r01+b1;a2*r01+b2;a3*r01+b3];
    location2 = [a1*r02+b1;a2*r02+b2;a3*r02+b3];
    
    tdsex1 = TD(stations,location1,c);
    tdsex2 = TD(stations,location2,c);
    
    if(abs(tdsex1(4)-tds(4))<1e-8)  % 引入时差信息，剔除伪解
        location = location1;
    else
        location = location2;
    end
end