程序介绍

TOA/TDOA使用三点法测距，在空间中，有4个锚节点就可以定位，但如果有多个节点，定位效果会更好。
锚点不同时，修改程序中的向量和矩阵维度比较繁琐，这里给出一个在锚点数量>3时，可以自动调节计算矩阵的方法，供参考。

程序截图和运行结果

程序截图

如下图，可以自己定义待测点的坐标（$point1$）和锚节点的个数（$n$）。

运行结果

命令行可以看到待求坐标的真实值、计算得到的输出值：

绘制一个三维图像，显示待测点真实值坐标、锚点坐标、计算出来的待求点坐标：

源代码

% TOA/TDOA测距定位，三维任意（>3)个锚节点，对一个未知点进行定位
% author:Evand
% 2024-8-3/Ver1
clear;clc;close all;
rng(0);
%% 主程序
point1 = [0.5,0.5,0.5]; %待求点坐标真值
n = 9; %定义锚节点数量
range_err = 0.1; %测距误差
baseP = [sin(1:n)+0.01*[1:n];cos(1.3*(1:n))+0.01*[1:n];cos(1.5*(1:n))+0.01*[1:n]]';
R_calcu = sqrt(diag((point1-baseP)*(point1'-baseP')))+range_err*randn; %含噪声的距离
% 完整代码，下载链接见：https://gf.bilibili.com/item/detail/1105965012
fprintf('完整代码，下载链接见：https://gf.bilibili.com/item/detail/1105965012');

%% 绘图
figure;

代码修改建议

• 如需修改锚节点的坐标，修改$baseP$这个矩阵，每行存放xyz坐标，有多少个节点就写多少行，如下是9个节点时的情况：
  
• 如需修改测距误差，更改$rang{e}_{e}rr$，其值为测距误差的标准差