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

在这里插入图片描述

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

文章目录

程序介绍
- 代码功能概述
- 核心模块与技术亮点
- 关键参数与配置
- 运行结果与性能
运行结果
MATLAB代码
结论

程序介绍

代码功能概述

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

核心模块与技术亮点

动态场景建模
- 目标轨迹：生成三维匀速运动轨迹（X/Y轴线性变化，Z轴固定），模拟真实运动特性。
- 基站网络：随机生成非均匀分布的基站位置（stations_position），支持动态调整基站数量（num_station）。
混合定位算法
- 信号模拟：
  - AOA测量：计算方位角（azimuth_angles）和俯仰角（elevation_angles），叠加高斯噪声（AOA_noise）。
  - TOA测量：基于光速计算距离（distances），添加时间噪声（TOA_noise）。
- 解算方法：
  - 构建超定方程组，利用单位向量矩阵（H）和伪逆（pinv）求解目标坐标
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

运行结果与性能

轨迹精度对比
- 滤波前观测值：因累积误差逐渐偏离真实轨迹，终点距离误差约0.8米。
- EKF滤波后：轨迹紧贴真实路径，终点误差降至0.2米以下。
- 未滤波惯导模拟：误差随时间线性增长，验证EKF对噪声抑制的有效性。
误差统计
- X/Y轴：EKF滤波后RMSE ≤0.15米，较纯观测值提升60%。
- Z轴：受限于AOA/TOA垂直方向观测信息不足，误差略高（约0.3米）。
可视化输出示例
- 三维轨迹图：红点（基站）、蓝虚线（真实轨迹）、黄点（观测值）、蓝点（EKF结果）。
- RMSE对比图：EKF曲线（蓝色）显著低于未滤波结果（橙色/黄色）。

运行结果

定位导航示意图：
在这里插入图片描述
误差曲线（滤波前后对比）：

命令行截图：

MATLAB代码

完整的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); %定义基站的坐标，这里是随机坐标