基于EKF与UKF的非线性状态估计仿真系统

资 源 简 介

本项目是一个针对非线性动态系统状态估计的综合仿真与分析平台，重点实现并对比拓展卡尔曼滤波（EKF）与无损卡尔曼滤波（UKF）两种核心算法。在功能上，系统首先定义典型的非线性状态方程（如物体运动模型）和非线性观测方程（如雷达测距测角模型），并基于真实轨迹通过添加高斯白噪声生成仿真观测数据。EKF模块通过计算系统方程关于状态变量的雅可比矩阵（Jacobian Matrix），采用一阶泰勒级数展开对非线性模型进行局部线性化，进而利用标准卡尔曼滤波框架进行状态预测与更新；UKF模块则基于无损变换（Unscented Transform）理论，通过设计确定性的Sigma点集来近似状态的概率密度函数，通过这些点在非线性函数中的传播来直接计算均值和协方差，避免了复杂的线性化误差和雅可比矩阵计算。项目包含完整的数据生成器、滤波算法核心库以及可视化分析模块，能够实时输出滤波后的状态向量（如位置、速度、姿态等）。此外，系统还将自动计算两种算法的均方根误差（RMSE）和运行时间，通过图表直观展示UKF在强非线性场景下相对于EKF的精度提升与稳定性优势，以及两者在计算资源消耗上的差异分析。

详 情 说 明

基于MATLAB的拓展与无损卡尔曼滤波非线性状态估计系统

项目介绍

本项目是一个基于MATLAB开发的综合仿真与分析平台，旨在解决非线性动态系统的状态估计问题。通过构建典型的非线性运动模型和观测模型，系统重点实现并对比了两种主流的非线性滤波算法：拓展卡尔曼滤波 (EKF) 和 无损卡尔曼滤波 (UKF)。

该项目不仅包含了完整的算法核心逻辑，还集成了数据生成器、性能评估指标计算以及多维度的可视化分析模块。它能够直观地展示两种算法在非线性场景下的轨迹跟踪效果、误差收敛情况以及计算效率差异，为非线性滤波算法的研究与应用提供了可靠的仿真基准。

功能特性

• 高保真非线性仿真环境：实现了CTRV（恒定转弯率和速度）运动模型和雷达（距离、方位角）观测模型，模拟真实的非线性动态场景。
• 拓展卡尔曼滤波 (EKF) 引擎：基于一阶泰勒级数展开，通过计算雅可比矩阵（Jacobian Matrix）实现非线性系统的线性化近似与状态更新。
• 无损卡尔曼滤波 (UKF) 引擎：基于无损变换（Unscented Transform）理论，采用确定性采样（Sigma点）策略，直接逼近状态概率密度函数，避免了线性化误差。
• 自动性能评估：实时计算算法运行时间，并自动统计各状态量的均方根误差 (RMSE)，量化对比算法精度。
• 多维度可视化：

* 全局轨迹跟踪对比图（真实值、观测值、EKF估计、UKF估计）。 * 状态估计误差与 $3sigma$ 协方差包络线图。 * RMSE 统计柱状图。

系统要求

• MATLAB R2016b 或更高版本。
• 建议安装 Signal Processing Toolbox 或 Phased Array System Toolbox（用于支持 wrapToPi 等辅助函数，如无工具箱需确认环境中包含该函数）。

代码实现逻辑与算法细节

本项目的主程序脚本集成了参数配置、数据生成、滤波算法执行及结果分析的全过程，具体实现逻辑如下：

1. 系统模型与参数初始化

程序首先定义了5维状态向量 $x = [pos_x, pos_y, velocity, heading, turn_rate]^T$。

• 过程噪声 (Q)：为位置、速度、航向角和转弯率设置了独立的高斯白噪声方差，模拟模型的不确定性。
• 观测噪声 (R)：模拟雷达传感器的测量误差，包含测距误差和测角误差。
• 初始状态：设置了目标的真实初始位置、速度（10m/s）、航向（45度）及转弯率（5度/s）。

2. 仿真数据生成 (Ground Truth & Measurements)

系统通过循环迭代，基于设定的采样时间 (T=0.1s) 生成真实轨迹和观测数据：

• 动态方程传播：调用 sys_dynamic_model 函数，采用CTRV模型更新下一时刻状态，并注入过程噪声。针对航向角进行了归一化处理。
• 观测方程生成：调用 measurement_model 函数，将状态向量映射为观测向量（距离和方位角），并注入观测噪声。

3. 下层核心算法实现

#### 拓展卡尔曼滤波 (EKF) 流程 EKF 模块通过显式计算雅可比矩阵来处理非线性：

1. 预测：利用非线性动态方程预测状态，利用雅可比矩阵 $F$ 推导协方差矩阵 $P$ 的预测值。
2. 雅可比计算：代码内部实时计算状态转移矩阵的雅可比 $F$ 和观测矩阵的雅可比 $H$。
3. 更新：计算卡尔曼增益 $K$，利用观测残差修正状态估计，并更新协方差矩阵。
4. 特殊处理：在残差计算和状态更新中，对角度变量进行了 wrapToPi 包裹处理，防止角度跳变导致的计算错误。

#### 无损卡尔曼滤波 (UKF) 流程 UKF 模块采用无损变换处理非线性分布：

1. 参数设置：配置了 $alpha=0.001, beta=2, kappa=0$ 等UT变换参数，并预计算了均值权重 ($w_m$) 和协方差权重 ($w_c$)。
2. Sigma点生成与传播：根据当前状态和协方差生成Sigma点集，并通过非线性动态方程直接传播这些点。
3. 预测重组：加权求和传播后的Sigma点，得到预测均值和预测协方差。
4. 观测更新：将预测后的Sigma点映射到观测空间，计算预测观测均值、观测协方差 $S$ 以及状态-观测互协方差 $P_{xz}$。
5. 状态修正：基于 $P_{xz}$ 和 $S$ 计算卡尔曼增益，完成状态与协方差的最终更新。

4. 辅助函数说明

程序底部定义了关键的数学模型函数：

• sys_dynamic_model：实现了协调转弯模型（CTRV）。代码包含逻辑判断，当转弯率 $omega$ 极小时（接近0），自动切换为直线运动模型，有效防止了除零错误。
• measurement_model：实现了笛卡尔坐标系到极坐标系的转换，输出距离 $r$ 和方位角 $phi$。
• calculate_jacobian_F：解析推导了CTRV模型关于状态变量的偏导数矩阵，用于EKF的线性化过程。

5. 性能评估与可视化

程序运行结束后，会进行以下后处理：

• RMSE 计算：分别计算 EKF 和 UKF 在位置、速度、航向等分量上的均方根误差。针对航向角误差，专门处理了 $ pm pi $ 的周期性问题。
• 控制台输出：打印算法运行时间对比表和各状态量的RMSE统计数据。
• 绘图：

* 轨迹图：在二维平面上叠加显示真实轨迹、带噪观测点、EKF估计轨迹和UKF估计轨迹。 * 误差分析图：绘制X方向位置误差曲线，并叠加计算得到的 $3sigma$ 置信区间，用于验证滤波器的收敛一致性（Consistency）。 * 柱状图：直观对比两种算法在各个状态量上的估计精度。

使用方法

1. 打开 MATLAB 环境。
2. 将包含代码的主脚本文件放置于当前工作路径。
3. 直接运行主脚本函数。
4. 脚本运行完成后，控制台将输出RMSE统计表和运行耗时，并自动弹出三张分析图表。