容积卡尔曼滤波CKF非线性状态估计仿真项目

资 源 简 介

本项目是一个基于MATLAB开发的容积卡尔曼滤波（Cubature Kalman Filter）教学与工程实践案例。CKF作为一种处理高维非线性系统状态估计的先进算法，相比于经典的扩展卡尔曼滤波（EKF）具有更高的泰勒展开精度，相比于无迹卡尔曼滤波（UKF）在处理高维系统时具有更好的数值稳定性。 本项目完整展示了CKF的核心实现过程，包括如何利用三阶球面径向容积规则来确定容积点，并执行非线性变换及协方差更新。 项目详细模拟了一个经典的目标跟踪应用场景，其中系统状态方程和观测方程均为复杂的非线性函数。核心功

详 情 说 明

容积卡尔曼滤波 (CKF) 非线性系统状态估计仿真项目

项目介绍

本项目是一个基于 MATLAB 开发的容积卡尔曼滤波（Cubature Kalman Filter, CKF）教学与工程实践案例。CKF 是一种专门用于处理高维非线性系统状态估计的先进滤波算法。在非线性处理能力上，CKF 凭借三阶球面径向容积规则，在泰勒展开精度上优于传统的扩展卡尔曼滤波（EKF）；在数值稳定性方面，CKF 函数在处理高维系统时比无迹卡尔曼滤波（UKF）表现更为稳健。

本项目通过一个经典的雷达目标跟踪场景，完整展示了 CKF 从理论推导到代码实现的全部流程，旨在帮助学习者掌握非线性滤波的核心逻辑及其在实际动态系统中的应用。

功能特性

三阶球面径向容积规则：严格按照 CKF 理论生成 2n 个等权重的容积点，确保非线性传递后的均值和协方差精度。 非线性运动与观测模型：内置匀速运动（CV）模型作为状态转移方程，以及距离-方位角（Range-Bearing）模型作为观测方程，模拟真实的雷达探测环境。 完整的滤波演化流程：涵盖状态预报（时间更新）与测量校正（状态更新）两大核心阶段，包含协方差矩阵的 Cholesky 分解与重构。 统计性能评估：自动计算位置估算的均方根误差（RMSE），通过量化指标直观反映滤波器性能。 可视化分析：自动生成目标运动轨迹对比图以及实时位置误差曲线，便于观察滤波器的收敛性与跟踪效果。

系统要求

软件环境：MATLAB R2016b 或更高版本。 必备工具箱：Statistics and Machine Learning Toolbox（用于生成多元正态分布随机数 mvnrnd）。

仿真项目实现逻辑

1. 参数初始化阶段

系统首先定义采样间隔 T 和总仿真步数 N。针对一个四维状态向量（位置与速度）和二维观测向量（距离与角度），配置过程噪声协方差矩阵 Q 和观测噪声协方差矩阵 R。程序为滤波器设定初始预测状态及较大的初始协方差 P，以模拟初始时刻的不确定性。

1. 真实场景模拟

在正式滤波前，系统根据匀速运动学方程生成目标的预测轨迹，并人为加入高斯过程噪声。随后，通过非线性观测方程将位置坐标转换为雷达感知的距离和角度，并叠加观测噪声，生成模拟的传感器输入数据。

1. CKF 核心递归循环

每一时间步，程序都会执行一次完整的 CKF 迭代： 时间更新：对当前时刻协方差进行 Cholesky 分解，利用容积因子和状态均值生成一组容积点。这些点通过非线性状态转移函数进行传播，进而计算出预测的状态均值和预测协方差矩阵。 测量更新：在预测状态的基础上重新生成容积点，通过非线性观测函数将其映射到观测空间。计算预测观测值后，获得自协方差矩阵与互协方差矩阵，最终推导出卡尔曼增益，并完成对状态向量与协方差矩阵的后验修正。

1. 结果评估与出图

滤波器运行结束后，程序通过欧几里得距离计算每一时刻的实时误差。系统会计算整个仿真过程的平均 RMSE，并绘制轨迹跟踪对比图，清晰对比真实路径与 CKF 估算路径的重合度。

关键算法与函数细节说明

容积点生成逻辑 算法核心在于利用矩阵运算生成容积点集。通过对协方差矩阵进行下三角分解，结合单位阵的正负偏移，产生 2*nx 个关键样本点。这些点能够捕捉高斯分布在非线性变换下的二阶矩信息。

状态转移函数 实现了一个标准的四维线性匀速模型（Constant Velocity）。虽然在此案例中运动模型是线性的，但 CKF 逻辑框架完全支持将其替换为复杂的非线性机动模型。

非线性观测模型 这是本项目的非线性核心，函数将笛卡尔坐标系下的位置信息转换为极坐标系下的距离（欧氏距离）和方位角（atan2 角度）。该转换具有强非线性特征，是检验 CKF 性能的关键点。

单步滤波迭代逻辑 滤波器函数封装了完整的矩阵运算。特别是在更新阶段，利用互协方差矩阵与观测自协方差矩阵的比值计算增益，避免了复杂的导数推导（Jacobian 矩阵），这也是 CKF 优于 EKF 的主要特征。

使用方法

1. 启动 MATLAB 软件，将工作路径切换至项目文件所在目录。
2. 在命令行窗口直接运行主仿真脚本函数。
3. 程序将自动开始迭代计算，并在命令行输出最终的平均位置估算误差 (RMSE)。
4. 计算完成后，系统会自动弹出仿真结果图形窗口。
5. 用户可以根据需要手动修改主函数开头的 q（过程噪声强度）或 sigma_r / sigma_theta（观测精度）参数，观察不同信噪比环境下滤波器的稳定性。