EKF与UKF非线性状态估计算法对比实现

资 源 简 介

本系统旨在MATLAB环境下完整实现并对比两种主流的非线性状态估计算法：拓展卡尔曼滤波（EKF）和无损卡尔曼滤波（UKF）。 针对非线性系统状态估计中的不确定性问题，本项目首先建立了通用的非线性动力学模型和传感器观测模型。 EKF部分采用泰勒级数展开的方法，通过实时计算状态转移函数和观测函数的雅可比矩阵，将非线性问题局部线性化，从而利用标准卡尔曼滤波框架进行状态更新。 UKF部分则采用无损变换（Unscented Transform）技术，通过确定性采样生成的Sigma点集来逼近非线性分布，绕过了雅可比矩

详 情 说 明

基于MATLAB的拓展卡尔曼滤波(EKF)与无损卡尔曼滤波(UKF)对比实现项目说明

项目介绍

本项目是一个在MATLAB环境下开发的非线性状态估计对比系统。核心任务是模拟一个典型的雷达目标跟踪场景，其中目标在二维平面上做匀速直线运动，而观测传感器（雷达）通过非线性的极坐标方式（距离和角度）获取数据。本项目通过完整实现EKF和UKF两种算法，旨在量化分析线性化近似与确定性采样技术在处理非线性观测模型时的性能差异。

功能特性

• 双算法并行处理：在同一仿真环境下，同步运行EKF与UKF算法，确保对比基准的一致性。
• 非线性模型模拟：内置极坐标观测模型，能够模拟真实的雷达距离和方位角探测过程。
• 动态性能评估：自动计算并输出位置均方根误差(RMSE)、速度均方根误差以及算法运行耗时。
• 多维度可视化：提供二维运动轨迹对比图以及随时间变化的坐标轴绝对误差曲线图。
• 角度归一化处理：针对非线性系统中的角度突变问题，内置了残差归一化逻辑，有效防止滤波器发散。

系统要求

• 运行环境：MATLAB R2016b 及以上版本。
• 工具箱依赖：需要安装 Statistics and Machine Learning Toolbox（用于生成多变量高斯噪声）。

算法实现逻辑

项目中算法的运行流程主要分为以下五个阶段：

1. 参数初始化与场景建模 系统设定采样时间为1秒，共仿真60个步长。状态向量包含横纵坐标及对应的速度分支。过程噪声矩阵 $Q$ 基于运动模型构建，观测噪声矩阵 $R$ 根据雷达测距和测角的精度标准差设定。

2. 仿真数据生成 使用线性恒速(CV)模型推演目标的真实轨迹。在每个时间步，系统将当前的直角坐标状态通过非线性映射转化为极坐标，并叠加高斯白噪声，生成模拟的雷达观测序列。

3. 拓展卡尔曼滤波(EKF)实现

• 预测：利用线性状态转移矩阵进行先验状态估计。
• 线性化：通过计算观测函数对状态变量的偏导数（雅可比矩阵），在当前预测点处将非线性观测模型局部线性化。
• 修正：计算卡尔曼增益，并结合观测残差（经角度归一化处理）更新后验状态。

4. 无损卡尔曼滤波(UKF)实现

• Sigma点采样：基于均值和协方差矩阵，利用Cholesky分解生成一组对称分布的Sigma点。
• 无损变换(UT)：将采集的Sigma点通过线性转移模型映射到预测空间。
• 观测映射：将预测后的Sigma点逐一带入非线性观测函数，从而获得观测空间的分布特性。
• 加权融合：通过预定义的权重计算预测均值、观测均值、互协方差及创新协方差，完成状态修正。

5. 性能指标分析 系统自动对比真实值与两种滤波器的估计值，计算位置与速度的累计RMSE，并记录 tic/toc 耗时，最终以表格形式在控制台输出。

关键函数与实现细节分析

非线性观测转换逻辑 系统通过专门的子函数实现从直角坐标系到极坐标系的转换。计算公式包含 $r = sqrt{x^2 + y^2}$ 以及 $theta = text{atan2}(y, x)$。该转换是整个系统中非线性的来源。

雅可比矩阵计算 针对EKF，项目实现了一个手动推导的雅可比求导函数。它处理了观测函数对位置分量的导数，并加入了奇异值保护机制。如果目标距离传感器过近（距离平方小于1e-6），则将雅可比矩阵置零，以保证数值计算的稳定性。

Sigma点权重配置 在UKF部分，引入了三个比例参数：$alpha$ 调节Sigma点的分布范围（默认为1e-3，体现强约束），$kappa$ 为待选次级参数，$beta$ 用于引入状态分布的先验信息（高斯分布下默认为2）。这些参数共同决定了均值权重 $W_m$ 和协方差权重 $W_c$。

残差归一化 在EKF的更新步和UKF的观测空间计算中，针对角度残差项，使用了 atan2(sin(diff), cos(diff)) 逻辑。这一步至关重要，因为它确保了当角度在 $pmpi$ 附近跳变时，滤波器不会因为残差过大而计算崩溃。

使用方法

1. 打开MATLAB软件，并将工作路径指向项目文件所在目录。
2. 在命令行窗口输入该主程序的全名并回车。
3. 程序将自动执行仿真，并在完成后弹出两张可视化图表。
4. 在MATLAB控制台（Command Window）查看两种算法的具体性能指标对比表。