4. 扩展卡尔曼滤波（EKF）：非线性系统线性化、雅可比矩阵计算、EKF在导航中的应用
卡尔曼滤波（KF）在导航领域很强大，但它有个硬伤——只能处理线性系统。现实世界哪有那么多线性？车辆转弯、卫星轨道、传感器姿态，全是非线性。这时候，扩展卡尔曼滤波（EKF）就派上用场了。
说白了，EKF 的核心思想就是：把非线性问题强行“掰直”，然后用标准卡尔曼滤波那一套去处理。怎么掰？用泰勒展开，只取一阶项。我刚开始学的时候觉得这方法有点粗暴，后来在项目中跑通了，才发现这招确实管用。
核心一句话：EKF 通过雅可比矩阵对非线性函数进行局部线性化，使得卡尔曼滤波框架可以应用于非线性系统。
4.1 为什么需要非线性处理？
先看一个典型的导航场景。你有个无人机，状态量是位置 (x, y) 和偏航角 θ。你通过里程计测量速度 v 和角速度 ω。状态方程是这样的：
x_k = x_{k-1} + v * cos(θ) * dt
y_k = y_{k-1} + v * sin(θ) * dt
θ_k = θ_{k-1} + ω * dt看到 cos 和 sin 了吗？这就是非线性。标准卡尔曼滤波假设状态转移和观测都是线性关系，遇到这种三角函数就傻眼了。
我印象很深，有一次做车载组合导航，直接用标准 KF 处理 GPS 和 IMU 数据，结果滤波发散得一塌糊涂。后来换成 EKF，问题就解决了。原因很简单——车辆转弯时的航向变化，必须用非线性模型才能准确描述。
4.2 非线性系统线性化：泰勒展开
EKF 的线性化手段是一阶泰勒展开。假设我们有非线性状态转移函数 f(x) 和观测函数 h(x)，在某个工作点（通常是上一时刻的状态估计值）附近展开：
f(x) ≈ f(x̂) + F * (x - x̂)
h(x) ≈ h(x̄) + H * (x - x̄)这里的 F 和 H 就是雅可比矩阵。它们分别描述了状态转移和观测函数对状态变量的偏导数。
你想想看，这就像你在爬山时，用脚下的切线方向来近似山势。虽然不能精确描述整座山，但在你站的那一小块区域里，这个近似是足够用的。
我的经验：泰勒展开的精度取决于两个因素：一是展开点离真实值有多近，二是系统非线性的强度。如果非线性太强（比如大角度机动），一阶近似可能不够，这时候可以考虑二阶 EKF 或无迹卡尔曼滤波（UKF）。
4.3 雅可比矩阵计算
雅可比矩阵是 EKF 的“心脏”。计算错了，整个滤波就废了。我们来看一个具体的例子。
假设状态向量为 x = [x, y, θ]ᵀ，控制输入为 u = [v, ω]ᵀ。状态转移函数 f 为：
f₁ = x + v * cos(θ) * dt
f₂ = y + v * sin(θ) * dt
f₃ = θ + ω * dt状态转移的雅可比矩阵 F 是 f 对 x 的偏导数：
F = ∂f/∂x =
[ 1, 0, -v * sin(θ) * dt ]
[ 0, 1, v * cos(θ) * dt ]
[ 0, 0, 1 ]注意第三列，那是 f 对 θ 的偏导。cos 的导数是 -sin，sin 的导数是 cos。这个细节很容易写错，我就在这上面栽过跟头。
我曾经踩过的坑：有一次我把 sin 的导数写成了 cos，结果滤波结果在转弯时出现剧烈震荡。排查了两天才发现是雅可比矩阵算错了。所以我的建议是：每次写完雅可比，先用数值微分验证一遍。
数值微分验证的方法很简单：
# 数值微分验证雅可比
epsilon = 1e-6
F_numerical = np.zeros((3, 3))
for i in range(3):
x_plus = x.copy()
x_plus[i] += epsilon
x_minus = x.copy()
x_minus[i] -= epsilon
f_plus = f(x_plus, u, dt)
f_minus = f(x_minus, u, dt)
F_numerical[:, i] = (f_plus - f_minus) / (2 * epsilon)把解析解和数值解对比一下，误差在 1e-6 量级就说明算对了。
4.4 EKF 在导航中的完整流程
EKF 的流程和标准 KF 很像，只是把线性矩阵换成了雅可比矩阵。我习惯把它分成五个步骤：
- 预测状态：用非线性状态转移函数计算先验估计 x̄_k = f(x̂_{k-1}, u_k)
- 预测协方差：P̄_k = F * P_{k-1} * Fᵀ + Q
- 计算卡尔曼增益：K_k = P̄_k * Hᵀ * (H * P̄_k * Hᵀ + R)⁻¹
- 更新状态：x̂_k = x̄_k + K_k * (z_k - h(x̄_k))
- 更新协方差：P_k = (I - K_k * H) * P̄_k
注意第三步和第四步里的 H 和 h。H 是观测函数的雅可比矩阵，h(x̄_k) 是预测观测值。残差 z_k - h(x̄_k) 是在非线性观测空间里算的，不是线性化之后的。
下面这张图展示了 EKF 在导航中的完整数据流：
4.5 导航中的典型应用场景
EKF 在导航领域用得最多的几个地方：
| 应用场景 | 状态量 | 非线性来源 | 观测方式 |
|---|---|---|---|
| GPS/IMU 组合导航 | 位置、速度、姿态 | 姿态旋转、重力模型 | GPS 伪距、IMU 加速度/角速度 |
| 车辆航位推算 | 位置、航向 | cos/sin 航向变化 | 轮速编码器、陀螺仪 |
| 无人机视觉SLAM | 位姿、地图点 | 相机投影模型 | 图像特征点匹配 |
| 机器人里程计融合 | 位姿、速度 | 运动学模型 | 激光雷达、IMU |
拿 GPS/IMU 组合导航来说，IMU 的加速度计和陀螺仪数据是高频的（100Hz 以上），但存在漂移。GPS 是低频的（10Hz），但精度高。EKF 的作用就是把两者融合起来——用 IMU 做高频预测，用 GPS 做低频修正。
我的建议：在实际项目中，不要一上来就写完整的 EKF。先写一个简化版本，只处理位置和速度，验证雅可比矩阵和协方差传播是否正确。确认无误后，再逐步加入姿态、偏置等状态量。这样调试起来会轻松很多。
4.6 调参实战要点
EKF 的调参和标准 KF 类似，但有几个额外需要注意的地方：
- 过程噪声 Q 矩阵：非线性系统的 Q 通常比线性系统大一些，因为线性化本身会引入误差。我一般会把 Q 设成标准 KF 的 1.5 到 2 倍。
- 观测噪声 R 矩阵：这个和传感器特性有关。GPS 的 R 可以根据 HDOP 动态调整，IMU 的 R 通常固定。
- 初始协方差 P₀：如果初始状态不确定，P₀ 设大一点（比如对角元素 100），让滤波器快速收敛。
- 雅可比更新频率：如果计算资源紧张，可以每 N 步重新计算一次雅可比矩阵，而不是每步都算。N 通常取 5-10。
我曾经踩过的坑：有一次在嵌入式平台上跑 EKF，发现每次计算雅可比矩阵耗时太长，导致滤波频率跟不上 IMU 数据率。后来改成每 5 步计算一次雅可比，中间步骤用上一次的雅可比近似，问题就解决了。代价是精度略有下降，但在可接受范围内。
4.7 一个完整的 EKF 代码框架
下面是一个简化的 EKF 实现框架，适用于二维车辆导航：
class EKF:
def __init__(self, dt):
self.dt = dt
# 状态: [x, y, θ]
self.x = np.zeros(3)
self.P = np.eye(3) * 100 # 初始协方差
self.Q = np.diag([0.1, 0.1, 0.05]) # 过程噪声
self.R = np.diag([1.0, 1.0]) # 观测噪声（GPS位置）
def f(self, x, u):
"""非线性状态转移函数"""
v, ω = u
θ = x[2]
return np.array([
x[0] + v * np.cos(θ) * self.dt,
x[1] + v * np.sin(θ) * self.dt,
x[2] + ω * self.dt
])
def F_jacobian(self, x, u):
"""状态转移雅可比矩阵"""
v, ω = u
θ = x[2]
return np.array([
[1, 0, -v * np.sin(θ) * self.dt],
[0, 1, v * np.cos(θ) * self.dt],
[0, 0, 1]
])
def h(self, x):
"""观测函数：直接观测位置"""
return x[:2]
def H_jacobian(self, x):
"""观测雅可比矩阵"""
return np.array([
[1, 0, 0],
[0, 1, 0]
])
def predict(self, u):
"""预测步骤"""
F = self.F_jacobian(self.x, u)
self.x = self.f(self.x, u)
self.P = F @ self.P @ F.T + self.Q
def update(self, z):
"""更新步骤"""
H = self.H_jacobian(self.x)
y = z - self.h(self.x) # 残差
S = H @ self.P @ H.T + self.R
K = self.P @ H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(3) - K @ H) @ self.P这个框架虽然简单，但已经包含了 EKF 的核心逻辑。实际项目中，你只需要替换 f、h 以及对应的雅可比矩阵，就能适配不同的传感器和运动模型。
嗯，EKF 的内容就讲到这里。记住一点：雅可比矩阵是 EKF 的灵魂，算对了，滤波就成功了一半。剩下的就是调参和工程优化了。