三维TDOA/AOA融合定位与EKF轨迹跟踪仿真系统

资 源 简 介

本项目开发了一套完整的三维空间目标定位与跟踪仿真程序，核心算法采用扩展卡尔曼滤波（EKF）技术来融合到达时间差（TDOA）和到达角度（AOA）两种异构观测数据。项目功能涵盖了从场景建模、数据模拟到算法处理及结果分析的全过程。首先，程序模拟了包含多个固定基站的监测环境和执行特定机动动作的空中目标，并根据相对几何关系生成叠加了高斯白噪声的TDOA时间差测量值（转化为距离差）以及AOA方位角和俯仰角测量值。其次，针对TDOA和AOA观测方程的高度非线性特性，利用一阶泰勒展开计算雅可比矩阵进行线性化处理，构建EKF的状态预测与更新回路，实时迭代计算目标的三维位置坐标及速度矢量。最后，系统对滤波结果进行深度分析，输出目标真实轨迹与估计轨迹的对比，计算均方根误差（RMSE）以量化评估定位精度，并分析在不同噪声水平和基站布局下的算法鲁棒性。该仿真不仅验证了多源信息融合相比单源定位的精度优势，也为实际无线电定位系统的设计提供了参数参考。

详 情 说 明

基于扩展卡尔曼滤波的三维TDOA/AOA融合定位仿真系统

项目简介

本项目是一个基于MATLAB开发的完整三维空间目标定位与跟踪仿真系统。核心功能是利用扩展卡尔曼滤波（EKF）算法，融合到达时间差（TDOA）和到达角度（AOA）两种异构观测数据，实现对可以在三维空间中运动的目标进行实时位置和速度估计。

该程序模拟了从传感器数据生成、噪声添加、滤波算法处理到结果误差分析的完整闭环流程，重点演示了如何通过多源信息融合来解决非线性定位问题。

主要功能特性

• 多源异构数据融合：结合了TDOA（距离差信息）和AOA（方位角与俯仰角信息）进行联合观测，相比单一观测源具有更高的定位稳定性和精度。
• 三维空间跟踪：状态向量包含三维位置 $(x, y, z)$ 和三维速度 $(v_x, v_y, v_z)$，基于匀速（CV）运动模型进行状态推演。
• 非线性滤波算法：实现了扩展卡尔曼滤波（EKF），通过解析法计算雅可比矩阵（Jacobian Matrix），处理观测方程的高度非线性问题。
• 高保真仿真环境：

* 支持自定义基站布局（默认4基站构型）。 * 模拟真实的信号传播几何关系。 * 引入高斯白噪声模拟真实传感器误差（距离差噪声及角度噪声）。

• 稳健的数值处理：

* 包含角度周期性跳变（Wrap-around）的处理逻辑。 * 包含协方差矩阵对称化处理以保证数值稳定性。

• 可视化评估：自动生成三维轨迹对比图、各轴状态误差曲线以及RMSE收敛曲线。

系统要求

• MATLAB R2016a 或更高版本。
• Statistics and Machine Learning Toolbox（用于 mvnrnd 函数生成多元高斯噪声）。

使用方法

1. 确保MATLAB工作路径包含本项目文件。
2. 直接运行 main.m 脚本。
3. 程序将自动执行仿真，并在控制台输出最终的平均均方根误差（RMSE）。
4. 运行结束后将弹出三个图形窗口，展示仿真结果和算法性能。

代码实现细节分析

本项目的核心逻辑完全封装在 main.m 文件中，以下是主要模块的实现原理详细说明：

1. 系统初始化与场景建模

• 仿真参数：设定采样时间 $T=1s$，总步数200步，光速 $c=3e8$ m/s。
• 基站配置：定义了4个基站的三维坐标。其中 BS1 被设定为TDOA测量的主参考基站。
• 目标模型：

* 采用 匀速运动模型 (CV Model) 构建状态转移矩阵 $F$。 * 构建过程噪声协方差矩阵 $Q$，模拟目标运动的不确定性（加速度扰动）。 * 生成真实的参考轨迹 $X_{true}$。

2. 量测数据模拟

• 观测向量构造：系统构建了一个11维的观测向量 $Z$，包含：

* 3维 TDOA数据：基于 $BS_i$ 与 $BS_1$ 到目标的距离差 ($i=2,3,4$)。 * 8维 AOA数据：所有4个基站均提供目标的 方位角 (Azimuth) 和 俯仰角 (Elevation)。

• 噪声添加：

* 根据设定的标准差（距离差15米，角度0.5度），构建观测噪声协方差矩阵 $R$。 * 利用 mvnrnd 函数在真实几何测量值上叠加高斯白噪声。

3. 扩展卡尔曼滤波 (EKF) 核心实现

滤波循环是代码的核心部分，主要步骤如下：

• 初始化：初始状态估计 $X_{est}$ 被人为引入了较大的偏差（位置误差约100m），以验证算法的收敛能力；初始协方差矩阵 $P$ 设为较大值。
• 预测步骤 (Time Update)：

* 根据状态转移矩阵 $F$ 预测下一时刻的先验状态。 * 计算先验误差协方差 $P_{pred}$。

• 新息计算：

* 调用 measurement_function 计算先验状态对应的观测预测值。 * 计算真实观测值与预测观测值的残差（Innovation）。 * 关键处理：包含一个专门的循环，对AOA观测的角度残差进行区间归一化，确保其落在 $[-pi, pi]$ 范围内，防止因角度周期性导致的滤波发散。

• 线性化 (Linearization)：

* 调用 calculate_jacobian 函数，计算观测方程关于状态向量的偏导数矩阵 $H$。 * 该函数分别导出了 TDOA（双曲线方程）、方位角（反正切）、俯仰角对此刻位置 $(x,y,z)$ 的解析解导数。由于观测仅与位置相关，其对速度项的偏导数为0。

• 更新步骤 (Measurement Update)：

* 计算卡尔曼增益 $K$。 * 更新后验状态估计 $X_{est}$ 和后验协方差 $P$。 * 强制 $P$ 矩阵对称化，增强数值稳定性。

4. 辅助函数说明

• measurement_function：

* 输入目标位置和基站坐标。 * 输出无噪声的理论观测值（距离差向量、方位角向量、俯仰角向量）。

• calculate_jacobian：

* 这是EKF精度的关键。它解析地计算了非线性观测函数 $h(x)$ 的雅可比矩阵。 * 对于TDOA部分，利用了距离梯度的差分。 * 对于AOA部分，分别推导了 $text{atan2}(dy, dx)$ 和 $text{atan2}(dz, d_{xy})$ 的偏导数。

5. 结果可视化与评估

程序运行结束后会生成以下图表用于分析：

1. 三维轨迹跟踪图：

* 在三维坐标系中同时绘制基站位置、目标真实轨迹和EKF估计轨迹。 * 直观展示滤波器如何迅速修正初始误差并跟踪目标。

1. 状态跟踪误差图：

* 包含6个子图，分别展示 X, Y, Z 三轴上的位置误差和速度误差随时间的变化。

1. 定位精度与收敛性图：

* 左图：位置均方根误差（RMSE）随时间的变化曲线，并在标题显示全轨迹平均RMSE。 * 右图：协方差矩阵的迹（Trace(P)）的对数曲线，用于评估滤波器的收敛速度和置信度。