进一步理解扩展卡尔曼滤波EKF

EKF 是对 KF 的扩展，使其能够处理非线性系统，它通过一阶泰勒展开（雅可比矩阵） 在当前估计点附近对非线性函数进行局部线性化，然后应用标准 KF 的预测和更新框架。 为非线性系统的状态估计提供了一个相对高效的框架，但线性化误差导致其是次优的，且计算雅可比矩阵可能很繁琐，对高度非线性系统效果不佳。

1. 核心思想：解决非线性问题

传统卡尔曼滤波（KF） 有一个根本性的限制：它只能应用于线性系统。这意味着系统的运动（状态转移）模型和观测（测量）模型都必须是线性的，可以用矩阵运算来描述。

线性运动模型示例： (F, B 是运动常量矩阵)

线性观测模型示例：(H 是观测矩阵)

然而，现实世界中的绝大多数系统都是非线性的，比如： 非线性运动模型（机器人转弯）： 新的位置和方向取决于当前的速度和角度的正弦/余弦函数。 非线性观测模型（机器人观测一个路标）： 观测到的距离和角度与机器人、路标之间的几何关系涉及平方根和反正切函数。

扩展卡尔曼滤波（EKF） 的核心思想就是解决这个问题，通过局部线性化，将卡尔曼滤波的理论应用到非线性系统中。

2. 技术原理：局部线性化（一阶泰勒展开）

EKF 解决非线性问题的技巧不是去推导一个全新的最优滤波器，而是对一个非线性函数进行一阶泰勒展开，在当前估计值（均值）附近用一个线性函数来近似它。

假设一个非线性函数 f(x) 和 h(x)，作为非线性状态转移函数:

和非线性观测函数：

EKF 的局部线性化步骤是：

1. 通过计算雅可比矩阵F_j（对状态 x 的偏导数矩阵），对 f 在

（上一时刻的最优估计）处进行线性化。

2. 通过计算雅可比矩阵 H_j（对状态 x 的偏导数矩阵），对 h 在

（当前时刻的预测值）处进行线性化：

雅可比矩阵的本质是一个多维函数的“斜率”，它描述了当输入变量发生微小变化时，输出变量的变化情况，EKF 就是用这个“斜率”来代替原函数中的非线性部分，从而将非线性系统看成一个线性系统来处理。

由于使用的是近似，EKF 不是全局最优的，而是次优的。线性化误差的大小决定了滤波器的性能。如果系统高度非线性，或者初始估计误差很大，这个近似会非常差，可能导致滤波器发散（结果完全错误）。

3. 实际实现流程

EKF 的流程与标准 KF 非常相似，也分为预测和更新两个步骤，但关键之处在于每一步都使用了上面提到的雅可比矩阵。

定义：

x：状态向量（例如：机器人的 [x坐标, y坐标, 朝向角度θ]） P：状态估计的不确定性（协方差矩阵） u：控制输入（例如：速度v和角速度ω） z：实际测量值（例如：到某个路标的距离r和角度φ） Q：过程噪声（运动模型的不确定性） R：观测噪声（传感器的不确定性） f：非线性状态转移函数 h：非线性观测函数

步骤 1：预测 这一步使用运动模型，根据上一刻的状态和控制输入，预测当前时刻的状态和不确定性。

1.1 预测状态：

(直接使用非线性函数 f 进行状态预测)

1.2 预测协方差：

(关键区别) 这里不再使用常量矩阵 F，而是使用从 f 推导出的雅可比矩阵来传播不确定性。

步骤 2：更新 这一步使用观测模型，将实际的传感器观测值与预测的观测值进行比较，用它们的差异（残差）来修正预测的状态，使其更准确。

2.1 计算卡尔曼增益：

(关键区别) 这里使用从 h 推导出的雅可比矩阵来代替观测矩阵 H。

2.2 更新状态（观测模型）估计： 计算残差：(实际观测 与预测观测之间的差异)

2.3 更新协方差（不确定性）估计：

3. 与传统卡尔曼滤波（KF）的改进与区别