基于无迹卡尔曼滤波器的非线性状态估计仿真程序

资 源 简 介

该项目提供了一套完整的无迹卡尔曼滤波器（Unscented Kalman Filter, UKF）算法实现框架，旨在解决非线性系统的状态估计难题。与传统的扩展卡尔曼滤波器（EKF）不同，本程序采用无迹变换（Unscented Transform）技术，通过确定性采样的方式捕捉非线性分布的均值和方差，避免了对非线性函数进行泰勒级数展开及雅可比矩阵的复杂计算，从而显著提高了滤波器的精度和稳定性，特别是在高度非线性的动态系统环境下表现优异。 项目核心功能涵盖了Sigma点的生成与加权重构、非线性状态转移预测、非

详 情 说 明

基于MATLAB的无迹卡尔曼滤波器（UKF）仿真与实现

项目介绍

本项目实现了一套完整的无迹卡尔曼滤波器（Unscented Kalman Filter, UKF）仿真框架，专门用于解决非线性系统的状态估计问题。程序通过无迹变换（Unscented Transform）技术，克服了传统扩展卡尔曼滤波器（EKF）在处理高度非线性系统时因泰勒级数展开线性化带来的精度损失及雅可比矩阵计算复杂的缺点。该实现方案通过一组确定性的采样点（Sigma点）来近似非线性分布，能够精确捕捉系统状态的均值和协方差。

项目以典型的雷达目标跟踪为仿真场景，模拟了物体在二维平面内进行匀速直线运动，并通过非线性的极坐标测量（距离与角度）进行状态更新。程序集成了从环境构建、动态仿真、算法滤波到性能评估的全流程逻辑，为非线性滤波器的研究与应用提供了标准化的参考依据。

功能特性

1. 核心算法实现：完整包含了无迹变换（UT）、Sigma点采样、状态预测、测量更新以及滤波增益计算等核心逻辑。
2. 非线性系统建模：内置匀速运动模型作为状态转移方程，并结合雷达坐标变换（直角坐标转极坐标）作为观测方程，体现了UKF处理非线性测量的优势。
3. 确定性采样方案：采用标准采点策略，生成 2L+1 个采样点，并通过缩放因子 alpha、beta 和 kappa 调节采样分布的分布特性。
4. 实时仿真环境：同步生成真实轨迹与含有高斯白噪声的观测数据，提供逼近真实场景的仿真环境。
5. 多维度评估体系：自动计算位置和速度的均方根误差（RMSE），并生成包含轨迹对比图、误差演变曲线、协方差收敛图在内的可视化统计结果。

实现逻辑说明

程序的执行严格遵循以下技术流程：

1. 初始化与参数设置 程序首先定义仿真步数为 100 步，采样间隔为 0.1 秒。状态向量定义为四维，分别代表 X 方向位置、X 方向速度、Y 方向位置及 Y 方向速度。同时配置了过程噪声协方差 Q 和测量噪声协方差 R。UKF 关键参数 alpha 设置为 1e-3 以保证采样点紧凑，beta 设置为 2 以适应高斯分布，并基于这些参数预先计算均值权重 wm 和协方差权重 wc。

2. 仿真数据生成 程序通过迭代产生真实状态轨迹，运动模型基于匀速模型（CV模型）。在每一时刻，程序将状态转换为极坐标（距离和方位角），并叠加符合正态分布的测量噪声，模拟雷达传感器的实际采集过程。

3. 无迹变换（UT）与 Sigma 点生成 在滤波循环中，程序利用 Cholesky 分解对当前误差协方差矩阵进行平方根运算。通过公式生成包含中心点、正向分布点和负向分布点的共 9 个 Sigma 点（对应 4 维状态）。

4. 状态预测逻辑 每一个 Sigma 点均通过非线性状态转移函数进行演化。预测均值通过对演化后的 Sigma 点进行加权求和获得，预测协方差则通过采样点间的偏差加权求和并叠加过程噪声 Q 得到。

5. 测量更新逻辑 程序再次利用非线性观测方程将预测的 Sigma 点投影到测量空间。计算预测测量值的均值及观测协方差 Pzz（包含测量噪声 R）。通过计算状态预测点与测量预测点之间的互协方差 Pxz，推导出卡尔曼增益 K。最终根据实际测得的极坐标数据对状态量进行修正。

6. 性能评估与可视化 计算全周期的位置 RMSE 和速度 RMSE。程序输出三张图表：

• 轨迹对比图：展示真实路径、UKF 估计路径以及原始噪声观测点的对比。
• 状态估计误差图：分别针对位置和速度显示 X/Y 方向的绝对误差随时间的变化。
• 稳定性分析图：展示协方差矩阵对角线元素的收敛情况，并以柱状图直观反映系统整体精度。

关键算法与实现细节分析

• 矩阵平方根分解：采用 chol 函数实现的 Cholesky 分解是 UKF 的核心。它保证了生成的 Sigma 点能够准确反映当前概率分布的扩展程度，且具有良好的数值稳定性。
• 非线性度处理：测量更新步骤中，程序直接处理 sqrt(px^2 + py^2) 和 atan2(py, px) 函数，无需对这些函数求偏导数，极大地简化了复杂传感器的建模过程。
• 权重补偿机制：权重 wm 和 wc 的差异化设计，特别是在中心权重 wc(1) 中引入了 alpha 和 beta 相关的修正项，增强了对状态分布高阶矩的捕捉能力。
• 数值精度：由于采用了缩放因子 lambda 的调整，当 alpha 选取较小时，能够有效防止协方差矩阵在非线性变换过程中失去正定性。

系统要求

• 运行环境：MATLAB R2016b 或更高版本。
• 所需工具箱：Statistics and Machine Learning Toolbox（用于处理 mvnrnd 等概率分布函数）。
• 硬件配置：标准个人电脑均可平滑运行，计算复杂度较低。

使用方法

1. 打开 MATLAB 软件并将工作路径设置到代码所在目录。
2. 直接运行核心程序文件。
3. 观察命令行窗口输出的 RMSE 统计结果和最终状态统计。
4. 查看自动生成的图形界面，分析滤波器在跟踪过程中的收敛速度与估计精度。
5. 如需适配特定场景，可根据实际物理模型修改状态转移矩阵或测量更新方程。