无迹卡尔曼滤波纯方位目标跟踪算法实现

资 源 简 介

本项目主要用于解决非线性系统中的目标跟踪问题，具体实现了一个针对纯方位（Bearings-Only）系统的无迹卡尔曼滤波（UKF）算法。在纯方位跟踪场景中，观测器（如被动声纳或无源雷达）仅能获取目标的角度信息而无法直接测量距离，导致观测模型呈现高度非线性。相比于传统的扩展卡尔曼滤波（EKF），本程序采用无迹变换（Unscented Transform）技术，通过确定性采样Sigma点来通过非线性函数传播，从而逼近状态的后验概率分布。这种方法无需对非线性函数进行线性化（计算雅可比矩阵），因此在处理强非线性问题时能提供更高的估计精度和更好的数值稳定性。项目功能细节包括构建二维平面内的目标运动模型（通常为匀速直线运动CV模型）和纯方位观测模型；生成即包含过程噪声也包含量测噪声的仿真数据以模拟真实环境；完整实现UKF算法的预测与更新循环，包括Sigma点的生成、权重的分配、状态预测、量测预测及卡尔曼增益的计算；最后，程序通过MATLAB绘图工具直观展示目标的真实轨迹与UKF估算轨迹的对比，并计算位置估算的均方根误差（RMSE）以及误差协方差边界，用于量化评估算法在纯方位跟踪任务中的收敛性和准确性。该代码结构清晰，适合用于目标跟踪、导航制导及多传感器数据融合领域的算法研究与验证。

详 情 说 明

基于MATLAB的无迹卡尔曼滤波（UKF）纯方位目标跟踪项目

项目简介

本项目实现了一个基于MATLAB的无迹卡尔曼滤波（UKF）算法，专门用于解决纯方位目标跟踪（Bearings-Only Tracking, BOT）问题。在纯方位跟踪场景中，观测站（如被动声纳或无源雷达）仅能获取目标的方位角信息，无法直接测量距离，导致观测模型呈现高度非线性。

本程序通过构建目标匀速运动模型和纯方位观测模型，利用无迹变换（Unscented Transform）技术处理非线性观测方程，避免了扩展卡尔曼滤波（EKF）中复杂的雅可比矩阵计算，实现了对目标位置和速度的有效估计。

功能特性

• 非线性滤波算法：完整实现了无迹卡尔曼滤波（UKF）的标准流程，包括Sigma点采样、非线性传播、状态更新及协方差更新。
• 纯方位观测模型：处理高度非线性的 atan2 观测函数，并专门针对角度数据的周期性（[-pi, pi]）进行了处理，防止角度模糊导致的滤波发散。
• 可观测性增强设计：仿真中的观测站设计了特定的机动轨迹（中途90度转弯），以满足纯方位跟踪系统的可观测性要求，确保滤波器能够收敛。
• 鲁棒性设计：在Cholesky分解步骤增加了数值稳定性处理（微小量扰动及异常捕获），防止因协方差矩阵非正定导致的程序崩溃。
• 可视化评估：提供完整的轨迹对比图、坐标轴误差分析图（含 $3sigma$ 边界）及位置均方根误差（RMSE）统计。

系统要求

• MATLAB R2016a 或更高版本
• 不需要额外的工具箱（代码仅使用基础矩阵运算和绘图函数，mvnrnd 属于统计工具箱但在新版MATLAB基础包中通常可用，若无可用 randn 替代）

算法实现细节与逻辑分析

本项目的核心逻辑包含在主程序中，按以下步骤严格执行：

1. 仿真场景构建

• 目标模型：采用匀速直线运动（CV）模型。状态向量为四维 $x = [x, v_x, y, v_y]^T$。
• 观测站策略：观测站初始沿直线匀速运动，在仿真第100步时主动改变速度分量（机动），改变运动方向。这是纯方位跟踪成功的关键，因为静止或匀速直线运动的单观测站通常无法完全解算目标距离（存在伪解）。
• 数据生成：基于真实状态方程生成目标轨迹，并根据相对位置计算真实方位角，叠加因为标准差为 $1^circ$ 的高斯白噪声生成观测数据。

2. UKF 滤波器初始化

• 初始误差：滤波器初始状态人为引入了较大的位置和速度偏差，以验证算法的收敛能力。
• 参数设置：

* 设置比例参数 $alpha=0.01$，状态分布先验参数 $beta=2$（针对高斯分布最优），缩放参数 $kappa=0$。 * 计算均值权重 $W_m$ 和协方差权重 $W_c$。

3. 滤波循环（预测与更新）

程序通过 N 步循环处理数据，每一步包含：

#### 预测阶段（Time Update）

1. Sigma点生成：基于上一时刻的后验协方差矩阵 $P$ 进行 Cholesky 分解。代码中加入了 try-catch 结构，若分解失败则降级使用对角阵分解，增强了数值计算的稳定性。
2. 状态传播：将生成的Sigma点代入线性状态转移矩阵 $F$（CV模型），得到预测后的Sigma点集。
3. 统计矩计算：加权求和计算先验状态估计 $hat{x}_{k|k-1}$ 和先验协方差 $P_{k|k-1}$，并在此处叠加了过程噪声 $Q$。

#### 更新阶段（Measurement Update）

1. 重采样：基于预测协方差 $P_{k|k-1}$ 再次生成新的Sigma点（标准UKF流程）。
2. 观测映射：将新的Sigma点集代入非线性观测方程 $h(x) = text{atan2}(dy, dx)$，映射到量测空间。
3. 角度均值处理：在计算预测量测值的均值时，代码采用了残差均值法，并严格执行了角度归一化处理（确保角度差值落在 $[-pi, pi]$ 区间），解决了跨越 $pmpi$ 处的计算错误问题。
4. 增益计算与更新：

* 计算自协方差 $S_k$ 和互协方差 $C_k$，同样包含角度差分的归一化处理。 * 计算卡尔曼增益 $K$。 * 计算新息（Innovation），即实际观测与预测观测的差值，并进行角度归一化。 * 更新后验状态 $hat{x}_k$ 和后验协方差 $P_k$。

4. 性能评估与可视化

程序最后会输出统计结果并绘制两张图表：

• 轨迹跟踪图：在一个二维平面图中同时展示真实目标轨迹（蓝色实线）、观测站轨迹（黑色虚线）和UKF估计轨迹（红色点划线），并标记了起点和终点。
• 误差分析图：

* 包含X轴和Y轴的位置估计误差曲线。 * 并在误差图中绘制了绿色的 $pm 3sigma$（3倍标准差）边界线，用于判断滤波器估计的一致性（误差是否在理论置信区间内）。 * 单独绘制随时间变化的位置距离误差曲线，标题中直接显示了计算出的全程RMSE值。

使用方法

1. 确保MATLAB环境已安装。
2. 直接运行主脚本文件。
3. 程序将在命令窗口输出全程RMSE误差值和最终位置误差。
4. 程序将自动弹出两个图形窗口展示跟踪效果和误差分析。

可以通过修改代码顶部的仿真参数（如采样时间 T、噪声标准差 sigma_angle 或 初始误差 initial_error）来测试不同条件下的算法性能。