A naive matlab implementation of ideal TDOA algorithm

原始算法描述参见：https://blog.csdn.net/lpsl1882/article/details/51519303

% 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);

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

disp([r1, r2, r3]);

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