第4章 扩展卡尔曼滤波（EKF）：非线性系统建模与目标跟踪实战
各位同学，欢迎来到第四章。说实话，卡尔曼滤波（KF）在工程界已经封神了——只要系统是线性的、噪声是高斯的，它几乎是无敌的。但现实世界哪有那么多线性系统？你想想看，雷达测距、视觉SLAM、无人机姿态估计，哪个不是非线性的？
我刚开始做目标跟踪那会儿，天真地以为套个标准KF就能搞定。结果呢？跟踪误差大得离谱，目标稍微转个弯就直接跟丢。后来我才明白——线性化才是关键。扩展卡尔曼滤波（EKF），说白了就是给KF装了个“线性化眼镜”，让它能处理非线性问题。
核心思想：EKF通过对非线性函数进行一阶泰勒展开（雅可比矩阵），将非线性系统近似为线性系统，然后套用标准KF的框架。
4.1 非线性系统建模
我们先看一个典型场景：雷达跟踪匀速转弯的目标。状态向量通常包含位置和速度：
x = [px, py, vx, vy]^T但雷达的观测是距离r和方位角θ，它们与状态的关系是非线性的：
r = sqrt(px^2 + py^2)
θ = atan2(py, px)你看，这里出现了开方和反正切。标准KF要求观测方程是线性的（即z = Hx），但这里显然不是。怎么办？
我个人的习惯是：先写出完整的非线性模型，再考虑线性化。系统模型分两部分：
- 状态方程（运动模型）：x_k = f(x_{k-1}, u_k) + w_k
- 观测方程（测量模型）：z_k = h(x_k) + v_k
其中f和h都是非线性函数，w_k和v_k是高斯噪声。嗯，这里要注意：噪声假设仍然是高斯的，只是函数关系是非线性的。
避坑指南：我曾经在项目中把非线性系统直接套用标准KF，结果滤波器发散。后来发现，当非线性程度较强时（比如目标做急转弯），一阶线性化可能不够，得考虑二阶EKF或无迹卡尔曼滤波（UKF）。
4.2 EKF线性化过程
EKF的核心就两步：预测和更新。但每一步都需要计算雅可比矩阵。
4.2.1 预测步骤
首先，用非线性函数f预测状态：
x_pred = f(x_est, u)然后，计算状态转移矩阵F（即f对x的雅可比矩阵）：
F = ∂f/∂x | x = x_est协方差预测：
P_pred = F * P_est * F^T + Q这里Q是过程噪声协方差。我刚开始学的时候总搞混：F是时变的，每个时刻都要重新计算，不像标准KF里F是常数矩阵。
4.2.2 更新步骤
计算观测矩阵H（即h对x的雅可比矩阵）：
H = ∂h/∂x | x = x_pred然后就是标准KF的更新公式：
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_est = x_pred + K * (z - h(x_pred))
P_est = (I - K * H) * P_pred你看，除了F和H需要实时计算，其他步骤和标准KF一模一样。说白了，EKF就是用雅可比矩阵把非线性问题“掰弯”成线性问题。
重要提醒：雅可比矩阵的计算一定要仔细！我见过太多人把偏导数算错，导致滤波器性能极差。建议先用符号计算验证，再写代码。
4.3 EKF在目标跟踪中的应用
我们用一个完整的例子来说明。假设你要跟踪一个做匀速圆周运动的目标，状态向量为：
x = [px, py, vx, vy]^T运动模型（匀速转弯，CTRV模型）：
px_k = px_{k-1} + vx_{k-1} * dt
py_k = py_{k-1} + vy_{k-1} * dt
vx_k = vx_{k-1} * cos(ω*dt) - vy_{k-1} * sin(ω*dt)
vy_k = vx_{k-1} * sin(ω*dt) + vy_{k-1} * cos(ω*dt)其中ω是转弯角速度。观测模型（雷达）：
r = sqrt(px^2 + py^2)
θ = atan2(py, px)下面给出EKF的核心代码（Python风格）：
import numpy as np
def ekf_predict(x, P, F, Q):
# 状态预测
x_pred = f(x) # 非线性函数
# 协方差预测
P_pred = F @ P @ F.T + Q
return x_pred, P_pred
def ekf_update(x_pred, P_pred, z, H, R):
# 卡尔曼增益
S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
# 状态更新
z_pred = h(x_pred) # 非线性观测
x_est = x_pred + K @ (z - z_pred)
# 协方差更新
P_est = (np.eye(len(x_pred)) - K @ H) @ P_pred
return x_est, P_est
# 计算雅可比矩阵（以观测模型为例）
def compute_H(x):
px, py = x[0], x[1]
r = np.sqrt(px**2 + py**2)
H = np.array([
[px/r, py/r, 0, 0],
[-py/(r**2), px/(r**2), 0, 0]
])
return H实战经验：我在做无人机跟踪项目时，发现EKF对初始状态非常敏感。如果初始位置偏差太大，线性化误差会累积，导致滤波器发散。我的建议是：先用几帧数据做初始化，或者用多假设方法。
4.4 知识体系与核心逻辑
下面这张图总结了EKF在目标跟踪中的完整流程：
4.5 常见问题与调试技巧
最后，分享几个我在项目中踩过的坑：
- 雅可比矩阵计算错误：这是最常见的错误。我建议用数值微分验证解析结果，比如用中心差分法。
- 噪声协方差Q和R的调参：Q太大，滤波器会震荡；Q太小，会滞后。我一般用试错法，先设大Q，再逐步减小。
- 非线性程度判断：如果目标运动剧烈，一阶EKF可能不够。这时候可以考虑迭代EKF（IEKF）或UKF。
个人经验：我曾经在车载雷达项目中，因为EKF的雅可比矩阵算错了一个符号，导致跟踪结果完全相反。排查了整整两天才发现。所以，写代码时一定要单元测试雅可比矩阵！
好了，这一章的内容就到这里。EKF虽然经典，但也不是万能的。下一章我们会聊聊更强大的无迹卡尔曼滤波（UKF），它不需要计算雅可比矩阵，处理非线性问题更鲁棒。但在此之前，建议你把EKF的代码亲手写一遍，感受一下线性化的威力。