A naive matlab implementation of TDOA Chan‘s Method

算法描述参见：TDOA算法综述--(2)--Chan‘s Method

% About the algorithm description, refer to: 
% https://blog.csdn.net/lpsl1882/article/details/51519303.
clc; close all; clear;
echo on;
format long;

tmp = unifrnd(0,255,4,2);
x1 = tmp(1,1); y1 = tmp(1,2);  % Anchor1
x2 = tmp(2,1); y2 = tmp(2,2);  % Anchor2
x3 = tmp(3,1); y3 = tmp(3,2);  % Anchor3
x  = tmp(4,1); y  = tmp(4,2);  % Tag to be located
% x1 = 10;  y1 = 10;
% x2 = 240; y2 = 20;
% x3 = 124; y3 = 250;
% x  = 122; y  = 134;

disp([x1, y1, x2, y2, x3, y3, x, y]);

figure; hold on; axis([0,256,0,256]);
scatter(x1,y1,120,'d'); text(x1,y1,'Anchor1');
scatter(x2,y2,120,'d'); text(x2,y2,'Anchor2');
scatter(x3,y3,120,'d'); text(x3,y3,'Anchor3');
scatter(x, y, 120,'o'); text(x , y,'Tag');

plot([x1,x],[y1,y],'LineWidth',2);
plot([x2,x],[y2,y],'LineWidth',2);
plot([x3,x],[y3,y],'LineWidth',2);

% Calculate the distance between tag and anchor as the ideal distance measurement.
% In practical TDOA application, the ToA(time of arrival) of signal (sent from Tag) 
% reaching each anchor, instead of the distance, should be measured by each anchor.
% But below we use only the difference r21 and r31, which, when dividing by light speed, % is used to represent the TDOA.

r1 = distance(x1, y1, x, y);
r2 = distance(x2, y2, x, y);
r3 = distance(x3, y3, x, y);

disp([r1, r2, r3]);

% r21 represents the TDOA between anchor1 and anchor2
% r31 represents the TDOA between anchor1 and anchor3
r21 = r2 - r1;
r31 = r3 - r1;

disp([r21, r31]);

x21 = x2 - x1;
x31 = x3 - x1;
y21 = y2 - y1;
y31 = y3 - y1;

disp([x21, x31, y21, y31]);

P1_tmp  = [[x21,y21];[x31,y31]];
P1  = (-1) * inv(P1_tmp);

disp(P1_tmp);
disp(P1);

P2  = [r21; r31];

disp(P2);

K1  = x1^2 + y1^2;
K2  = x2^2 + y2^2;
K3  = x3^2 + y3^2;

disp([K1,K2,K3]);

P3  = 0.5 * [ (-K2 + K1 + r21^2);  (-K3 + K1 + r31^2) ]; 

disp(P3);

xy_esti = (P1 * P2) * r1 + P1 * P3;  

fprintf(1,'x = %g; y = %g; x_esti = %g; y_esti = %g\n', x, y, xy_esti(1), xy_esti(2));

function dist = distance(x1,y1,x2,y2)
    dist = sqrt( (x1-x2)^2 + (y1-y2)^2 );    
end

【2021-08-21】

        针对留言中几个小伙伴留下的问题，追加一点补充说明。

        这个程序描述的是TDOA算法，而TDOA算法是需要各个Anchor对Tag发出的信号的ToA测量值作为输入的。以上程序中基于Anchor和Tag的坐标计算r1,r2,r3可以看作是对各个Anchor对Tag发出的信号的ToA的测量的一个理想化表示。

        最后估计出来的结果是xy_esti，这个和已知的(x,y)（Ground Truth）之前的差距就反映了估计误差。

【2022-05-28】

        实际上R1是需要另行求解的，而且R1会存在多个解需要参考其它条件消除这个模糊性取其中最合理的一个。以上demo program跳过了这个R1的求解过程（直接用真值了）。R1的求解具体可以参考文中提到的两篇参考博客。

 

        相关博文：

        TDOA算法综述(An overview of TDOA algorithm)--(1)

        What are Triangulation, Trilateration, and Multilateration? (1)