本 MATLAB 程序實現了基于 Angle of Arrival (AOA) 與 Time Difference of Arrival (TDOA) 的二維定位方法，通過自適應融合與最小二乘優化，實現對未知目標的高精度估計。本例中固定使用了 3 個基站（錨點），算法框架支持擴展到更多基站。
直接運行后可得到二維、3個錨點的TDOA（到達時間差）與AOA（到達角度）的混合定位方法的定位結果展示與對比

程序簡介

系統模型與參數

• 目標位置：未知點 $\mathbf{p}=[x,y{]}^{?}$

• 錨點位置：第 $i$ 個基站坐標為 ${\mathbf{s}}_i=[x_i,y_i{]}^{?}$，$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}～\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}}^{?}\mathbf{H}{)}^{?1}{\mathbf{H}}^{?}\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

如需幫助，或有導航、定位濾波相關的代碼定制需求，請點擊下方卡片聯系作者