基于MATLAB的扩展卡尔曼滤波器非线性状态估计系统

资 源 简 介

本项目旨在开发一个基于MATLAB的扩展卡尔曼滤波器（Extended Kalman Filter, EKF）仿真与应用平台，专门针对非线性动态系统的状态监测与参数估计问题。该系统利用在线线性化技术，通过泰勒级数展开将非线性状态转移方程和观测方程在当前估计点附近进行一阶线性化近似，从而克服标准卡尔曼滤波器仅适用于线性系统的限制。项目功能涵盖了雅可比矩阵的推导与计算（支持解析法与数值差分法）、预测步骤（时间更新）与校正步骤（量测更新）的循环迭代逻辑实现。该项目不仅能够处理高斯白噪声环境下的多维状态估计，还包含一套完整的仿真测试套件，用于模拟如雷达目标跟踪、车辆导航或电池状态估计等实际场景，直观展示滤波算法在抑制噪声、提高数据精度方面的效果，并计算均方根误差（RMSE）以量化评估算法性能。

详 情 说 明

基于MATLAB的扩展卡尔曼滤波器非线性状态估计系统

项目简介

本项目是一个基于MATLAB开发的仿真与应用平台，专注于演示和实现扩展卡尔曼滤波器（Extended Kalman Filter, EKF）在非线性动态系统中的应用。项目通过模拟一个经典的“独轮车”运动模型和一个雷达观测站，展示了EKF如何通过在线线性化技术（泰勒级数展开），从充满噪声的观测数据中精确估计系统的位置和航向角。

该系统不仅实现了完整的EKF算法循环，还包含真值轨迹生成、噪声模拟、多维度误差分析以及直观的可视化展示，是理解非线性状态估计原理和验证算法性能的理想工具。

功能特性

• 非线性系统建模：集成了非线性过程模型（独轮车运动学方程）和非线性观测模型（雷达测距测角）。
• 双模式雅可比计算：代码架构支持解析法（Analytical）和数值差分法（Numerical）两种方式计算雅可比矩阵（F和H），可通过参数灵活切换。
• 鲁棒的滤波算法：

* 实现了EKF标准的预测与校正步骤。 * 在角度处理上包含归一化逻辑（wrapToPi），有效解决航向角突变问题。 * 协方差更新采用约瑟夫（Joseph）稳定形式，保证了矩阵的对称正定性，提高了数值稳定性。

• 全流程仿真环境：自动生成含时变控制输入的真实轨迹，并叠加高斯白噪声生成仿真观测数据。
• 多维性能评估：计算位置（X, Y）和航向角（Theta）的均方根误差（RMSE），量化滤波精度。
• 可视化分析：提供轨迹对比图、带有$3sigma$置信区间的误差分析图以及协方差矩阵收敛性分析图。

系统要求

• MATLAB R2016b 或更高版本
• 推荐安装 Navigation Toolbox 或 Mapping Toolbox（用于 wrapToPi 函数，若无工具箱需手动实现该函数）

使用方法

1. 初始化环境（清理工作区、设置随机数种子等）。
2. 运行主脚本即可启动仿真。
3. 程序将自动执行以下流程：及参数设置 -> 数据生成 -> 滤波迭代 -> 误差计算 -> 绘图。
4. 控制台将输出初始化的状态信息以及最终的RMSE统计结果。
5. 运行结束后，会弹出三个图形窗口展示仿真结果。

详细实现逻辑与算法分析

该项目的主程序逻辑严密，主要分为五个核心模块：

1. 系统参数初始化

在这部分，程序定义了系统运行的基础配置：

• 时间参数：设置了总仿真时长为50秒，采样间隔为0.1秒。
• 状态定义：系统状态向量定义为 $x = [pos_x, pos_y, theta]^T$，即二维平面坐标和航向角。
• 输入控制：设计了时变的控制输入 $u = [v, omega]^T$，其中线速度恒定，角速度呈余弦变化，使模拟轨迹呈现S形或圆形特征。
• 噪声矩阵：

* 过程噪声协方差矩阵 $Q$：考虑了位置和角度的过程不确定性。 * 测量噪声协方差矩阵 $R$：模拟雷达传感器的测距和测角误差。 * 初始协方差 $P_{init}$：定义了初始估计的不确定度。

• 初始状态：设定了真实初始状态，并在此基础上人为添加偏移量作为EKF的初始估计值，以测试算法的收敛能力。

2. 真实轨迹与观测数据生成

程序首先在不启用滤波的情况下运行过程模型和观测模型：

• 利用非线性过程方程配合预设的控制输入和过程噪声，迭代生成真实的系统状态轨迹（Ground Truth）。
• 基于真实位置和雷达基站位置（设在 [-10, 20]），利用非线性观测方程生成理想观测值（距离和方位角），并叠加测量噪声得到模拟的传感器观测数据（Measurement Data）。

3. 扩展卡尔曼滤波 (EKF) 核心循环

这是代码的核心部分，包含完整的循环迭代逻辑：

• 时间更新（预测步骤）：

1. 状态预测：将上一时刻的后验状态代入非线性状态转移方程，得到先验状态估计。 2. 雅可比计算（F）：根据当前状态和输入，计算状态转移方程相对于状态向量的偏导数矩阵（雅可比矩阵）。支持解析解和数值解的切换。 3. 协方差预测：利用线性化后的F矩阵推演预测协方差 $P_{pred}$。

• 量测更新（校正步骤）：

1. 观测预测：将先验状态代入非线性观测模型，计算预测的距离和角度。 2. 残差计算：计算实际雷达观测与预测观测的差值。关键细节：对角度残差执行了 wrapToPi 操作，确保角度差值限制在 $[-pi, pi]$ 之间，防止跨越 $pmpi$ 时出现数值跳变。 3. 雅可比计算（H）：计算观测方程相对于状态向量的偏导数矩阵。 4. 卡尔曼增益计算：基于预测协方差和测量噪声计算增益矩阵 $K$。 5. 状态更新：结合残差修正先验状态，得到后验状态估计。 6. 协方差更新：使用 $(I - KH)P(I - KH)^T + KRK^T$ 的约瑟夫形式更新协方差，确保了数值计算中P矩阵的对称正定性。

4. 性能评估

• 计算真实状态与估计状态之间的误差。
• 对航向角误差同样进行了归一化处理。
• 分别计算并输出三个状态分量（X, Y, Theta）的均方根误差（RMSE），并在控制台打印。

5. 可视化分析

代码生成了三幅详细的图表：

• 轨迹跟踪结果：在2D平面上绘制真实轨迹（黑线）、EKF估计轨迹（蓝虚线）以及雷达观测点（绿色散点）。直观展示了滤波后的轨迹比原始观测更加平滑且贴近真值。
• 状态误差与$3sigma$边界：分别绘制X、Y、Theta的实时误差曲线，并叠加由协方差矩阵计算出的 $pm 3sqrt{P_{ii}}$ 包络线。这用于验证滤波器的一致性（即误差是否大部分落在置信区间内）。
• 协方差收敛性分析：绘制协方差矩阵的Frobenius范数随时间的变化，展示滤波器从初始的大不确定性逐渐收敛并趋于稳定的过程。