
本程序实现了基于 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
绘制三维散点图,显示基站、真实位置、三种估计值。 - 输出误差信息至命令行。
更多详解,见付费专栏:
或有导航、定位滤波相关的代码定制需求,请点击下方卡片联系作者