多传感器融合与运动状态估计 - 第4节
一、为什么需要扩展卡尔曼滤波？
做自动驾驶定位的朋友都知道，现实世界没有完美的线性系统。
标准的卡尔曼滤波，说白了只能处理线性问题。比如速度乘以时间等于位移，这种关系很直白。但我们的车辆运动呢？航向角变化、转弯半径、加速度非线性...这些都不是简单的线性关系能描述的。
我记得刚入行那会儿，天真地以为直接用标准KF就能搞定定位。结果呢？车辆一转弯，估计位置直接飘到马路牙子上去了。嗯，那次教训挺深刻的。
所以就有了扩展卡尔曼滤波——EKF。它的核心思想其实很朴素：把非线性问题，在局部近似成线性问题来处理。
一句话总结：EKF = 标准KF + 线性化近似。线性化的工具就是雅可比矩阵。
二、非线性系统长什么样？
我们先看看自动驾驶中典型的非线性系统。
2.1 状态方程的非线性
车辆的运动模型，通常写成这样：
x_k = f(x_{k-1}, u_k) + w_k这里的f()不是矩阵乘法，而是一个非线性函数。比如CTRV（恒定转率和速度）模型：
状态向量: [x, y, v, ψ, ψ_dot]
其中:
x, y = 位置
v = 速度
ψ = 航向角
ψ_dot = 角速度
预测方程:
x_k = x_{k-1} + (v/ψ_dot) * [sin(ψ + ψ_dot*Δt) - sin(ψ)]
y_k = y_{k-1} + (v/ψ_dot) * [-cos(ψ + ψ_dot*Δt) + cos(ψ)]
ψ_k = ψ_{k-1} + ψ_dot * Δt
...你看，这里面有sin、cos，还有除法。标准KF的线性假设完全失效。
2.2 观测方程的非线性
传感器观测也是非线性的。比如毫米波雷达测量目标：
观测向量: [ρ, θ, ρ_dot]
其中:
ρ = 距离
θ = 方位角
ρ_dot = 距离变化率
观测方程:
ρ = sqrt(px² + py²)
θ = atan2(py, px)
ρ_dot = (px*vx + py*vy) / ρ又是开方、又是反正切。这些操作在标准KF里根本没法直接处理。
我的经验：在实际项目中，80%的传感器融合问题都涉及非线性。如果你发现标准KF结果不对劲，先检查一下模型是不是线性的。
三、雅可比矩阵：线性化的核心工具
EKF怎么处理非线性？答案就是泰勒展开。
我们把非线性函数f(x)在某个点附近展开，只取一阶项：
f(x) ≈ f(x₀) + J * (x - x₀)这里的J就是雅可比矩阵。它本质上是一个偏导数矩阵。
3.1 雅可比矩阵长什么样？
假设状态向量有n维，观测向量有m维：
雅可比矩阵 J 的形状是 m × n
J[i][j] = ∂f_i / ∂x_j
也就是第i个输出对第j个输入的偏导数举个例子。对于上面提到的雷达观测方程：
观测函数 h(x):
ρ = sqrt(px² + py²)
θ = atan2(py, px)
ρ_dot = (px*vx + py*vy) / ρ
雅可比矩阵 H (3×4):
H[0][0] = ∂ρ/∂px = px / ρ
H[0][1] = ∂ρ/∂py = py / ρ
H[0][2] = ∂ρ/∂vx = 0
H[0][3] = ∂ρ/∂vy = 0
H[1][0] = ∂θ/∂px = -py / ρ²
H[1][1] = ∂θ/∂py = px / ρ²
H[1][2] = ∂θ/∂vx = 0
H[1][3] = ∂θ/∂vy = 0
H[2][0] = ∂ρ_dot/∂px = (vx*ρ² - px*(px*vx+py*vy)) / ρ³
...（后面略）注意：雅可比矩阵必须在当前状态估计值处计算。每次预测和更新时都要重新算，不能复用上一次的值。我曾经因为这个bug排查了整整两天...
3.2 代码实现示例
下面是一个简化的EKF预测步骤实现：
def predict_ekf(x, P, u, dt):
"""
EKF预测步骤
x: 当前状态 (4x1)
P: 协方差矩阵 (4x4)
u: 控制输入
dt: 时间间隔
"""
# 1. 状态预测（非线性）
x_pred = motion_model(x, u, dt)
# 2. 计算雅可比矩阵 F
F = compute_jacobian_F(x, u, dt)
# 3. 协方差预测（线性化后）
Q = process_noise_matrix(dt)
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def compute_jacobian_F(x, u, dt):
"""
CTRV模型的雅可比矩阵
"""
v = x[2] # 速度
psi = x[3] # 航向角
psi_dot = x[4] # 角速度
F = np.eye(5)
if abs(psi_dot) > 0.001:
# 非零角速度情况
F[0, 2] = (np.sin(psi + psi_dot*dt) - np.sin(psi)) / psi_dot
F[0, 3] = (v/psi_dot) * (np.cos(psi + psi_dot*dt) - np.cos(psi))
# ... 其他偏导数
else:
# 近似直线运动
F[0, 2] = dt * np.cos(psi)
F[0, 3] = -v * dt * np.sin(psi)
return F四、EKF在定位中的完整流程
搞清楚了雅可比矩阵，EKF的流程其实和标准KF很像，只是多了线性化这一步。
4.1 核心步骤
- 初始化：设定初始状态x₀和协方差P₀
- 预测：用非线性模型预测状态，用雅可比矩阵传播协方差
- 更新：计算观测雅可比矩阵，计算卡尔曼增益，更新状态和协方差
- 重复：回到步骤2
4.2 更新步骤详解
def update_ekf(x_pred, P_pred, z, R):
"""
EKF更新步骤
z: 实际观测值
R: 观测噪声协方差
"""
# 1. 计算观测雅可比矩阵 H
H = compute_jacobian_H(x_pred)
# 2. 预测观测值（非线性）
z_pred = observation_model(x_pred)
# 3. 计算卡尔曼增益
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
# 4. 更新状态
y = z - z_pred # 残差
x_updated = x_pred + K @ y
# 5. 更新协方差
I = np.eye(len(x_pred))
P_updated = (I - K @ H) @ P_pred
return x_updated, P_updated关键点：EKF的更新步骤中，残差y的计算用的是真实观测值减去预测观测值，而不是减去H*x_pred。因为观测模型是非线性的，直接用H*x_pred会引入误差。
五、EKF的局限性
说实话，EKF虽然好用，但也不是万能的。我在项目中踩过不少坑：
- 线性化误差：泰勒展开只取一阶项，如果非线性程度很高，误差会累积
- 雅可比计算复杂：对于高维系统，手动推导偏导数容易出错
- 对初始值敏感：如果初始状态估计偏差太大，线性化点选得不好，滤波器可能发散
避坑指南：我曾经在一个项目中，因为车辆急转弯导致航向角变化剧烈，EKF直接炸了。后来加了角度归一化处理，并且在角速度大的时候降低预测步长，才稳定下来。
六、本章小结
EKF是处理非线性系统的经典方法。它的核心就两件事：
- 用雅可比矩阵把非线性模型线性化
- 在标准KF框架下完成预测和更新
虽然现在有UKF、粒子滤波等更先进的方法，但EKF凭借计算量小、实现简单的优势，仍然是工业界的主流选择。我个人的建议是：先掌握EKF，再根据实际需求考虑更复杂的滤波器。
嗯，这一章的内容就到这里。记住，纸上得来终觉浅，建议你动手写一个完整的EKF定位程序，跑一跑真实数据，感受一下非线性带来的影响。