【MATLAB例程】三维空间中使用TOA（到达时间）给多目标定位的程序，目标、锚点的数量均可调整，MATLAB例程

本文给出的MATLAB例程，用于仿真三维空间中使用TOA（到达时间），多个锚点给多个目标定位。目标和锚点的数量均可调整。

程序详解

基于 TOA（到达时间） 测距的 三维定位 方法，特别适用于对多个目标进行定位。本方法的核心思想是利用至少四个已知位置的锚点，通过测量信号从未知目标到这些锚点的传播时间，来计算出目标的位置。

工作原理

程序的几个关键步骤：

1. 数据生成：首先，代码会随机生成一组已知坐标的锚点（baseP），以及多个待定位目标的真实坐标（targets_true）。这些锚点的数量可以自定义，但为了实现三维定位，数量必须大于等于4。
2. 距离计算与噪声模拟：对于每一个目标，程序计算它到所有锚点的真实距离，然后根据这个距离，通过光速（c）计算出真实的信号到达时间（TOA）。为了模拟实际测量中的不确定性，代码会向这个时间值中加入一个高斯白噪声（range_err*randn），从而得到带噪声的测量值。
3. 核心定位算法：程序调用 position_3dim 函数来处理这些带噪声的测量值。这个函数是整个定位过程的核心。它通过将非线性的 TOA 测距方程转换为线性方程组来求解目标坐标。
4. 结果展示与分析：最后，程序会绘制出锚点、目标的真实位置和估计位置，并用线条连接，直观地展示定位效果。同时，它还会计算并输出每个目标的定位误差，以及总体平均误差、最大/最小误差和标准差等统计信息，全面评估算法的性能。

使用说明

• 代码的 rng(0) 和 clear;clc;close all; 命令确保了每次运行都能得到可重复的结果，同时清空工作区和命令窗口，方便调试。
• 通过修改 num_targets 变量，可以轻松地改变待定位目标的数量。
• targets_true 和 baseP 部分的代码可以通过调整来改变目标和锚点的初始分布，从而测试在不同布局下的定位效果。
• range_err 变量可以用于模拟不同的测量精度，通过修改它可以观察误差对定位结果的影响。在实际应用的时候，这个变量的值应该与实际情况相匹配。

运行结果

修改锚点和目标点的方法如下：

这里以5锚点、5目标来做示例，定位结果如下：

各个目标的误差柱状图如下：

命令行窗口显示的误差统计特性：

MATLAB源代码

部分代码如下：

% TOA测距定位，三维任意（>3)个锚节点，对多个目标进行定位
% 作者：matlabfilter
% 2025-08-23/Ver1

clear;clc;close all;
rng(0);
%% 主程序参数设置
c = 3e8; % 信号传输速度，即光速
range_err = 1e-10; % 时钟与时间计算误差
n = 5; % 定义锚节点数量

% 目标数量设置（可调节）
num_targets = 5; % 修改此处可改变目标数量

% 生成多个待定位点坐标真值
targets_true = zeros(num_targets, 3);
for i = 1:num_targets
    targets_true(i, :) = [1+0.5*i, 1+0.3*i, 1+0.2*i]; % 可根据需要修改目标分布
end

% 生成锚节点坐标

完整代码：
https://download.csdn.net/download/callmeup/91799882

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者