本 MATLAB 程序实现了基于 Angle of Arrival (AOA) 与 Time Difference of Arrival (TDOA) 的二维定位方法，通过自适应融合与最小二乘优化，实现对未知目标的高精度估计。本例中固定使用了 3 个基站（锚点），算法框架支持扩展到更多基站。
直接运行后可得到二维、3个锚点的TDOA（到达时间差）与AOA（到达角度）的混合定位方法的定位结果展示与对比

程序简介

系统模型与参数

• 目标位置：未知点 $\mathbf{p}=[x,y{]}^{\top }$

• 锚点位置：第 $i$ 个基站坐标为 ${\mathbf{s}}_i=[x_i,y_i{]}^{\top }$，$i=1,2,3$

• 噪声模型：

  • AOA噪声：高斯白噪声 $\mathcal{N}(0,{\sigma }_{\theta }^2)$，默认 ${\sigma }_{\theta }=10^{-2}$
  • TDOA噪声：高斯白噪声 $\mathcal{N}(0,{\sigma }_t^2)$，默认 ${\sigma }_t=10^{-10}$

TDOA建模

TDOA测量基于不同基站之间接收到信号的时间差。对于目标到第 $i$ 个基站的理想距离为：

$$r_i=\Vert \mathbf{p}-{\mathbf{s}}_i{\Vert }_2$$

TDOA观测值定义为：

$$\Delta r_i=r_i-r_1+n_i,i=2,3$$

对应时间差：

$$\Delta t_i=\frac{\Delta r_i}{c},c\text{ 为光速}$$

最终形成的观测方程：

$$R_i=r_i-r_1,i=2,3$$

用于构建非线性约束方程，后续由最小二乘方法迭代解算。

AOA建模

AOA 使用信号到达方向（角度）进行定位。第 $i$ 个基站测得的理想角度为：

$${\theta }_i=\arctan 2(y-y_i,x-x_i)$$

加入角度噪声 $n_{\theta ,i}\sim \mathcal{N}(0,{\sigma }_{\theta }^2)$ 后，构造以下线性测量模型：

$$\sin ({\theta }_i)(x-x_i)-\cos ({\theta }_i)(y-y_i)=0$$

整理成最小二乘形式 $\mathbf{Hp}=\mathbf{Y}$：

$${\mathbf{H}}_i=\left[\begin{array}{cc}\sin ({\theta }_i)& -\cos ({\theta }_i)\end{array}\right],{\mathbf{Y}}_i=x_i\sin ({\theta }_i)-y_i\cos ({\theta }_i)$$

所有基站数据合并后估计目标位置：

$${\hat{\mathbf{p}}}_{AOA}=({\mathbf{H}}^{\top }\mathbf{H}{)}^{-1}{\mathbf{H}}^{\top }\mathbf{Y}$$

AOA/TDOA混合定位方法

详见代码中的注释。

运行结果

运行结果如下：

命令行输出的定位结果与误差：

MATLAB源代码

程序结构如下：

部分代码如下：

% AOA与TDOA混合定位例程，适用于二维环境、3个锚点的定位
% 作者：matlabfilter
% 2025-07-05/Ver1%% 初始化
clc;clear;close all;
rng(0);
% 定义参数和待测点位置
num_stations = 3; % 基站数量（锚点数量）
TDOA_noise = 1e-10; %TDOA误差
AOA_noise = 1e-2; % AOA 角度噪声
% 固定基站位置
stations_position = 100*randn(3,2);
c = 3e8;
% 待定位点
position = 0*ones(1,2)+5*randn(1,2);
% TDOA 建模
delta = ones(3,1)*position - stations_position; %未知点与各基站之间的相对位置（矢量）
r_ideal = (sum(delta.^2,2)).^(1/2); %计算移动台到各个基站的实际距离（标量）
delta_t = r_ideal/c+TDOA_noise*randn(size(r_ideal));
r = delta_t*c;
Ri = r(2:end,:);
R1 = ones(2,1)*r(1,:);
R = Ri-R1; %表示从[2,i]开始MS与基站i和基站1的距离差%% AOA定位
% 计算目标到各基站的距离
num_station = size(stations_position, 1);
% true_distances = vecnorm(stations_position - position, 2, 2);% 模拟接收到的AOA（此时是角度理想值，后面要加噪声）
azimuth_angles = atan2(position(2) - stations_position(:, 2), position(1) - stations_position(:, 1));% 假设测量的AOA角度和距离上加一些噪声
azimuth_angles = azimuth_angles + AOA_noise * randn(num_station, 1);
% 使用最小二乘法进行定位估计 直接求解（无需距离）
H = zeros(1*num_station,2);
Y = zeros(1*num_station,1);

完整代码的下载链接：https://download.csdn.net/download/callmeup/91274669

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