决策算法在跟踪中的应用 - 第4节

一、非线性系统的挑战：为什么卡尔曼滤波会失效？

标准卡尔曼滤波（KF）有个前提——系统必须是线性的。说白了，就是状态转移和观测方程都必须是线性函数。但现实世界哪有那么理想？

我刚开始做目标跟踪时，遇到一个雷达跟踪飞机的项目。飞机做匀速直线运动时，KF表现很好。可一旦飞机转弯，滤波器就开始发散。当时我百思不得其解，后来才明白：转弯运动是非线性的，KF的线性假设被打破了。

非线性系统带来的问题主要有三个：

• 高斯分布不再保持：高斯分布经过非线性变换后，不再是高斯分布。而KF的核心假设就是状态服从高斯分布。
• 均值传播困难：非线性函数作用在随机变量上，均值的计算变得复杂。你没法简单地把均值代入函数就完事。
• 协方差传播失真：协方差的传播更是麻烦，非线性变换会扭曲协方差的形状。

举个例子，假设状态是位置和速度，观测是距离和角度。距离和位置的关系是平方和开根号，这是非线性函数。你用KF硬算，结果就是：预测的协方差椭圆被扭曲，实际误差远大于理论值。

二、一阶泰勒展开线性化：EKF的核心思想

EKF的思路其实很朴素——既然非线性问题难处理，那就把它近似成线性问题。怎么近似？用泰勒展开。

具体做法是：在状态估计值附近，对非线性函数做一阶泰勒展开，忽略高阶项。这样就把非线性函数变成了线性函数，然后就可以套用KF的框架了。

数学上，对于非线性函数 f(x)，在 x̂ 处展开：

f(x) ≈ f(x̂) + F · (x - x̂)

其中 F 是 f 对 x 的雅可比矩阵。说白了，就是用切线来近似曲线。

我个人的习惯是，在写代码前先手算一遍雅可比矩阵。虽然现在有自动微分工具，但手算能帮你理解每个状态变量对观测的影响。有一次我在项目中偷懒用了数值微分，结果雅可比矩阵算错了，滤波器直接发散。从那以后，我都是手算验证一遍。

EKF的完整流程如下：

1. 预测步：用非线性状态方程预测状态，用线性化的雅可比矩阵传播协方差
2. 更新步：用非线性观测方程计算预测观测，用线性化的雅可比矩阵计算卡尔曼增益

你看，本质上还是KF的那套流程，只是多了个线性化的步骤。

三、EKF在目标跟踪中的应用：一个具体例子

假设我们要跟踪一个做匀速转弯运动的车辆。状态向量包括位置 (x, y)、速度 v、航向角 θ 和角速度 ω：

x = [x, y, v, θ, ω]^T

状态方程是非线性的：

x_{k+1} = x_k + (v_k/ω_k) * [sin(θ_k + ω_k·Δt) - sin(θ_k)]
y_{k+1} = y_k + (v_k/ω_k) * [cos(θ_k) - cos(θ_k + ω_k·Δt)]
v_{k+1} = v_k
θ_{k+1} = θ_k + ω_k·Δt
ω_{k+1} = ω_k

观测方程也是非线性的（雷达测量距离和角度）：

r = sqrt(x^2 + y^2)
φ = atan2(y, x)

EKF的做法就是：对这两个非线性方程分别求雅可比矩阵，然后代入KF框架。

我记得有一次做多传感器融合，激光雷达和毫米波雷达的观测模型不同。激光雷达是笛卡尔坐标（线性），毫米波雷达是极坐标（非线性）。EKF可以自然地处理这种混合观测模型——线性观测用标准KF更新，非线性观测用EKF更新。

def ekf_update(x_pred, P_pred, z, H_jacobian, R):
    # 预测观测
    z_pred = h(x_pred)  # 非线性观测函数
    
    # 计算卡尔曼增益
    S = H_jacobian @ P_pred @ H_jacobian.T + R
    K = P_pred @ H_jacobian.T @ np.linalg.inv(S)
    
    # 更新状态和协方差
    x_upd = x_pred + K @ (z - z_pred)
    P_upd = (np.eye(len(x_pred)) - K @ H_jacobian) @ P_pred
    
    return x_upd, P_upd

这段代码看起来简单，但实际项目中坑很多。比如雅可比矩阵 H_jacobian 必须在当前状态估计值处计算，而不是在预测值处。我曾经犯过这个错误，结果滤波器性能差了一大截。

四、EKF的局限性：不是万能的

EKF虽然应用广泛，但局限性也很明显。我总结了几点：

局限性	具体表现	实际影响
线性化误差	一阶泰勒展开忽略高阶项	强非线性下误差大，甚至发散
雅可比矩阵计算	需要解析求导，复杂系统难推导	容易出错，调试困难
高斯假设	状态仍假设为高斯分布	多模态分布无法处理
初始化敏感	初始状态误差大时容易发散	需要良好的初始估计

嗯，这里要重点说一下线性化误差。你想想看，泰勒展开只在展开点附近有效。如果系统非线性很强，或者状态不确定性很大，那么线性化后的模型就和真实模型差得很远。我做过一个无人机姿态估计的项目，EKF在剧烈机动时直接发散，后来换成UKF才解决问题。

另外，EKF对非线性强度很敏感。怎么判断非线性强不强？一个简单的方法是：计算非线性函数在估计点附近的曲率。曲率越大，线性化误差越大。如果曲率很大，建议考虑UKF或粒子滤波。

最后说一句，EKF虽然不完美，但它是理解更高级滤波器的基石。我建议初学者先把EKF吃透，再学UKF、PF等。毕竟，连线性化都搞不明白，怎么理解无迹变换和重要性采样？