本程序实现了基于 AOA（Angle of Arrival，角度到达法） 与 TDOA（Time Difference of Arrival，到达时间差） 的三维空间混合定位算法。它模拟一个空间中未知位置的目标点，通过四个空间锚点（基站）对其进行角度与时间差的测量，实现三种方式的定位估计并输出定位误差，绘制定位结果和锚点分布图
文章目录
📌 程序介绍：AOA与TDOA混合定位（适用于三维空间）
🧭 功能概述
本程序实现了基于 AOA（Angle of Arrival，角度到达法） 与 TDOA（Time Difference of Arrival，到达时间差） 的三维空间混合定位算法。它模拟一个空间中未知位置的目标点，通过四个空间锚点（基站）对其进行角度与时间差的测量，实现三种方式的定位估计：
- AOA定位
- TDOA定位
- AOA + TDOA混合定位
并输出定位误差，绘制定位结果和锚点分布图。
运行结果
定位示意图：
命令行输出：
代码获取
部分代码如下：
matlab
% AOA与TDOA混合定位例程，适用于三维环境、4个锚点的情况
%
% 2025-06-22/Ver1
%% 初始化
clc;clear;close all;
rng(0);
% 定义参数和待测点位置
std_var1 = 1e-10; %TDOA误差
% 固定基站位置
stations_position = 100*randn(4,3);
c = 3e8;
% 生成轨迹数组
position = 50*ones(1,3)+50*randn(1,3);
% TDOA 建模
delta = ones(4,1)*position - stations_position; %未知点与各基站之间的相对位置（矢量）
r_ideal = (sum(delta.^2,2)).^(1/2); %计算移动台到各个基站的实际距离（标量）
delta_t = r_ideal/c+std_var1*randn(size(r_ideal));
r = delta_t*c;完整代码下载链接：https://download.csdn.net/download/callmeup/91203911
🧱 主要模块说明
初始化与数据生成
- 使用随机数生成目标点和四个基站的三维坐标。
- 模拟TDOA测量数据，并加入高斯白噪声（
std_var1控制标准差）。 - 使用光速 c = 3 × 1 0 8 c = 3 \times 10^8 c=3×108 m/s 将时间差转换为距离差。
AOA定位
- 分别计算每个基站对目标的 方位角（azimuth） 与 俯仰角（elevation），模拟真实AOA测量。
- 构建线性方程组 H x = Y Hx = Y Hx=Y，利用 最小二乘法（pseudo-inverse） 估计目标位置。
TDOA定位
- 使用TDOA公式构建非线性最小二乘估计问题，采用两阶段迭代法（粗解 + 微调）来优化位置估计。
- 对测量差分进行加权处理，引入协方差矩阵 Q Q Q 与权重矩阵 W W W。
AOA + TDOA 混合定位
- 使用AOA估计结果作为初始值，将其与TDOA测量结合，迭代优化。
- 同样分为粗解与细解阶段，对位置进行连续修正。
误差评估与绘图
- 计算三种定位方式下的误差值（欧几里得距离）。
- 使用
plot3绘制三维散点图，显示基站、真实位置、三种估计值。 - 输出误差信息至命令行。
更多详解，见付费专栏：
或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者