基于MATLAB的无迹卡尔曼滤波(UKF)算法源码

资 源 简 介

本项目提供了无迹卡尔曼滤波（Unscented Kalman Filter, UKF）算法的完整MATLAB代码实现。程序不依赖MATLAB工具箱中的现成函数，而是基于UKF数学原理从底层编写，旨在解决强非线性系统的状态估计问题。核心算法采用了经典的对称采样策略，通过生成2n+1个Sigma点（其中n为状态维数）来逼近非线性函数的概率密度分布，从而避免了扩展卡尔曼滤波（EKF）中繁琐的雅可比矩阵计算及线性化误差。代码模块清晰地实现了权重计算、Sigma点生成、无迹变换（UT）、时间更新（预测）和量测更新（校正）等关键步骤。该项目适用于目标跟踪、组合导航、电池SOC估计等非线性系统场景，用户只需替换系统状态方程和观测方程即可直接应用于自己的数据处理任务，具有极高的可移植性和教学研究价值。

详 情 说 明

基于MATLAB的无迹卡尔曼滤波(UKF)算法实现

项目介绍

本项目提供了一套完整的无迹卡尔曼滤波（Unscented Kalman Filter, UKF）算法的MATLAB实现方案。该程序完全基于底层数学原理编写，不依赖MATLAB自带的控制工具箱或任何现成的滤波函数，旨在为研究强非线性系统的状态估计提供一个清晰、可移植且易于修改的代码框架。

UKF通过无迹变换（Unscented Transformation, UT）来处理均值和协方差的非线性传递问题。与扩展卡尔曼滤波（EKF）不同，UKF不需要计算雅可比矩阵，而是采用确定性采样策略（Sigma点），能够更准确地逼近非线性函数的概率密度分布，尤其适用于高阶非线性模型。

主要功能与特性

• 零依赖实现：核心算法从零构建，包含Sigma点生成、权重计算、协方差更新等全过程，便于理解UKF内部机制。
• 非线性模型支持：代码中预置了经典的单变量非线性增长模型（UNGM），包含复杂的三角函数和分式非线性特性，用于验证算法的鲁棒性。
• 标准UKF流程：采用经典的对称采样策略（2n+1个Sigma点），包含完整的时间更新（预测）和量测更新（校正）步骤。
• 数值稳定性设计：在矩阵分解步骤中包含了针对非正定矩阵的容错处理（从Cholesky分解回退到特征值分解），增强了算法运行时的稳定性。
• 全方位可视化：程序运行结束后会自动生成包含状态跟踪对比、误差分析、3-Sigma置信区间以及误差分布直方图的综合图表。

算法实现细节

本项目的主程序脚本逻辑严密，完全按照UKF的标准数学推导流程执行，具体步骤如下：

1. 参数配置与数据模拟

程序首先定义了系统仿真参数，包括仿真步数、状态维数、观测维数以及过程噪声和测量噪声的方差。随后，脚本通过内置的非线性状态方程和观测方程，生成了真实的系统状态轨迹和受噪声污染的模拟观测数据。为了保证结果的可复现性，代码中固定了随机数种子。

2. 初始化与权重计算

在滤波循环开始前，算法初始化了状态估计值和协方差矩阵。同时，根据设置的各种比例参数（alpha, beta, kappa），预先计算了用于均值和协方差加权的系数向量（Wm 和 Wc）。其中，权重的计算考虑了高斯分布的最优参数设定（beta=2）。

3. 系统预测（时间更新）

在每个时间步，算法首先根据当前的状态估计和协方差生成一组Sigma点。这些点通过非线性状态方程传播得到预测的Sigma点集。随后，利用加权求和的方式，计算出预测状态的均值和预测协方差矩阵。在此阶段，过程噪声方差矩阵被累加到预测协方差中。

4. 量测更新（校正）

获得预测状态和协方差后，算法基于预测分布重新采样生成新的Sigma点（这一步提高了算法处理强非线性的能力）。这些新生成的点通过非线性观测方程传播，得到观测空间的预测Sigma点集。接着，程序计算观测量的预测均值、自协方差矩阵以及状态与观测的互协方差矩阵。最终，计算卡尔曼增益，利用真实观测值对状态和协方差进行更新。

5. 结果评估与绘图

滤波循环结束后，程序计算均方根误差（RMSE）以量化估计精度，并输出总计算耗时。最后，通过三个子图展示仿真结果：

• 状态跟踪图：直观展示真实值、估计值与含噪观测值的对比。
• 误差分析图：绘制状态估计误差，并叠加3倍标准差（3-Sigma）边界，用于验证滤波器的一致性（误差是否在置信区间内）。
• 误差分布图：统计误差的直方图并拟合高斯分布曲线，检验残差是否符合高斯白噪声假设。

关键函数与算法逻辑

代码中包含以下几个核心的辅助函数，它们构成了UKF的数学基础：

• state_function

定义了系统的状态转移模型。代码中实现的是一个强非线性模型，包含线性项、分式非线性项以及随时间余弦变化的强迫项。

• measurement_function

定义了系统的观测模型。代码中实现了一个将状态平方并缩放的非线性映射，模拟了传感器对状态的非线性响应。

• calculate_weights

根据状态维数和缩放参数计算Sigma点的均值权重（Wm）和协方差权重（Wc）。该函数还负责计算lambda参数，用于控制Sigma点的分布范围。

• generate_sigma_points

负责根据给定的状态均值和协方差矩阵生成Sigma点。该函数通过矩阵的Cholesky分解来计算协方差的平方根矩阵。为了处理数值计算中可能出现的矩阵非正定问题，该函数包含一个try-catch结构，当Cholesky分解失败时，自动切换到基于特征值分解（Eig）的鲁棒算法。

使用方法

1. 确保计算机上安装了MATLAB软件。
2. 打开MATLAB，将工作路径切换到包含脚本的文件夹。
3. 直接运行主函数脚本（main）。
4. 等待控制台输出RMSE误差值和计算耗时，并查看弹出的仿真结果图形窗口。

系统要求

• 软件：MATLAB R2016a及以上版本（推荐）。
• 工具箱：不需要任何特定工具箱，仅需MATLAB基础模块。