完整的MATLAB代码（m文件）下载链接，可直接运行原创代码，欢迎交流讨论，请勿翻卖。
文章目录
程序介绍
概览
本为 二维A*路径规划与AOA-TDOA融合定位算法 所需的路径规划和定位算法防止验证。路径规划部分采用A*栅格搜索，定位部分采用 AOA + TDOA 量测模型。融合部分只保留AOA与TDOA两类残差，并将二者堆叠为同一个最小二乘问题。
路径规划方面：采用A*算法，在二维栅格地图上使用八邻域扩展和欧氏距离启发函数。A*搜索得到的栅格路径会转换为连续坐标点，并作为后续定位仿真的真实轨迹。
定位方面：AOA量测提供目标相对各锚节点的方向角，TDOA量测提供目标到不同锚节点的距离差信息。三维AOA会同时使用方位角和俯仰角。程序将AOA角度残差和TDOA距离差残差归一化后堆叠，形成紧耦合最小二乘问题。
运行结果
真实截图
程序运行后会通过普通 figure; 命令生成图像窗口，主要包括：
路径规划结果图：
路径规划轨迹与定位估计轨迹对比图：
各坐标分量随路径点序号变化曲线：
定位误差曲线：
命令行会输出路径长度、路径点数、规划迭代次数、平均定位误差、最大定位误差、最小定位误差和RMSE等统计结果。截图如下：
MATLAB源代码
部分代码如下：
matlab
%% 二维A*路径规划与AOA-TDOA融合定位算法
% 作者: matlabfilter（V同号，可接代码定制、讲解）
% 2026-09-11/Ver2
clear; clc; close all;
rng(0);
%% 参数设置
algorithmName = '二维A*路径规划与AOA-TDOA融合定位算法';
measureName = 'AOA + TDOA';
sigmaAngle = 0.012; % AOA角度噪声，单位rad
sigmaTdoaRange = 0.45; % TDOA距离差噪声，单位m
maxGnIter = 14; % Gauss-Newton最大迭代次数
%% 路径规划
[rawPath, anchors, mapLimit, obstacles, planStats] = planAstar2D();
%% 沿规划轨迹进行定位仿真
[estPath, posErr, iterUsed] = runAoaTdoaLocalization2D(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/93426763
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我