% TDOA测距定位，二维平面， 4个锚节点的情况 % author:Evand（VX：matlabfilter，除前期达成一致外，讲解需付费） % 2024年9月25日/Ver1 clear;clc;close all; rng(0); %% 主程序 c = 3e8; %信号传输速度，即光速 range_err = 1e-9; %时钟与时间计算误差 point1 = [1,1]; baseP = [ 0,1; 0,0; 2,2; 2,0]; R_real = sqrt(diag((point1-baseP)*(point1'-baseP'))); %含噪声的距离 TDOA = R_real/c+range_err*randn; %含噪声的传播时间 R_calcu = TDOA*c; weight = eye(3); %权重设置，如果不用权重，这里不动即可 [p_out] = triposition_weight(R_calcu,baseP,weight); %% 绘图 figure; plot(point1(1),point1(2),'o'); hold on plot(p_out(1),p_out(2),'o'); scatter(baseP(:,1),baseP(:,2),'*r'); xlim([-1,3]);ylim([-1,3]); legend('待定位点（真实值）','待定位点位置解算','锚点（已知点）') fprintf('待定位点的真实坐标为：(%f,%f)\n',point1(1),point1(2)); fprintf('解得的位置坐标为：(%f,%f)\n',p_out(1),p_out(2));