扩展卡尔曼滤波(EKF)原理与实现详解

1. 扩展卡尔曼滤波（EKF）基础概念

在机器人定位、自动驾驶和无人机导航等领域，我们经常需要处理非线性系统的状态估计问题。标准卡尔曼滤波（KF）虽然优雅高效，但它仅适用于线性系统。而现实世界中，绝大多数系统都是非线性的——从无人机的空气阻力到GPS的极坐标转换，非线性无处不在。

扩展卡尔曼滤波（EKF）就是为解决这个问题而生的。它的核心思想很直观：通过一阶泰勒展开，将非线性系统在局部近似为线性系统，从而继续使用卡尔曼滤波的框架。这种近似虽然会引入误差，但对于许多实际应用来说已经足够精确。

1.1 EKF与标准KF的关键区别

标准KF假设系统的状态转移和观测都是线性的，可以用矩阵表示：

• 状态方程：x_k = Fx_{k-1} + w_k
• 观测方程：z_k = Hx_k + v_k

而EKF则允许这两个方程是非线性的：

• 状态方程：x_k = f(x_{k-1}, u_{k-1}) + w_k
• 观测方程：z_k = h(x_k) + v_k

这里的f(·)和h(·)可以是任意非线性函数，只要它们可微就行。这个看似简单的改变，却让EKF能够处理现实世界中各种复杂的非线性系统。

1.2 EKF的三大核心假设

EKF建立在三个关键假设上：

1. 非线性函数可微：因为要用泰勒展开，所以f(·)和h(·)必须可微
2. 噪声是高斯白噪声：和KF一样，过程噪声w和观测噪声v都必须是零均值高斯白噪声
3. 一阶近似足够：泰勒展开只保留一阶项，忽略高阶项带来的误差在可接受范围内

这些假设在实际应用中意味着什么？举个例子，如果你用EKF做无人机定位：

• 无人机的运动模型（f）必须平滑，不能有突变或不可导的点
• 传感器噪声要尽量符合高斯分布，否则滤波效果会变差
• 系统的非线性不能太强，否则一阶近似误差会很大

2. EKF的数学原理详解

2.1 非线性系统的数学表示

一个典型的非线性系统可以表示为：

状态方程（过程模型）：
x_k = f(x_{k-1}, u_{k-1}) + w_

观测方程（测量模型）：
z_k = h(x_k) + v_k

其中：

• x_k是系统状态（如位置、速度等）
• u_k是控制输入
• w_k是过程噪声（协方差Q）
• v_k是观测噪声（协方差R）
• f(·)是非线性状态转移函数
• h(·)是非线性观测函数

2.2 泰勒展开与线性化

EKF的核心技巧是对非线性函数进行一阶泰勒展开。以状态转移函数f为例，在x_{k-1}点展开：

f(x) ≈ f(x_{k-1}) + F_k(x - x_{k-1})

其中F_k是f在x_{k-1}处的雅可比矩阵：

F_k = ∂f/∂x |{x=x{k-1}}

同理，观测函数h在x_k点展开：

h(x) ≈ h(x_k) + H_k(x - x_k)

H_k = ∂h/∂x |_

这种线性化近似相当于用切线代替曲线，在x_k附近的小范围内效果很好，但离展开点越远误差越大。

2.3 EKF的预测-更新流程

EKF的完整流程分为预测和更新两个阶段：

预测阶段：

1. 状态预测：x̂_k^- = f(x̂_{k-1}^+, u_{k-1})
2. 计算雅可比矩阵F_k
3. 协方差预测：P_k^- = F_k P_{k-1}^+ F_k^T + Q

更新阶段：

1. 计算观测雅可比H_k
2. 计算卡尔曼增益：K_k = P_k^- H_k^T (H_k P_k^- H_k^T + R)^
3. 状态更新：x̂_k^+ = x̂_k^- + K_k (z_k - h(x̂_k^-