TOA与AOA联合定位的高精度算法，三维、4个基站的情况，MATLAB例程，附代码下载链接

本文所述的代码实现了三维空间内目标的高精度定位，结合到达角（AOA） 和到达时间（TOA） 两种测量方法，通过4个基站的协同观测，利用最小二乘法解算目标位置。代码支持噪声模拟、误差分析及三维可视化

代码介绍

算法原理

1. 信号建模：

   • AOA测量：计算目标与各基站的方位角（azimuth_angles）和俯仰角（elevation_angles，三维空间才有此项），并添加高斯噪声（noise）。
   • TOA测量：基于光速（3e8 m/s）将传播时间转换为距离，加入时间噪声（TOA_noise）。

2. 混合定位解算：

   • 单位向量构建：根据AOA角度构造三维单位向量矩阵A，结合TOA距离向量b，构建线性方程组。
   • 最小二乘求解：通过伪逆（pinv）求解超定方程，分别得到纯AOA估计（estimated_positions_AOA1）和AOA-TOA混合估计（estimated_positions_AOA2）。

技术亮点

• 三维适应性：支持三维空间坐标解算，通过俯仰角与方位角联合建模，突破传统二维定位限制。
• 噪声鲁棒性：
  • AOA角度噪声默认1e-1 rad，TOA时间噪声1e-8 s，模拟实际测量误差。 在代码中可随意修改
  • 通过最小二乘法抑制噪声对定位精度的影响。
• 多方法对比：同时输出纯AOA与混合定位结果，便于性能对比分析。

代码结构

1. 初始化：

   stations_position = 100*randn(num_station,3);  % 随机生成4个基站的3D坐标
   position = 10*randn(1,3);                      % 生成目标点坐标
   

   • 基站与目标位置均随机生成，模拟动态场景。

2. 信号模拟与解算：

   • 计算真实距离true_distances，添加噪声后构建观测方程。
   • 通过单位向量矩阵H和伪逆求解目标坐标，支持两种解算方法对比。

3. 可视化与输出：

   • 绘制三维空间中的基站（红色*）、真实位置（蓝色○）及两种估计结果（▲/▼）。 详见后面的运行结果示意图
   • 输出三轴误差（X/Y/Z）和欧氏距离误差，量化定位精度。

推荐的应用场景

• 无人机导航：三维空间内的精准轨迹追踪。
• 室内定位：复杂建筑结构中的设备定位。
• 工业监测：移动机器人或AGV小车的实时位置监控。

扩展建议

1. 动态目标追踪：集成卡尔曼滤波（如EKF）优化运动轨迹。 参考：【MATLAB例程】AOA（到达角度）与TOA（到达时间）混合定位方法，在三维空间下的运动轨迹定位和滤波，滤波使用CKF，附完整源代码

运行结果

运行结果：

命令行输出截图：

部分代码截图：

MATLAB例程

完整的MATLAB代码如下，粘贴到MATLAB空脚本后即可直接运行：

% AOA与TOA混合的三维测距，基站数量：4
% 2025-01-23/Ver1

%% 初始化
clc;clear;close all;
rng(0);
% 生成目标点坐标
position = 10*randn(1,3);

% 固定基站位置
num_station =4; %基站数量
stations_position=100*randn(num_station,3);

for i1 = 1:size(position,1)
    %% 定位
    % 计算目标到各基站的距离
    num_station = size(stations_position, 1);
    true_distances = vecnorm(stations_position - position, 2, 2);
    
    % 模拟接收到的AOA角度信息
    azimuth_angles = ...;
    elevation_angles = ...;
CSDN原创文章

完整可运行的代码下载链接：https://download.csdn.net/download/callmeup/90647984

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