完整代码，附下载链接。包运行，讲解、定制可联系我
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我
文章目录
程序简介
本程序主要分为路径规划和定位两部分。路径规划采用快速扩展随机树（RRT）算法，在三维空间中规划一条能够绕开障碍物的运动路径；定位部分采用到达角（AOA）和到达时间差（TDOA）两种量测信息，并将两者结合起来估计目标的位置。
路径规划
RRT的基本思路比较直观：程序会不断在三维空间中随机选取新的位置，并从已有路径树中找到距离最近的节点，然后朝这个方向继续扩展。每次扩展时都会检查路径是否与障碍物发生碰撞，只有安全的节点才会加入路径树。找到终点后，再从终点向前回溯，就可以得到一条完整的三维避障路径。
AOA与TDOA定位
AOA主要提供目标相对于各个锚节点的方向信息，在三维情况下包括方位角和俯仰角；TDOA则通过比较目标到不同锚节点之间的距离差，为目标位置提供进一步约束。
程序将AOA和TDOA两类量测信息放在一起进行联合计算，通过不断调整估计位置，使预测量测与实际量测之间的误差尽可能小，最终得到目标的三维位置估计结果。
运行结果
路径规划结果图：
路径规划轨迹与定位估计轨迹对比图：
各坐标分量随路径点序号变化曲线：
定位误差曲线：
命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和RMSE等统计结果。
MATLAB源代码
部分代码如下：
matlab
%% 三维RRT路径规划与AOA-TDOA融合定位算法
% 作者: matlabfilter（V同号，可接代码定制、讲解）
% 2026-09-16/Ver2
%
% 程序流程：
% 1. 完成RRT三维采样路径规划，将规划路径点作为运动轨迹真值；
% 2. 在每个路径点处模拟AOA + TDOA量测；
% 3. 使用本脚本对应的定位模型估计路径点位置；
% 4. 使用普通figure窗口绘制规划轨迹、定位轨迹、坐标分量和误差曲线。
clear; clc; close all;
rng(0);
%% 参数设置
algorithmName = '三维RRT路径规划与AOA-TDOA融合定位算法';
measureName = 'AOA + TDOA';
sigmaAngle = 0.010; % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45; % TDOA距离差噪声，单位m
maxGnIter = 14; % Gauss-Newton最大迭代次数
%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planRrt3D();
%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaTdoaLocalization3D(rawPath, anchors, sigmaAngle, sigmaTdoaRange, maxGnIter, mapLimit);
%% 结果绘图与输出
plotPlanningResult(rawPath, anchors, mapLimit, obstacles, algorithmName);
plotLocalizationResult(rawPath, estPath, anchors, obstacles, mapLimit, algorithmName);
plotCoordinateResult(rawPath, estPath, algorithmName);
plotErrorResult(posErr, algorithmName);
printSummary(rawPath, posErr, iterUsed, planStats, algorithmName, measureName);
%% 函数完整代码和函数：https://download.csdn.net/download/callmeup/93455644