4、扩展卡尔曼滤波：非线性系统建模、雅可比矩阵计算、EKF在GPS/IMU融合中的应用
各位同学，欢迎来到第四章。
前面我们聊了标准卡尔曼滤波，它很强大，但有个硬伤——只能处理线性系统。现实世界哪有那么多线性关系？GPS信号传到你手机里，中间隔着大气层、多路径效应；IMU输出的加速度和角速度，要换算成位置和姿态，全是三角函数和旋转矩阵。说白了，真实世界是非线性的。
那怎么办？扩展卡尔曼滤波（EKF）就是来解决这个问题的。它的核心思想很朴素：既然非线性不好处理，那我就用泰勒展开，在估计点附近把它线性化。嗯，听起来有点绕，我们一步步拆解。
4.1 非线性系统建模：状态方程与观测方程
在EKF里，我们的系统模型长这样：
- 状态方程：\( x_k = f(x_{k-1}, u_k) + w_k \)
- 观测方程：\( z_k = h(x_k) + v_k \)
注意，这里的 \( f \) 和 \( h \) 都是非线性函数。\( w_k \) 和 \( v_k \) 仍然是高斯噪声，这点没变。
举个例子。假设我们要融合GPS和IMU。IMU提供加速度和角速度，GPS提供位置和速度。状态向量通常包括位置、速度、姿态（四元数或欧拉角）、以及IMU的零偏。
我个人习惯用四元数表示姿态，因为它没有万向锁问题。状态方程 \( f \) 就是IMU的动力学模型：
# 简化的IMU状态预测（离散时间）
def predict_state(x, u, dt):
# x: [px, py, pz, vx, vy, vz, qw, qx, qy, qz, bgx, bgy, bgz, bax, bay, baz]
# u: [ax, ay, az, wx, wy, wz] # IMU测量值
p = x[0:3] + x[3:6]*dt + 0.5*(u[0:3] - x[13:16])*dt**2
v = x[3:6] + (u[0:3] - x[13:16])*dt
q = quaternion_update(x[6:10], u[3:6] - x[10:13], dt) # 四元数更新
bg = x[10:13] # 陀螺零偏，通常建模为随机游走
ba = x[13:16] # 加速度零偏
return np.hstack([p, v, q, bg, ba])观测方程 \( h \) 就是GPS的测量模型。GPS直接给出位置和速度，所以 \( h \) 就是从状态向量里取出位置和速度：
def observe_gps(x):
# 从状态中提取位置和速度
return x[0:6] # [px, py, pz, vx, vy, vz]你看，观测方程反而是线性的。但状态方程是非线性的，因为里面有四元数更新、有加速度积分。这就是EKF要处理的核心问题。
4.2 雅可比矩阵计算：线性化的关键
EKF和标准KF最大的区别，就在于这一步。标准KF直接使用状态转移矩阵 \( F \) 和观测矩阵 \( H \)。EKF没有现成的，得自己算。
怎么算？对非线性函数 \( f \) 和 \( h \) 求偏导。得到的矩阵就叫雅可比矩阵。
我曾经在项目里踩过一个坑。当时为了省事，我用数值差分代替解析求导。结果滤波器跑着跑着就发散了。后来排查发现，数值差分的步长选得不对，导致雅可比矩阵不准确。从那以后，我坚持用解析雅可比。
解析雅可比怎么算？拿上面的状态方程举例。我们需要计算 \( F_k = \frac{\partial f}{\partial x} \) 在 \( x_{k-1} \) 处的值。这是一个16x16的矩阵（假设状态维度是16）。
嗯，手动推导16维的雅可比矩阵，确实有点痛苦。但我们可以分块处理：
- 位置对位置、速度的偏导：这部分是线性的，很简单。
- 速度对加速度零偏的偏导：也是线性的。
- 四元数对陀螺零偏的偏导：这部分是非线性的，需要用到四元数微积分。
我给大家一个简化版的代码示例，只展示位置和速度部分的雅可比：
def compute_jacobian_F(x, u, dt):
# 假设状态只有位置和速度 [px, py, pz, vx, vy, vz]
F = np.eye(6)
F[0:3, 3:6] = np.eye(3) * dt # 位置对速度的偏导
# 速度对加速度零偏的偏导（假设加速度零偏在状态中）
# 这部分需要根据实际状态定义来写
return F观测方程的雅可比 \( H_k = \frac{\partial h}{\partial x} \) 就简单多了。因为GPS观测直接取位置和速度，所以 \( H \) 就是一个选择矩阵：
def compute_jacobian_H(x):
H = np.zeros((6, 16))
H[0:6, 0:6] = np.eye(6) # 只观测位置和速度
return H你想想看，有了这两个雅可比矩阵，EKF的预测和更新步骤就和标准KF一模一样了。只是把 \( F \) 和 \( H \) 换成了雅可比矩阵。
核心要点：EKF的预测步骤用非线性函数 \( f \) 传播状态，用雅可比矩阵 \( F \) 传播协方差。更新步骤用非线性函数 \( h \) 计算预测观测，用雅可比矩阵 \( H \) 计算卡尔曼增益。
4.3 EKF在GPS/IMU融合中的应用
好了，理论讲完了，我们来看看实际怎么用。GPS/IMU融合是EKF最经典的应用场景之一。
为什么需要融合？GPS更新频率低（10Hz左右），而且容易受遮挡影响。IMU更新频率高（100Hz以上），但会随时间漂移。两者互补，天作之合。
EKF在这里扮演的角色，就是用IMU做高频预测，用GPS做低频修正。
具体流程是这样的：
- 初始化：用第一帧GPS数据初始化位置和速度。IMU零偏可以设为零，协方差设大一些。
- 预测步骤（IMU驱动）：每来一帧IMU数据，就用状态方程 \( f \) 预测状态，用雅可比 \( F \) 更新协方差。这一步频率很高。
- 更新步骤（GPS驱动）：每来一帧GPS数据，就用观测方程 \( h \) 计算预测观测，用雅可比 \( H \) 计算卡尔曼增益，然后更新状态和协方差。
我建议你在实现时，注意时间同步。IMU和GPS的时间戳可能不一致，需要插值或外推。我曾经因为时间戳没对齐，导致融合结果出现几米的偏差，排查了好久才发现。
实战技巧：在GPS信号丢失时（比如进入隧道），可以暂时只用IMU做纯预测。但要注意，协方差会随着时间增大，表示不确定性增加。等GPS信号恢复后，EKF会自动修正。这就是EKF的鲁棒性所在。
下面是一个简化的EKF融合伪代码：
class EKF_INS_GPS:
def __init__(self):
self.x = None # 状态向量
self.P = None # 协方差矩阵
self.Q = None # 过程噪声
self.R = None # 观测噪声
def predict(self, imu_data, dt):
# 1. 用非线性函数 f 预测状态
self.x = predict_state(self.x, imu_data, dt)
# 2. 计算雅可比矩阵 F
F = compute_jacobian_F(self.x, imu_data, dt)
# 3. 更新协方差
self.P = F @ self.P @ F.T + self.Q
def update(self, gps_data):
# 1. 计算雅可比矩阵 H
H = compute_jacobian_H(self.x)
# 2. 计算预测观测
z_pred = observe_gps(self.x)
# 3. 计算卡尔曼增益
S = H @ self.P @ H.T + self.R
K = self.P @ H.T @ np.linalg.inv(S)
# 4. 更新状态和协方差
y = gps_data - z_pred # 残差
self.x = self.x + K @ y
self.P = (np.eye(len(self.x)) - K @ H) @ self.P注意事项：EKF的线性化是在当前估计点进行的。如果初始误差太大，或者系统非线性太强，线性化误差会很大，导致滤波器发散。这时候可以考虑使用无迹卡尔曼滤波（UKF）或粒子滤波（PF）。
4.4 本章小结：一张图看懂EKF
说了这么多，我们来画个图总结一下。下面这张SVG图展示了EKF在GPS/IMU融合中的完整流程：
从这张图你可以看到，EKF的核心就是预测-更新-线性化这个循环。IMU驱动预测，GPS驱动更新，雅可比矩阵把非线性问题拉回线性框架。说白了，EKF就是用线性化的近似，去逼近真实世界的非线性。
好了，这一章的内容就到这里。记住，EKF不是万能的，但在GPS/IMU融合这个场景下，它足够好用。下一章我们会聊聊更高级的滤波方法，但EKF绝对是你的基本功。