基于SR-UKF的平方根无迹卡尔曼滤波算法

资 源 简 介

本算法项目详细实现了平方根无迹卡尔曼滤波（SR-UKF）的核心流程，旨在解决标准无迹卡尔曼滤波在运行过程中可能出现的协方差矩阵非正定性问题。其核心功能在于通过传递状态协方差矩阵的平方根（通常是Cholesky分解的下三角矩阵），在计算过程中利用QR分解和秩一更新来维护状态协方差的数值稳定性，并显著减少计算量。该项目特别针对初学者设计，包含完整的仿真环境，能够处理典型的非线性系统状态估计任务。项目实现了Sigma点的生成、非线性传播、加权值重构以及平方根形式的状态更新。代码结构清晰易读，将系统更新与滤波过程

详 情 说 明

平方根无迹卡尔曼滤波 (SR-UKF) 算法项目

项目介绍

本项目是一个基于MATLAB开发的平方根无迹卡尔曼滤波（Square Root Unscented Kalman Filter, SR-UKF）算法实现。与标准的无迹卡尔曼滤波（UKF）相比，SR-UKF通过直接传递协方差矩阵的平方根（Cholesky因子）来保证状态协方差矩阵在计算过程中的代数特征，即半正定性。该算法能有效解决因浮点运算舍入误差导致的协方差矩阵非正定问题，并显著提升在大规模非线性系统中的数值稳定性和计算效率。

功能特性

1. 数值稳定性增强：通过QR分解和秩一更新（Rank-1 Update）代替传统的协方差矩阵求和与减法，确保滤波器在长时间运行中保持稳健。
2. 非线性建模支持：算法支持高度非线性的状态转移模型和观测模型，能够处理极坐标下的雷达探测等复杂任务。
3. 计算优化：采用矩阵左除和QR分解替代部分矩阵求逆和显式协方差计算，降低了运算复杂性。
4. 模块化设计：状态传播、观测预测、Sigma点生成与加权重构均采用独立的逻辑块实现，方便用户修改系统模型。
5. 完整仿真环境：内置恒定速度（CV）运动模型、合成雷达观测数据生成器以及多维度的结果可视化分析工具。

实现逻辑与算法细节

项目的核心执行逻辑严格遵循SR-UKF的标准流程，具体步骤如下：

1. 参数初始化与权重计算

• 状态定义：系统包含四维状态量（x轴位置、x轴速度、y轴位置、y轴速度）。
• 观测定义：两维观测值，分别为目标相对于观测站的距离（Range）和方位角（Bearing）。
• 权重重构：根据比例因子（$alpha$）、状态分布参数（$beta$）和缩放参数（$kappa$）计算无迹变换（UT）所需的均值权重（$w_m$）和协方差权重（$w_c$）。
• 平方根初始化：通过对初始协方差矩阵进行Cholesky分解获取上三角矩阵 $S$。

2. 核心滤波循环

• Sigma点生成：利用当前状态估计和协方差矩阵的平方根 $S$ 生成 $2L+1$ 个Sigma点，捕获非线性分布的统计特性。
• 预测步（Time Update）：

* 对所有Sigma点执行非线性状态转移。 * 利用加权求和计算预测均值。 * QR分解更新：将传播后的Sigma点偏差（权重的平方根倍）与过程噪声的Cholesky因子合并为一个增广矩阵，通过QR分解计算预测协方差的平方根。 * 秩一更新：针对第一个Sigma点权值可能为负的情况，利用 cholupdate 函数对QR分解结果进行修正。

• 测量步（Measurement Update）：

* 将预测Sigma点映射到观测空间，计算预测观测均值。 * QR分解更新：对观测Sigma点的偏差与测量噪声因子进行QR分解，获得观测协方差的平方根。 * 互协方差计算：显式计算状态预测偏差与观测预测偏差之间的互协方差矩阵 $P_{xz}$。

• 状态调节与增益更新：

* 利用矩阵左除及预测出的平方根矩阵计算卡尔曼增益 $K$。 * 通过递归形式的 cholupdate 执行连续的秩一减法更新，直接从预测协方差平方根中减去观测带来的不确定度减少量，从而得到更新后的状态协方差平方根。

3. 数据处理与可视化

• 真实轨迹生成：基于恒定速度模型叠加高斯过程噪声生成。
• 观测模拟：根据真实位置计算极坐标距离和角度，并加入传感器白噪声。
• 评价指标：自动计算X方向和Y方向的位置均方根误差（RMSE）。

关键函数与算法分析

• 状态转移函数：实现线性/非线性动力学方程，本例中为恒定速度模型。
• 观测模型函数：负责将状态空间映射到观测空间，实现迪卡尔坐标系到极坐标系的非线性转换。
• QR分解（qr）：在预测和更新步中用于聚合Sigma点散度和环境噪声，是维持平方根形式的关键。
• Cholesky秩一更新（cholupdate）：用于在已知Cholesky因子的基础上，添加（+）或移除（-）特定向量的影响，保证矩阵更新的精度比直接加减更高。

使用方法

1. 启动MATLAB软件。
2. 将包含项目代码的目录设置为当前工作文件夹。
3. 在命令行窗口输入脚本名并回车，或直接点击编辑器中的运行按钮。
4. 程序将自动执行100步仿真。
5. 仿真结束后，MATLAB将自动弹出轨迹对比图、估计偏差曲线以及RMSE统计图表，并输出误差结果。

系统要求

• 开发环境：MATLAB R2016b 及以上版本（需支持 cholupdate 系统函数）。
• 必要工具箱：基础MATLAB即可运行。若是为了数据分析，建议具备统计与机器学习工具箱（用于 mvnrnd 函数），如果没有该工具箱，也可以自行使用 randn 替代。