从KF到EKF：非线性系统问题、线性化思想、雅可比矩阵的作用与计算

标准卡尔曼滤波（KF）自20世纪60年代提出以来，在线性高斯系统中展现了卓越的估计性能。然而，现实世界的工程问题几乎都是非线性的：无人机姿态解算中的欧拉角微分方程、GPS/INS组合导航中的三角函数观测模型、目标跟踪中的极坐标到直角坐标转换、以及SLAM中的机器人运动模型与激光雷达观测模型，无一例外地打破了KF的线性假设。如果强行将非线性系统套入KF框架，会导致状态预测与量测更新产生系统性偏差，甚至引发滤波器发散。本章将深入剖析非线性系统带来的本质困难，阐述EKF的核心思想——局部线性化，并重点讲解雅可比矩阵（Jacobian Matrix）在EKF中的关键作用与工程计算方法。

上图展示了EKF线性化处理的核心链路。与KF直接使用状态转移矩阵A和观测矩阵H不同，EKF需要针对非线性函数f(·)和h(·)在特定工作点计算雅可比矩阵。这个工作点不是固定的，而是随着滤波递推不断更新——预测步围绕上一时刻的最优估计值x̂ₖ₋₁展开，更新步则围绕当前时刻的预测值x̂ₖ₋展开。这种“在线线性化”机制使得EKF能够适应系统状态的变化，但也带来了额外的计算负担和线性化误差。

3.1 非线性系统的本质困难与KF失效机理

要理解EKF的必要性，首先需要明确非线性系统对KF构成的三个层面的挑战。KF的推导基于两个核心假设：一是系统状态转移和观测均为线性变换，二是过程噪声与量测噪声均为高斯分布。当系统变为非线性时，即使噪声仍然服从高斯分布，经过非线性函数传播后的状态分布也不再是高斯分布。具体而言，非线性函数会扭曲概率密度函数的形状，使其产生偏斜、多峰或厚尾现象，而KF仅用均值和协方差来描述状态分布，这种参数化方式在强非线性场景下会产生严重的信息损失。

从数学推导角度看，KF的递推公式依赖于线性代数中的矩阵运算。当状态方程变为xₖ = f(xₖ₋₁, uₖ) + wₖ时，我们无法直接得到状态均值和协方差的解析递推式。例如，对于一维非线性函数y = x²，若x服从均值为μ、方差为σ²的高斯分布，则y的期望为μ² + σ²，而不仅仅是f(μ) = μ²。这意味着“将均值代入非线性函数”的做法会引入系统性偏差，且偏差大小与状态的不确定性σ²直接相关。在强非线性场景下，这种偏差会随着递推步数累积，最终导致滤波器输出严重偏离真实状态。

从工程实践角度观察，非线性系统的失效模式通常表现为两种：一是滤波结果出现明显偏置，即估计值与真值之间存在稳定的偏差，但协方差却收敛到很小的值，这会让用户对滤波精度产生错误信心；二是滤波器完全发散，协方差矩阵失去正定性或数值爆炸，状态估计彻底失效。前者更为隐蔽，往往在系统调试阶段难以发现，直到与高精度参考系统对比时才暴露问题。

3.2 线性化思想：泰勒展开与局部近似

EKF的线性化思想源于微积分中的泰勒级数展开。对于非线性函数f(x)，在某个工作点x₀处进行一阶泰勒展开：f(x) ≈ f(x₀) + J_f(x₀)·(x - x₀)，其中J_f(x₀)是f在x₀处的雅可比矩阵。这个近似的本质是用切线（或切平面）替代原函数在x₀附近的局部行为。EKF的核心假设是：如果滤波器的估计误差保持在较小范围内，那么围绕当前最优估计的线性化是足够精确的。

具体到EKF的递推流程，线性化发生在两个位置。第一处是状态预测阶段，围绕上一时刻的最优估计x̂ₖ₋₁对f(·)线性化，得到状态转移矩阵Fₖ₋₁；第二处是观测更新阶段，围绕当前时刻的状态预测值x̂ₖ₋对h(·)线性化，得到观测矩阵Hₖ。值得注意的是，这两个线性化点并不相同，且都随着滤波递推不断变化，因此EKF的“系统矩阵”是时变的，需要在每个递推周期重新计算。

线性化误差的控制是EKF工程应用的关键。一阶泰勒展开忽略了二阶及以上高阶项，当系统非线性强或估计误差较大时，这些被忽略的高阶项会显著影响滤波精度。工程上常用的缓解手段包括：提高滤波更新频率以减小步长、采用迭代EKF（IEKF）在更新步内多次线性化、或使用无迹卡尔曼滤波（UKF）等sigma点方法。但在计算资源受限的嵌入式系统中，EKF仍然是性价比最高的选择。

3.3 雅可比矩阵：定义、作用与工程计算

雅可比矩阵是多元函数的偏导数矩阵，它描述了向量值函数在某点的一阶线性逼近。对于n维状态向量x ∈ ℝⁿ和m维函数向量f(x) ∈ ℝᵐ，雅可比矩阵J_f ∈ ℝᵐˣⁿ的第(i,j)元素定义为∂fᵢ/∂xⱼ。在EKF中，状态转移雅可比Fₖ₋₁ = ∂f/∂x |ₓ̂ₖ₋₁ 是n×n方阵，观测雅可比Hₖ = ∂h/∂x |ₓ̂ₖ₋ 是m×n矩阵。

雅可比矩阵在EKF中承担三个关键任务。第一，在预测步中，Fₖ₋₁将上一时刻的协方差Pₖ₋₁映射到当前时刻的预测协方差Pₖ₋，即Pₖ₋ = Fₖ₋₁Pₖ₋₁Fₖ₋₁ᵀ + Q。这个变换本质上是对状态误差的线性传播。第二，在更新步中，Hₖ将预测协方差映射到观测空间，用于计算卡尔曼增益Kₖ = Pₖ₋Hₖᵀ(HₖPₖ₋Hₖᵀ + R)⁻¹。第三，雅可比矩阵的秩和条件数可以反映系统的可观测性——如果Hₖ在某些方向上的行向量接近零，说明这些状态分量在当前观测下不可观测。

工程计算雅可比矩阵有两种常用方法：解析法和数值法。解析法需要手动推导偏导数表达式，适用于函数形式相对简单的场景。例如，对于二维状态x = [px, py]ᵀ，观测为距离r = √(px² + py²)，则观测雅可比为H = [∂r/∂px, ∂r/∂py] = [px/r, py/r]。解析法的优点是计算精度高、计算量小，但推导过程容易出错，尤其当状态维数较高或函数嵌套复杂时。数值法采用有限差分近似，即J[:,j] ≈ (f(x + εeⱼ) - f(x - εeⱼ)) / (2ε)，其中eⱼ是第j个单位向量，ε是微小扰动。数值法实现简单、通用性强，但需要仔细选择扰动步长ε——过大会引入截断误差，过小则可能因浮点精度导致数值噪声。

// 数值法计算雅可比矩阵示例（C语言风格伪代码）
void computeJacobian(double (*f)(double*, int), double* x, int n, double** J) {
    double eps = 1e-6;
    double x_plus[n], x_minus[n];
    double f_plus[n], f_minus[n];
    for (int j = 0; j < n; j++) {
        for (int i = 0; i < n; i++) {
            x_plus[i] = x[i];
            x_minus[i] = x[i];
        }
        x_plus[j] += eps;
        x_minus[j] -= eps;
        f_plus[j] = f(x_plus, n);
        f_minus[j] = f(x_minus, n);
        for (int i = 0; i < n; i++) {
            J[i][j] = (f_plus[i] - f_minus[i]) / (2 * eps);
        }
    }
}

在实际工程中，推荐优先使用解析法，并辅以数值法进行交叉验证。对于复杂系统，可以先用数值法快速验证解析推导的正确性，再在正式代码中采用解析表达式以提高运行效率。此外，许多现代工具如MATLAB的Symbolic Math Toolbox、Python的SymPy库可以自动推导雅可比矩阵，大幅降低人工推导的出错概率。

3.4 案例拆解：GPS/INS组合导航中的EKF线性化

以车载GPS/INS组合导航系统为例，状态向量通常包含位置（经纬度或东北天坐标）、速度、姿态角以及传感器偏差。为简化说明，考虑一个二维平面运动模型，状态向量x = [x, y, vₓ, vᵧ]ᵀ，其中(x,y)为位置，(vₓ,vᵧ)为速度。系统采用常速度模型，但实际运动存在转弯，因此状态转移函数为：

xₖ = xₖ₋₁ + vₓₖ₋₁·Δt + 0.5·aₓ·Δt²
yₖ = yₖ₋₁ + vᵧₖ₋₁·Δt + 0.5·aᵧ·Δt²
vₓₖ = vₓₖ₋₁ + aₓ·Δt
vᵧₖ = vᵧₖ₋₁ + aᵧ·Δt

其中aₓ和aᵧ是未知的加速度扰动，被建模为过程噪声。这个模型实际上是线性的，但如果我们引入航向角θ作为状态，则运动模型变为：xₖ = xₖ₋₁ + v·cos(θₖ₋₁)·Δt，yₖ = yₖ₋₁ + v·sin(θₖ₋₁)·Δt，此时状态转移对θ的偏导数就涉及三角函数，系统变为非线性。

GPS观测提供位置信息，观测方程为z = [√(x² + y²), atan2(y, x)]ᵀ，即距离和方位角。这个观测模型是典型的非线性函数。在EKF中，观测雅可比Hₖ的计算如下：

Hₖ = [ ∂r/∂x, ∂r/∂y, ∂r/∂vₓ, ∂r/∂vᵧ ]
     [ ∂θ/∂x, ∂θ/∂y, ∂θ/∂vₓ, ∂θ/∂vᵧ ]

其中：
∂r/∂x = x / √(x² + y²)
∂r/∂y = y / √(x² + y²)
∂θ/∂x = -y / (x² + y²)
∂θ/∂y = x / (x² + y²)
其余项为0

在工程实现中，我们首先用GPS的原始测量值（伪距或载波相位）计算位置，然后将其转换为距离和方位角作为EKF的观测输入。在每次更新步中，需要根据当前预测状态x̂ₖ₋重新计算Hₖ矩阵。如果车辆位于GPS接收机正北方向（即x≈0, y>0），则∂θ/∂x = -y/(x²+y²)会变得很大，这意味着方位角观测对x方向的位置误差非常敏感，滤波器会给予该观测较大的权重来修正x方向的误差。

实际调试中我们发现，当车辆经过GPS信号遮挡区域时，观测噪声R会增大，此时EKF会更多地依赖INS预测。但如果INS的航向角误差较大，线性化点偏离真实状态较远，雅可比矩阵的近似精度下降，可能导致滤波器在信号恢复后出现短暂的发散。解决方法是采用自适应噪声调节或故障检测与隔离（FDI）机制，在观测质量差时降低其对状态更新的影响。

3.5 常见误区与总结复盘

在EKF的学习和工程应用中，以下误区最为常见。第一个误区是混淆线性化点。有些初学者在计算Fₖ和Hₖ时，错误地使用同一个状态值，或者使用初始状态而非当前最优估计。正确的做法是：Fₖ₋₁必须围绕x̂ₖ₋₁计算，Hₖ必须围绕x̂ₖ₋计算，两者不可混用。第二个误区是忽略雅可比矩阵的时变性。在KF中，A和H是常数矩阵，可以预先计算；但在EKF中，每个递推周期都需要重新计算雅可比矩阵，如果错误地将其视为常数，滤波器会逐渐失效。

第三个误区是过度依赖数值法计算雅可比。虽然数值法实现简单，但在状态维数较高时，计算量呈线性增长，且扰动步长ε的选择需要经验。对于嵌入式实时系统，解析法配合代码生成工具是更优选择。第四个误区是忽视线性化误差的累积效应。即使每个递推周期的线性化误差很小，长期运行后这些误差也可能累积，导致滤波器性能下降。工程上常采用定期重置或引入渐消因子（fading factor）来抑制误差累积。

总结本章核心要点：EKF的本质是“在最优估计点附近用一阶泰勒展开近似非线性函数”，雅可比矩阵是这一线性化过程的数学载体。Fₖ和Hₖ的计算精度直接影响协方差传播和卡尔曼增益的合理性。在实际工程中，应根据系统非线性强度、计算资源、实时性要求等因素，在EKF、UKF和粒子滤波之间做出合理选择。对于大多数中等非线性程度的工程问题，EKF凭借其计算效率高、实现成熟、理论完备等优势，仍然是首选的滤波方案。

最后需要强调的是，EKF的线性化思想虽然朴素，但其工程实现涉及大量细节。从雅可比矩阵的推导、线性化点的选择、数值稳定性维护到噪声参数的整定，每一个环节都可能成为系统性能的瓶颈。建议读者在掌握本章理论的基础上，通过仿真平台（如MATLAB/Simulink或Python）搭建完整的EKF模型，针对不同非线性强度场景进行对比实验，深入体会线性化误差对滤波性能的影响规律。