AOA与TOA混合定位，MATLAB例程，三维空间下的运动轨迹，滤波使用EKF，附下载链接

本文介绍一个MATLAB代码，实现基于 到达角（AOA） 和 到达时间（TOA） 的混合定位算法，结合 扩展卡尔曼滤波（EKF） 对三维运动目标的轨迹进行滤波优化。代码通过模拟动态目标与基站网络，展示了从信号测量、定位解算到轨迹滤波的全流程，适用于城市峡谷、室内等复杂环境下的定位研究。

程序介绍

代码功能概述

本文所述代码实现了一个三维动态目标定位与滤波系统，通过融合**到达角（AOA）和到达时间（TOA）的混合定位方法，结合扩展卡尔曼滤波（EKF）**优化轨迹精度。代码支持自适应基站数量配置，适用于城市峡谷、室内等复杂场景的定位研究。

核心模块与技术亮点

1. 动态场景建模

   • 目标轨迹：生成三维匀速运动轨迹（X/Y轴线性变化，Z轴固定），模拟真实运动特性。
   • 基站网络：随机生成非均匀分布的基站位置（stations_position），支持动态调整基站数量（num_station）。

2. 混合定位算法

   • 信号模拟：
     • AOA测量：计算方位角（azimuth_angles）和俯仰角（elevation_angles），叠加高斯噪声（AOA_noise）。
     • TOA测量：基于光速计算距离（distances），添加时间噪声（TOA_noise）。
   • 解算方法：
     • 构建超定方程组，利用单位向量矩阵（H）和伪逆（pinv）求解目标坐标

3. EKF轨迹滤波

   • 状态模型：假设目标匀速运动，状态向量为位置（[x, y, z]），速度通过固定增量（[0.2, -0.2, 0]）建模。
   • 滤波流程：
     • 预测：根据运动模型预测下一状态，更新协方差矩阵（PP = F * P * F' + Q）。
     • 更新：融合混合定位结果，计算卡尔曼增益（Kk），修正状态估计值

关键参数与配置

参数/变量	描述	典型值/范围
num_station	基站数量	10（可调）
AOA_noise	AOA角度噪声标准差	0.1 rad
TOA_noise	TOA时间噪声标准差	1e-9 s
Q	过程噪声协方差矩阵	diag([1,1,1])*0.01
R	观测噪声协方差矩阵	diag([1,1,1])*0.02

运行结果与性能

1. 轨迹精度对比

   • 滤波前观测值：因累积误差逐渐偏离真实轨迹，终点距离误差约0.8米。
   • EKF滤波后：轨迹紧贴真实路径，终点误差降至0.2米以下。
   • 未滤波惯导模拟：误差随时间线性增长，验证EKF对噪声抑制的有效性。

2. 误差统计

   • X/Y轴：EKF滤波后RMSE ≤0.15米，较纯观测值提升60%。
   • Z轴：受限于AOA/TOA垂直方向观测信息不足，误差略高（约0.3米）。

3. 可视化输出示例

   • 三维轨迹图：红点（基站）、蓝虚线（真实轨迹）、黄点（观测值）、蓝点（EKF结果）。
   • RMSE对比图：EKF曲线（蓝色）显著低于未滤波结果（橙色/黄色）。

运行结果

定位导航示意图：

误差曲线（滤波前后对比）：


命令行截图：

MATLAB代码

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

% AOA与TOA混合定位例程，自适应基站数量，三维，轨迹滤波使用EKF
% 作者：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); %定义基站的坐标，这里是随机坐标

完整代码下载链接：https://download.csdn.net/download/callmeup/90540441

结论

本代码通过AOA/TOA混合定位与EKF滤波的结合，有效提升了复杂环境下的定位精度，为多传感器融合算法研究提供了可扩展的仿真框架。通过调整基站数量、噪声参数或运动模型，可进一步适配具体应用需求。

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