4、扩展卡尔曼滤波(EKF)：非线性系统、泰勒展开线性化、EKF算法流程、Python实现
各位同学，欢迎来到第四章。前面我们聊的标准卡尔曼滤波，说白了就是处理线性系统的神器。但现实世界哪有那么多线性关系？你想想看，机器人转弯、无人机姿态变化、传感器观测距离和角度——这些全是非线性。我当年第一次把标准KF用在无人机姿态估计上，结果滤波直接发散，飞机差点炸了。嗯，从那以后我就老老实实学EKF了。
4.1 为什么需要EKF？非线性系统的困境
标准卡尔曼滤波有两个核心假设：状态转移是线性的，观测也是线性的。用数学语言说就是：
x_k = A * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k但实际系统往往是这样的：
x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k这里的f和h是非线性函数。比如你用一个激光雷达测距，观测方程是距离 = sqrt(x² + y²)，这明显不是线性的。我做过一个项目，用里程计和IMU做融合，里程计的模型是带三角函数的——不处理非线性，误差会越积越大。
核心问题：高斯分布经过非线性变换后，不再是高斯分布。而卡尔曼滤波要求所有噪声都是高斯分布。怎么办？
4.2 泰勒展开：把非线性掰成线性
解决思路其实很朴素：既然非线性不好处理，那就在某个点附近把它近似成线性的。用什么近似？泰勒展开。
回忆一下高数：对于一个函数f(x)，在点a处的泰勒展开是：
f(x) ≈ f(a) + f'(a)(x - a) + 高阶项EKF只取一阶项，忽略高阶项。说白了就是用切线代替曲线。我刚开始学的时候总觉得这样近似太粗糙，后来发现只要系统不是极度非线性，一阶近似完全够用。
具体到EKF中，我们在两个地方做线性化：
- 状态预测阶段：对f(x)在上一时刻最优估计处求雅可比矩阵
- 观测更新阶段：对h(x)在当前预测值处求雅可比矩阵
雅可比矩阵是什么？就是多元函数的一阶偏导数组成的矩阵。比如f(x)有n个状态变量，m个输出，雅可比就是m×n的矩阵。
个人经验：我建议你在写代码前，先用纸笔把雅可比矩阵推导一遍。虽然现在有自动微分工具，但手推一遍能帮你理解每个状态量对系统的影响。我曾经偷懒直接用数值微分，结果在强非线性场景下滤波精度差了一个数量级。
4.3 EKF算法五步走
EKF的流程和标准KF几乎一样，只是把线性方程换成了非线性方程，并在计算协方差时用雅可比矩阵代替原来的状态转移矩阵和观测矩阵。
咱们一步步来：
步骤1：状态预测
x_pred = f(x_est_{k-1}, u_k)
P_pred = F * P_{k-1} * F^T + Q这里的F是f对x的雅可比矩阵，在x_est_{k-1}处计算。Q是过程噪声协方差，和标准KF一样。
步骤2：计算卡尔曼增益
K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}这里的H是h对x的雅可比矩阵，在x_pred处计算。R是观测噪声协方差。
步骤3：状态更新
x_est = x_pred + K * (z - h(x_pred))注意这里的残差计算用的是真实的非线性观测函数h，不是线性近似。这一点很重要——我见过有人直接用H*x_pred算预测观测，那就不对了。
步骤4：协方差更新
P_est = (I - K * H) * P_pred这一步和标准KF完全一样。
步骤5：迭代
把x_est和P_est作为下一时刻的输入，重复步骤1-4。
避坑指南：我曾经在协方差更新时用了错误的公式，导致P矩阵不再对称正定，滤波直接崩溃。建议每次更新后检查P是否对称，或者直接用对称形式更新：P = (I - KH)P(I - KH)^T + KRK^T，这个形式数值稳定性更好。
4.4 Python实现：一个简单的EKF例子
咱们用一个经典例子：雷达跟踪一个在二维平面运动的物体。状态量是位置和速度[x, y, vx, vy]，观测是距离和角度[r, theta]。
先定义非线性函数：
import numpy as np
def f_state(x, dt):
"""状态转移函数：匀速运动模型"""
F = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
return F @ x
def h_obs(x):
"""观测函数：距离和角度"""
px, py, vx, vy = x
r = np.sqrt(px**2 + py**2)
theta = np.arctan2(py, px)
return np.array([r, theta])然后计算雅可比矩阵：
def jacobian_f(x, dt):
"""状态转移雅可比：匀速模型就是F本身"""
return np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
def jacobian_h(x):
"""观测雅可比：对位置求偏导"""
px, py, vx, vy = x
r = np.sqrt(px**2 + py**2)
if r < 1e-6:
return np.zeros((2, 4))
H = np.array([
[px/r, py/r, 0, 0],
[-py/(r**2), px/(r**2), 0, 0]
])
return H最后是EKF主循环：
def ekf_predict(x, P, Q, dt):
F = jacobian_f(x, dt)
x_pred = f_state(x, dt)
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def ekf_update(x_pred, P_pred, z, R):
H = jacobian_h(x_pred)
z_pred = h_obs(x_pred)
y = z - z_pred # 残差
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
x_est = x_pred + K @ y
P_est = (np.eye(4) - K @ H) @ P_pred
return x_est, P_est调试技巧：我建议你在仿真中先跑一遍，把残差y画出来。如果残差均值不为零，说明模型有偏差或者线性化误差太大。这时候可以考虑用迭代EKF（IEKF），在更新步骤中多次线性化。
4.5 EKF的局限与改进方向
EKF虽然好用，但有几个硬伤：
- 一阶近似精度有限：对于强非线性系统，线性化误差会导致滤波发散。我做过一个四旋翼的强机动飞行实验，EKF直接跟丢了。
- 雅可比计算麻烦：对于复杂系统，手推雅可比容易出错。数值微分又不够精确。
- 高斯假设：如果噪声不是高斯分布，EKF效果会打折扣。
改进方案有哪些？
- UKF（无迹卡尔曼滤波）：用sigma点传播，不需要计算雅可比，精度更高。
- IEKF（迭代EKF）：在更新步骤中多次线性化，收敛性更好。
- 粒子滤波：完全抛弃高斯假设，用粒子近似任意分布。
但话说回来，EKF仍然是工业界最常用的非线性滤波方法。为什么？因为它计算量小、实现简单、在大多数场景下够用。我个人的建议是：先上EKF，如果效果不行再考虑更复杂的方法。
4.6 本章小结
咱们这一章聊了EKF的核心思想：用泰勒展开把非线性系统掰成线性，然后套用标准KF的框架。关键点有三个：
- 非线性函数f和h要能写出来，并且可微
- 雅可比矩阵要算对，这是EKF的命门
- 协方差更新要注意数值稳定性
下一章咱们会讲UKF，它不需要算雅可比，用起来更省心。但EKF的基础打牢了，后面学什么都快。嗯，今天就到这儿，大家回去把代码跑一遍，有问题随时交流。