AOA（到达角度）与TOA（到达时间）两个技术的混合定位，MATLAB例程(附下载链接)，自适应基站数量，三维空间下的运动轨迹，滤波使用UKF（无迹卡尔曼滤波）

最新推荐文章于 2026-01-06 18:13:03 发布

原创最新推荐文章于 2026-01-06 18:13:03 发布 · 1.1k 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #开发语言

定位与导航专栏收录该内容

153 篇文章

订阅专栏

在这里插入图片描述

本文介绍一个MATLAB代码，实现了三维动态目标非线性定位与滤波系统，通过融合 到达角（AOA） 和 到达时间（TOA） 的混合定位方法，结合 无迹卡尔曼滤波（UKF） 处理非线性观测模型，优化轨迹精度。代码支持自适应基站数量配置，适用于复杂非线性场景的定位研究。

程序介绍

概述

该程序实现了基于到达角（AOA）和到达时间（TOA）混合定位的方法。它可以自适应调整基站数量，并在三维空间中进行目标定位。程序使用无迹卡尔曼滤波（UKF）对轨迹进行优化和滤波。

代码讲解

1. 初始化

clc; clear; close all;
rng(0);

清空命令窗口、工作区和关闭所有图形窗口，设置随机种子以确保结果可重现。

2. 生成目标点与运动轨迹

position = [-1,1,1];
positions = repmat(position,21,1) + [0:0.2:4; 0:-0.2:-4; zeros(1,21)]';

设置目标的初始位置，并生成其运动轨迹，目标在三维空间中沿特定路径移动。

3. 基站位置设置

num_station = 10;
stations_position = 2 * randn(num_station, 3);

随机生成固定基站的位置，共10个基站。

4. 定位计算

for i1 = 1:size(positions, 1)
    position = positions(i1, :);
    ...
end

对每一个时间步进行定位计算：
- 计算目标到基站的距离：使用vecnorm函数计算目标到所有基站的真实距离。
- 模拟AOA角度：通过计算目标与基站之间的相对位置，得到方位角和俯仰角，并添加噪声。
- 模拟TOA距离：通过真实距离和光速计算时间，添加TOA噪声。

5. 最小二乘法定位估计

A = zeros(num_station, 3);
b = zeros(num_station, 1);

构建线性方程来估计目标位置：
- 计算当前锚点到目标的单位向量。
- 使用最小二乘法求解目标位置的估计值。

6. UKF部分

% 滤波模型初始化
Q = 0.01 * diag([1, 1, 1]); 
...
X_ukf = zeros(3, length(t));

初始化UKF的模型参数，包括过程噪声和观测噪声的协方差矩阵，准备进行状态估计。

7. UKF迭代

for k = 2:length(t)
    ...
end

在主循环中进行状态预测和更新：
- 预测步骤：根据状态转移矩阵预测下一状态。
- 观测更新：使用AOA和TOA观测数据更新状态估计，校正预测的状态。

8. 绘图

figure;
hold on
plot3(stations_position(:,1), stations_position(:,2), stations_position(:,3), 'r*', 'DisplayName', '锚点');
...
title('定位示意图');

绘制三维图像，展示基站位置、真实轨迹、观测值和UKF估计值。

9. 误差分析与输出

fprintf('终点真实值坐标 ：%s\n', num2str(position));
...

计算并输出终点的真实坐标、估计值、误差以及每时刻的均方根误差（RMSE），分析定位精度。

运行结果

定位示意图：
在这里插入图片描述
三周RMSE曲线对比（UKF估计值是最低的）：

命令行输出的结果截图：
在这里插入图片描述

MATLAB代码

部分代码如下：

% AOA与TOA混合定位例程，自适应基站数量，三维，轨迹滤波使用UKF
% 作者：matlabfilter
% 2025-03-26/Ver1

%% 初始化
clc;clear;close all;
rng(0);
% 生成目标点坐标
position = [-1,1,1];
% 生成目标的运动
positions = repmat(position,21,1)+[0:0.2:4;0:-0.2:-4;zeros(1,21)]';

% 固定基站位置
num_station =10; %基站数量
stations_position=2*randn(num_station,3); %定义基站的坐标，这里是随机坐标

for i1 = 1:size(positions,1)
    position = positions(i1,:);
    %% 定位
    % 计算目标到各基站的距离
    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));
    elevation_angles = atan2(position(3) - stations_position(:, 3), ...
        sqrt((position(1) - stations_position(:, 1)).^2 + (position(2) - stations_position(:, 2)).^2));
    
    % 假设AOA角度和TOA测量的时间上加一些噪声
    AOA_noise = 1e-1; % AOA 角度噪声
    TOA_noise = 1e-9; %TOA时间噪声
    azimuth_angles = azimuth_angles + AOA_noise * randn(num_station, 1);
    elevation_angles = elevation_angles + AOA_noise * randn(num_station, 1);