【MATLAB例程】二维A*路径规划与TOA-AOA-TDOA融合定位算法。附下载链接，包运行成功，有中文注释

完整代码，包运行成功。后面有下载链接

程序概览

本程序实现二维A*避障路径规划与TOA、AOA、TDOA融合定位，并对定位轨迹及误差进行分析。地图范围、障碍物、起终点、锚节点位置及量测噪声等参数均可自行修改，便于构建不同仿真场景。

路径规划模型

本程序采用A*算法，在二维栅格地图上使用八邻域扩展和欧氏距离启发函数。

A*搜索得到的栅格路径会转换为连续坐标点，并作为后续定位仿真的真实轨迹。

量测模型

TOA量测提供绝对距离，AOA量测提供方向角，TDOA量测提供相对距离差。
三维AOA会同时使用方位角和俯仰角。程序将三类残差按各自噪声标准差归一化后统一迭代求解。

运行结果

路径规划结果图：

路径规划轨迹与定位估计轨迹对比图：

各坐标分量随路径点序号变化曲线：

定位误差曲线：

命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和RMSE等统计结果。

MATLAB源代码

部分代码如下：

%% 二维A*路径规划与TOA-AOA-TDOA融合定位算法
% 作者: matlabfilter（V同号，可接代码定制、讲解）
% 2026-09-11/Ver2


clear; clc; close all;
rng(0);

%% 参数设置
algorithmName = '二维A*路径规划与TOA-AOA-TDOA融合定位算法';
measureName = 'TOA + AOA + TDOA';
sigmaToaRange = 0.55;      % TOA等效测距噪声，单位m
sigmaAngle = 0.012;        % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45;     % TDOA距离差噪声，单位m
maxGnIter = 14;            % Gauss-Newton最大迭代次数

%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planAstar2D();

%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runToaAoaTdoaLocalization2D(rawPath, anchors, sigmaToaRange, 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.youkuaiyun.com/download/callmeup/93431924

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我