【MATLAB例程】任意基站数量的AOA+测距辅助定位，适用于三维环境。基站数量改变后，基站坐标矩阵、AOA角度量测、距离量测、线性方程矩阵、绘图颜色和误差统计都会自动适配。

附完整代码的下载链接，禁止翻卖

程序简介

概览

本程序是4基站AOA定位程序的n基站扩展版本。程序不再把基站数量固定为4，而是通过 num_station 控制基站数量。基站数量改变后，基站坐标矩阵、AOA角度量测、距离量测、线性方程矩阵、绘图颜色和误差统计都会自动适配。

AOA角度量测

对每个基站，程序根据目标点与基站之间的相对位置计算方位角。

代码中对应实现为：

azimuth_angles = atan2(position(i1, 2) - stations_position(:, 2), ...
                       position(i1, 1) - stations_position(:, 1));

elevation_angles = atan2(position(i1, 3) - stations_position(:, 3), ...
    sqrt((position(i1, 1) - stations_position(:, 1)).^2 + ...
         (position(i1, 2) - stations_position(:, 2)).^2));

随后对角度和距离加入随机噪声：

azimuth_angles = azimuth_angles + AOA_noise * randn(num_station, 1);
elevation_angles = elevation_angles + AOA_noise * randn(num_station, 1);
distances = true_distances + distance_noise * randn(num_station, 1);

• 方法1：不带距离辅助的AOA定位
• 方法2：带距离辅助的AOA定位

运行结果

程序运行后会依次生成6个图窗。默认 num_station = 8 时，图中会显示8个基站；如果用户修改基站数量，图中的基站数量也会同步变化。

AOA三维定位示意图（8个基站）为例。该图展示所有基站、真实目标点、不带距离辅助AOA估计点和带距离辅助AOA估计点。若基站数量不超过12个，程序还会在三维图中标注基站编号。

XY平面投影展示定位结果在水平面上的投影。

XZ平面投影/ YZ平面投影也有，不再展示。运行程序后可查看。

三轴定位平均误差对比（越接近0表示误差越低）：

定位平均距离误差对比（越小表示误差越低）。展示两种方法的平均三维距离误差，并在柱子上标注数值：

命令行输出：

修改为20个基站的情况，则为：

其他数量也可自行调整

MATLAB源代码

部分代码：

% AOA三维定位，测距仅作为辅助精度增强，n个基站数量可调
% 作者微信：matlabfilter（可联系我获取完整例程、个性化定制和讲解）
% 带距离辅助的AOA定位为新提出的精度增强方法
% 2024-11-21/Ver1
% 2026-06-08/Ver2：由4基站版本扩展为n基站版本，添加误差输出和多视角绘图
%% 初始化
clc; clear; close all;
rng(0);

%% 用户可调参数
num_station = 8;      % 基站数量，用户可自行修改，建议设置为4或更多
num_target = 1;       % 目标点数量
target_scale = 10;    % 目标点随机分布尺度
station_scale = 20;   % 基站随机分布尺度
AOA_noise = 1e-2;     % AOA角度噪声
distance_noise = 0.1; % 辅助AOA的距离噪声

if num_station < 2 || mod(num_station, 1) ~= 0
    error('num_station必须为不小于2的整数，三维定位建议设置为4或更多。');
end

% 随机生成目标点坐标和基站位置
position = target_scale * randn(num_target, 3);
stations_position = station_scale * randn(num_station, 3);

% 结果变量预分配
estimated_positions_AOA1 = zeros(size(position, 1), 3);
estimated_positions_AOA2 = zeros(size(position, 1), 3);

for i1 = 1:size(position, 1)

    %% AOA定位
    num_station = size(stations_position, 1);

    % 计算目标到各基站的真实距离
    true_distances = vecnorm(stations_position - position(i1, :), 2, 2);

完整代码也可根据下方链接获取：
https://download.csdn.net/download/callmeup/92958286

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我