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

资 源 简 介

本项目基于MATLAB平台开发，旨在实现扩展卡尔曼滤波（Extended Kalman Filter, EKF）算法，以解决非线性动态系统的状态估计难题。鉴于标准卡尔曼滤波仅适用于线性高斯系统，而实际工程中（如机器人导航、雷达跟踪、卫星姿态控制）大量存在非线性特性，本项目通过核心算法对非线性系统进行处理。具体实现方法是利用泰勒级数展开，将非线性的状态转移方程和观测方程在当前估计点附近进行一阶线性化截断，通过计算雅可比矩阵（Jacobian Matrix）来近似系统的局部线性行为。算法流程严格遵循“预测-更新”的递归循环：在预测阶段，根据上一时刻的后验估计推算当前时刻的先验状态及误差协方差；在更新阶段，结合带有高斯白噪声的实时测量数据计算卡尔曼增益，修正先验估计以获得最优的后验状态估计。项目内容不仅包含通用的EKF求解器函数，还包含一个完整的仿真演示框架，能够生成模拟的非线性轨迹和噪声数据，直观展示滤波器如何从含噪数据中还原真实系统状态，并提供误差分析功能以评估滤波性能。

详 情 说 明

扩展卡尔曼滤波 (EKF) 非线性系统状态估计

项目介绍

本项目是一个基于MATLAB开发的扩展卡尔曼滤波（Extended Kalman Filter, EKF）算法演示仿真。该项目旨在解决具有非线性动力学特征和非线性观测模型的系统状态估计问题。

在本项目中，我们模拟了一个在二维平面上运动的车辆（单车模型），利用EKF算法融合带有噪声的过程模型预测与带有噪声的雷达观测数据（距离和方位角），从而精确估计车辆的实时位置、速度和航向角。该代码通过泰勒级数的一阶线性化（Jacobians）处理非线性方程，展示了EKF处理非高斯、非线性系统的核心能力。

功能特性

• 非线性运动建模：模拟包含位置、速度和航向角的四维状态向量，并在运动方程中引入了恒定的转弯率，构建非线性的车辆轨迹。
• 非线性观测模拟：模拟雷达传感器模型，观测数据为极坐标系下的距离和方位角，并叠加了高斯白噪声。
• EKF核心算法实现：完整实现了预测与更新的递归循环，包括状态转移矩阵和观测矩阵的雅可比矩阵（Jacobian Calculation）计算。
• 鲁棒的标量处理：在角度差计算中实现了归一化处理（wrapToPi），防止航向角跨越 $pmpi$ 时产生错误的卡尔曼增益。
• 可视化误差分析：提供直观的轨迹对比图和包含 $3sigma$（三倍标准差）置信区间的状态误差分析图，用于评估滤波收敛性能。

系统要求

• MATLAB R2016b 或更高版本
• Statistics and Machine Learning Toolbox（用于 mvnrnd 函数生成多元正态分布噪声）

使用方法

1. 将代码保存为 main.m 文件。
2. 在 MATLAB 命令窗口或编辑器中直接运行 main 函数。
3. 程序将自动执行时长为50秒的仿真，并在运行结束后弹出两个结果分析窗口。

详细算法与代码逻辑分析

代码完全包含在一个主函数文件中，逻辑流程严格遵循EKF的标准步骤。以下是各部分的详细实现逻辑：

1. 系统参数与初始化

• 状态向量定义：$x = [p_x, p_y, v, theta]^T$，分别代表X坐标、Y坐标、速度和航向角。
• 噪声协方差矩阵：

* 过程噪声 $Q$：模拟系统内部的不确定性，主要施加在纵向加速度和角加速度上。 * 观测噪声 $R$：模拟雷达传感器的测量误差，包含测距误差（米）和测角误差（弧度）。

• 初始状态：设定真实的初始状态，并设定一个带有显著偏差的“估计初始值”，以验证滤波器修正误差的能力。初始误差协方差矩阵 $P$ 被设定为较大的值，表示初始估计的不确定性。

2. 仿真主循环 (预测-更新)

程序运行一个 for 循环，模拟总时长50秒、采样间隔0.1秒的过程：

#### A. 真实环境模拟 (Ground Truth)

• 利用非线性状态转移函数生成下一时刻的真实状态，并在过程中加入了随机过程噪声。
• 利用非线性观测函数计算真实的距离和方位，并加入随机测量噪声，生成这一时刻的模拟雷达数据。

#### B. EKF 预测阶段 (Prediction)

• 状态预测：将上一时刻的后验估计代入非线性运动方程 sys_f，得到先验状态估计。
• 线性化 (Jacobian F)：调用 jacobian_F 函数，计算状态转移方程在当前估计点处的偏导数矩阵。该矩阵反映了速度和航向角微小变化对位置更新的影响。
• 协方差预测：利用 $F$ 矩阵传播误差协方差，并叠加过程噪声 $Q$。

#### C. EKF 更新阶段 (Update)

• 观测预测：将先验状态估计代入非线性观测方程 sys_h，计算预期的雷达测量值（距离和角度）。
• 计算残差 (Innovation)：

* 计算实际测量值与预测测量值的差。 * 关键处理：对角度残差执行 wrapToPi 操作，确保角度差值始终保持在 $[-pi, pi]$ 范围内，避免因角度周期性导致的计算错误。

• 线性化 (Jacobian H)：调用 jacobian_H 函数，计算观测方程关于状态变量的偏导数。代码中加入了防除零保护，当距离极小时避免计算溢出。
• 卡尔曼增益计算与修正：计算残差协方差 $S$ 和卡尔曼增益 $K$，利用测量残差修正先验估计，得到最优后验状态 $x_{est}$ 和更新后的协方差 $P$。

3. 辅助函数详解

• 非线性状态转移函数：基于单车运动学模型，根据当前的速度和航向角更新位置，并假设存在0.05 rad/s的恒定转弯率，使车辆做圆周运动。
• 非线性观测函数：将笛卡尔坐标系下的状态 $(p_x, p_y)$ 转换为极坐标系下的 $(range, bearing)$。
• Jacobian F 计算：解析推导了运动方程对状态向量的偏导数，构建 $4times4$ 的状态转移雅可比矩阵。
• Jacobian H 计算：解析推导了 sqrt 和 atan2 函数对位置坐标的偏导数，构建 $2times4$ 的观测雅可比矩阵。

4. 结果可视化

代码最后部分通过 plot_results 函数生成两张图表：

• 轨迹跟踪图：在X-Y平面绘制真实轨迹（黑实线）、EKF估计轨迹（红虚线）以及转换回直角坐标系的含噪雷达观测点（灰点），直观展示滤波效果。
• 状态误差分析图：包含四个子图，分别显示X位置、Y位置、速度和航向角的实时误差（真实值 - 估计值）。图中还绘制了红色虚线表示 $pm3sigma$ 边界（这也是从协方差矩阵 $P$ 的对角线元素计算得出的），用于验证滤波器是否一致收敛（误差应主要由 $3sigma$ 包络线包裹）。