一阶与二阶扩展卡尔曼滤波算法库

资 源 简 介

本项目是一个专门用于MATLAB环境下的非线性状态估计算法库，其核心在于实现了扩展卡尔曼滤波（EKF）的完整预测与更新逻辑。项目深度集成了针对非线性系统建模的一阶和二阶预测M文件。一阶扩展卡尔曼滤波采用泰勒级数的一阶展开，通过计算状态转移方程和观测方程的雅可比矩阵，将非线性系统进行局部线性化处理。二阶扩展卡尔曼滤波则更进一步，在预测步中引入了二阶偏导数（黑塞矩阵）修正项，能够补偿一阶线性化带来的截断误差，从而在处理强非线性动态系统（如航天器轨道修正、自动驾驶路径跟踪等）时表现出更高的估计精度和更好的数值收

详 情 说 明

项目介绍

本项目是一个专门用于MATLAB环境下的非线性状态估计算法库，聚焦于扩展卡尔曼滤波（EKF）的一阶与二阶实现。项目通过一个经典的强非线性动态系统模型，演示了如何通过高阶泰勒展开来提升滤波算法在非线性系统中的估计精度。二阶EKF通过引入黑塞矩阵（Hessian Matrix）修正项，有效地补偿了标准一阶EKF由于线性化导致的截断误差，为导航、制导与控制等领域的精密状态估计提供了理论参考和仿真实现方案。

功能特性

1. 非线性系统仿真：内置了包含正弦扰动的复杂非线性状态转移模型和二次项观测模型，能够模拟真实物理世界的非线性动态。
2. 一阶EKF实现：完整实现了基于雅可比矩阵的标准化预测与更新逻辑，作为性能对比的基准。
3. 二阶EKF实现：实现了包含二阶项修正的预测步和更新步，能够处理更强的非线性特征。
4. 综合性能评价：自动计算均方根误差（RMSE）、观测残差以及误差协方差演变曲线，直观展示算法的收敛速度与估计精度。
5. 多维度可视化：通过四分图形式展示状态追踪轨迹、绝对误差、协方差变化及残差分布。

算法逻辑与实现细节

系统建模

程序模拟了一个离散时间非线性系统。状态转移方程包含线性项、非线性分式项以及随时间变化的余弦项。观测方程则采用了具有强非线性的平方项模型。系统中加入了高斯白噪声以模拟过程扰动和测量误差。

一阶扩展卡尔曼滤波 (EKF1) 实现逻辑

一阶EKF的核心在于局部线性化。在预测阶段，利用雅可比矩阵计算先验协方差；在更新阶段，通过观测雅可比矩阵计算卡尔曼增益。此逻辑仅保留泰勒展开的一阶项，适用于非线性程度较低或采样间隔极短的场景。

二阶扩展卡尔曼滤波 (EKF2) 实现逻辑

二阶EKF在预测和更新步中均引入了二阶偏导数修正。

1. 状态预测修正：在标准预测值的基础上，增加了基于黑塞矩阵和前一时刻协方差的修正项，使预测值更接近真实均值。
2. 协方差预测增强：协方差预测公式中引入了黑塞矩阵的平方项修正，以反映二阶线性化带来的不确定性。
3. 观测预测与创新：对预测观测值进行二阶修正，并同步更新创新协方差（S矩阵），从而使卡尔曼增益的计算更加精确。

关键函数分析

状态转移函数：实现了非线性状态方程，该方程具有显著的摆动特性和非线性衰减特征。

观测模型函数：将状态映射到观测空间，采用平方比例缩放逻辑。

状态转移雅可比函数：计算状态方程对状态变量的一阶偏导数，决定了线性化的基本斜率。

观测雅可比函数：计算观测方程对状态变量的一阶偏导数，定义了观测空间的线性投影模式。

状态转移黑塞函数：计算状态方程对于状态变量的二阶偏导数，用于捕捉系统的曲率特征，是二阶修正的关键。

观测黑塞函数：计算观测方程的二阶偏导数，在该项目中表现为一个恒定的缩放因子。

使用方法

1. 环境配置：确保计算机已安装MATLAB R2016b或更高版本。
2. 运行仿真：直接在MATLAB命令行窗口运行主程序逻辑，程序将自动启动仿真循环。
3. 参数调节：用户可以根据需要修改初始化部分的仿真时长（T）、过程噪声标准差（sigma_w）或测量噪声标准差（sigma_v）。
4. 结果查看：程序运行结束后将自动弹出结果图表，用户可以通过对比RMSE数值来评估一阶与二阶算法的性能差异。

系统要求

1. 软件环境：MATLAB。
2. 依赖工具箱：无需额外工具箱，仅使用MATLAB基础算术运算和绘图功能。
3. 硬件要求：标准个人计算机，内存4GB以上即可流畅运行。