基于无迹卡尔曼滤波的目标跟踪算法实现

资 源 简 介

本项目通过MATLAB代码实现了标准的无迹卡尔曼滤波（UKF）算法，并将其核心功能应用于强非线性环境下的目标跟踪任务。在实际的雷达或传感器监测场景中，目标的运动状态（如转弯半径、变加速运动）以及传感器的观测过程（从距离角度到笛卡尔坐标系的转换）通常表现出显著的非线性特性。传统的扩展卡尔曼滤波（EKF）依靠泰勒级数展开来近似非线性函数，这在高度非线性情况下会导致较大的线性化误差甚至滤波发散。本项目采用无迹变换（Unscented Transformation）技术，通过确定性地采样一组Sigma点来捕捉状态

详 情 说 明

基于无迹卡尔曼滤波(UKF)的目标跟踪算法实现

项目介绍

本项目是一个基于MATLAB的运动目标跟踪仿真系统。其核心采用无迹卡尔曼滤波（Unscented Kalman Filter, UKF）算法，专门用于解决非线性动态系统中的状态搜索与估计问题。在目标跟踪领域，当传感器观测模型（如雷达的距离和角度）或目标的运动模型（如协同转弯模型）具有强非线性时，传统的线性卡尔曼滤波不再适用。本项目通过无需计算雅可比矩阵的无迹变换技术，实现了比扩展卡尔曼滤波（EKF）更高精度的估计效果，能够实时恢复含噪声目标的运动轨迹。

功能特性

• 强非线性处理：通过采样Sigma点进行无迹变换，精确捕捉状态分布的均值和协方差。
• 协同转弯模型(CT)：支持目标在水平面内以恒定速率转弯的复杂运动建模。
• 非线性观测集成：直接处理径向距离和方位角的非线性测量数据。
• 数值稳定性：采用Cholesky分解生成Sigma点，并包含角度归一化处理（wrapToPi）以防止滤波发散。
• 多维度评估：自动计算位置和速度的均方根误差（RMSE），并生成多维度的可视化分析图表。

系统要求

• 环境：MATLAB R2016b 或更高版本。
• 工具箱：基础MATLAB环境即可运行，无需额外特殊工具箱。

算法实现逻辑

项目核心逻辑遵循无迹卡尔曼滤波的标准流程，具体步骤如下：

1. 系统参数初始化 算法配置了5维状态向量：包括X轴位置、Y轴位置、X轴速度、Y轴速度以及转弯率（omega）。设定采样时间 $T$ 为1秒，同时定义了过程噪声协方差 $Q$（涵盖位置、速度和转弯率）和测量噪声协方差 $R$（涵盖距离和角度）。

2. UKF 权重计算 根据无迹变换理论，通过分布参数（alpha=1e-3, kappa=0, beta=2）计算均值权重 $w_m$ 和协方差权重 $w_c$。通过确定的比例缩放，保证采样点能够代表先验分布的统计特性。

3. 运动轨迹与观测模拟 利用协同转弯（State Transition）方程生成目标的真实运行轨迹。随后，将直角坐标系下的状态通过非线性函数转换为雷达坐标系下的距离和方位角数据，并添加高斯白噪声作为滤波器的输入。

4. 递推滤波循环

• 预测步骤：利用当前的估计值和协方差矩阵，通过Cholesky分解生成 $2n+1$ 个Sigma点；将这些点代入非线性运动模型进行传播，加权求和得出预测的均值和协方差。
• 更新步骤：将预测后的Sigma点映射到观测空间（计算距离和角度）；计算预测观测与实际观测的残差（对角度残差进行归一化处理）；计算交叉协方差和卡尔曼增益，最终修正状态向量和协方差矩阵。

5. 性能评估与可视化 计算每一时刻估计轨迹与真实轨迹的欧氏距离误差，并输出全局平均位置/速度RMSE。

关键函数与实现细节分析

• 状态转移模型 (State Transition Function)：该函数实现了协同转弯（CT）模型。当转弯率不为零时，采用三角函数项（$sin(omega T)$ 和 $cos(omega T)$）描述速度和位置的耦合变化；当转弯率接近零时，自动退化为线性匀速模型，保证了数值计算的健壮性。
• Sigma 点生成：算法通过 sqrt(n + lambda) * L 计算偏移量（其中 $L$ 是协方差矩阵的下三角分解），在当前估计值周围对称地选取采样点。
• 角度归一化处理 (wrapToPi)：在观测更新中，由于方位角在 $[-pi, pi]$ 边界存在跳变，算法专门设计了角度修正处理，确保残差计算不会因为周期性问题导致滤波器失效。
• 卡尔曼增益动态计算：通过预测点在观测空间的离散分布计算 $P_{zz}$（观测协方差）和 $P_{xz}$（互协方差），直接解算增益矩阵，避免了繁琐的偏导数计算。

使用方法

1. 启动MATLAB软件。
2. 将项目代码复制到编辑器中并保存，或者直接打开包含主函数的脚本。
3. 点击“运行 (Run)”按钮。
4. 在命令行窗口查看计算出的平均位置误差（RMSE）和平均速度误差。
5. 观察弹出的四个图形窗口：

* 轨迹对比图：显示真实轨迹、原始含噪观测和UKF估计轨迹的对比。 * 误差演化图：展示位置和速度随时间变化的误差曲线。 * 权重分布图：展示用于无迹变换的Sigma点权重。 * 协方差收敛图：展示方差矩阵对角线元素的演化过程，反映滤波器收敛的快慢。