3. 从线性到非线性：为什么需要EKF、非线性系统的挑战、线性化思想（泰勒展开）

在上一章中，我们详细推导了标准卡尔曼滤波（KF）的五个核心方程，并验证了它在线性高斯系统下的最优性。然而，Edge AI 传感器融合的真实场景几乎从来不是线性的：IMU 的姿态更新涉及三角函数，GPS 的经纬度到平面坐标的投影涉及曲率修正，视觉里程计的特征点深度估计涉及除法与透视变换。当系统模型或观测模型呈现出非线性特征时，标准 KF 的线性假设被打破，其最优性也随之失效。本章将深入剖析非线性系统带来的本质挑战，并引出扩展卡尔曼滤波（EKF）的核心思想——通过泰勒展开对非线性函数进行局部线性化，从而在保留 KF 框架的前提下，实现对非线性系统的近似最优估计。

3.1 非线性系统的本质挑战：高斯分布不再“幸运”

3.1.1 核心原理：线性变换与非线性变换的本质区别

标准卡尔曼滤波之所以优雅，在于它利用了线性代数的一个关键性质：高斯分布经过线性变换后仍然是高斯分布。设状态 x ~ N(μ, Σ)，经过线性映射 y = Ax + b，则 y ~ N(Aμ + b, AΣAᵀ)。这意味着，在预测步和更新步中，我们只需要传递均值和协方差，就能完整描述整个状态分布。

然而，当状态转移函数 f(x) 或观测函数 h(x) 是非线性函数时，情况发生了根本变化。一个高斯分布经过非线性变换后，其输出分布通常不再是高斯分布。例如，考虑一维随机变量 x ~ N(0, 1)，经过非线性变换 y = x²，输出 y 服从卡方分布（自由度为1），其均值变为1，方差变为2，且分布明显偏态。此时，仅用均值和协方差无法完整刻画分布形态，而卡尔曼滤波的更新公式恰恰假设了高斯性。

3.1.2 非线性系统的具体挑战

在 Edge AI 传感器融合中，非线性来源无处不在。下表总结了常见场景及其非线性特征：

这些非线性带来的直接后果是：即使过程噪声和观测噪声都是高斯白噪声，状态的后验分布也不再是高斯分布。如果我们强行用高斯分布去近似，就会产生误差，且这种误差在强非线性场景下会迅速累积，导致滤波器发散。

3.1.3 案例拆解：无人机高度估计中的非线性

考虑一个 Edge AI 无人机高度估计场景。无人机搭载气压计和超声波传感器，融合估计离地高度 h。超声波传感器的观测模型为：

z_ultrasonic = h + v,  v ~ N(0, σ²(h))

其中噪声方差 σ²(h) = 0.01 + 0.05·h²，即高度越高，超声波波束扩散越严重，噪声越大。这是一个典型的异方差非线性观测模型——观测噪声方差本身是状态的函数。

如果使用标准 KF，我们假设观测噪声方差为常数 R = 0.01，那么在低空（h≈0.5m）时，实际噪声约为0.0125，误差尚可接受；但在高空（h≈5m）时，实际噪声达到1.26，是假设值的126倍。KF 会过度信任超声波观测，导致高度估计出现剧烈抖动甚至发散。

正确的做法是使用 EKF，在每次更新时根据当前估计高度 ĥ 重新计算观测噪声方差 R_k = 0.01 + 0.05·ĥ²，并计算观测函数的雅可比矩阵 H = ∂h/∂x = 1（本例中观测函数本身是线性的，但噪声方差非线性）。这个案例说明：非线性不仅存在于函数映射中，也可能存在于噪声模型中，EKF 的灵活性使其能够处理这类问题。

3.2 线性化思想：泰勒展开与雅可比矩阵

3.2.1 核心原理：一阶泰勒展开的几何意义

EKF 的核心思想非常直观：既然非线性函数在全局范围内难以处理，我们就在当前估计点附近用切线（或切平面）来近似原函数。对于多元函数 f(x)，在点 x̂ 处的一阶泰勒展开为：

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

其中 J_f(x̂) 是雅可比矩阵，其第 i 行第 j 列元素为 ∂f_i/∂x_j，在 x = x̂ 处取值。这个近似在 x̂ 附近是精确的，离 x̂ 越远，近似误差越大。

在 EKF 中，我们对状态转移函数 f 和观测函数 h 分别进行线性化：

• 预测步线性化：在上一时刻后验估计 x̂_{k-1} 处展开 f，得到状态转移矩阵 F_k = J_f(x̂_{k-1})，用于协方差预测。
• 更新步线性化：在当前预测状态 x̂_k⁻ 处展开 h，得到观测矩阵 H_k = J_h(x̂_k⁻)，用于卡尔曼增益计算和协方差更新。

3.2.2 操作流程：EKF 的完整计算步骤

EKF 的算法流程与标准 KF 高度相似，区别在于用雅可比矩阵替代了常数矩阵，且状态和观测的预测使用原始非线性函数。具体步骤如下：

1. 初始化：设定初始状态 x̂₀ 和初始协方差 P₀。
2. 预测步：
   • 状态预测：x̂_k⁻ = f(x̂_{k-1}, u_k, 0)（直接代入非线性函数）
   • 协方差预测：P_k⁻ = F_k · P_{k-1} · F_kᵀ + Q_k，其中 F_k = ∂f/∂x |_{x̂_{k-1}}
3. 更新步：
   • 计算观测预测：ẑ_k = h(x̂_k⁻, 0)
   • 计算观测雅可比：H_k = ∂h/∂x |_{x̂_k⁻}
   • 计算卡尔曼增益：K_k = P_k⁻ · H_kᵀ · (H_k · P_k⁻ · H_kᵀ + R_k)⁻¹
   • 状态更新：x̂_k = x̂_k⁻ + K_k · (z_k - ẑ_k)
   • 协方差更新：P_k = (I - K_k · H_k) · P_k⁻

3.2.3 案例拆解：二维机器人定位中的 EKF 实现

考虑一个在平面中运动的差速驱动机器人，状态向量为 x = [x, y, θ]ᵀ（位置和航向角）。控制输入为线速度 v 和角速度 ω。运动模型为：

x_{k} = x_{k-1} + v·cos(θ_{k-1})·Δt
y_{k} = y_{k-1} + v·sin(θ_{k-1})·Δt
θ_{k} = θ_{k-1} + ω·Δt

这是一个典型的非线性状态转移函数（包含三角函数）。观测模型为机器人携带的测距传感器，测量到两个已知地标的距离：

z_i = sqrt((x - x_i)² + (y - y_i)²) + v_i,  i = 1, 2

这也是非线性的（包含平方根）。

EKF 实现要点：

• 状态转移雅可比：

F = [[1, 0, -v·sin(θ)·Δt],
     [0, 1,  v·cos(θ)·Δt],
     [0, 0,  1]]

• 观测雅可比（对地标i）：

H_i = [(x - x_i)/d_i, (y - y_i)/d_i, 0],  d_i = sqrt((x-x_i)² + (y-y_i)²)

在 Edge AI 平台上（如 Cortex-M4 或树莓派），计算雅可比矩阵涉及三角函数和除法运算，需要约 20-50 个浮点运算周期。相比标准 KF 的纯矩阵乘法，EKF 的计算量增加了约 30%-50%，但在实时性要求不高的场景（如 10-50Hz 更新率）下完全可接受。

实际运行效果：在仿真中，当机器人转弯时（θ 快速变化），标准 KF 由于使用固定的线性模型，位置误差会累积到 0.8m；而 EKF 通过在每个时刻重新计算雅可比矩阵，将误差控制在 0.15m 以内。这验证了 EKF 在中等非线性程度下的有效性。

3.3 线性化的局限性与工程应对

3.3.1 核心原理：一阶近似的误差边界

泰勒展开的余项（拉格朗日余项）告诉我们，一阶近似的误差与 ‖x - x̂‖² 成正比，比例系数取决于函数的二阶导数（Hessian 矩阵）在展开点和真实点之间的最大值。这意味着：

• 状态不确定性越大，线性化误差越大。如果 P_k 的协方差较大，状态的真实值可能偏离展开点较远，一阶近似就不再准确。
• 函数非线性越强，线性化误差越大。例如 h(x) = sin(x) 在 x = 0 附近近似线性，但在 x = π/2 附近曲率很大，一阶近似误差显著。

3.3.2 常见误区与工程应对策略

3.3.3 案例复盘：EKF 在四旋翼姿态估计中的调优

背景与约束：某 Edge AI 四旋翼项目使用 MPU6050（三轴加速度计+三轴陀螺仪）和 HMC5883L 磁力计进行姿态估计。系统运行在 STM32F405（168MHz Cortex-M4）上，要求 500Hz 更新率，且 RAM 限制为 64KB。初始方案采用标准 KF，将陀螺仪积分作为线性状态转移，加速度计和磁力计作为线性观测。

初始现象：在悬停测试中，当无人机快速旋转 90° 后，姿态估计出现约 5° 的稳态误差，且伴随 2° 左右的振荡。分析发现，问题根源在于：

• 陀螺仪积分模型 θ_k = θ_{k-1} + ω·Δt 在俯仰角接近 ±90° 时，欧拉角表示出现奇异性（万向节锁），导致状态转移矩阵不准确。
• 加速度计观测模型 φ = atan2(a_y, a_z) 在非水平加速时引入非线性误差。

关键输入数据：陀螺仪零偏稳定性 0.02°/s，加速度计噪声密度 200 μg/√Hz，磁力计在室内受铁磁干扰，航向角噪声方差约为 4°²。

分析判断过程：团队决定改用 EKF，并将状态向量从欧拉角切换为四元数 q = [q0, q1, q2, q3]ᵀ。四元数避免了万向节锁问题，且其微分方程 q̇ = 0.5·Ω(ω)·q 是线性的（在给定角速度下），但观测模型（加速度计和磁力计到四元数的映射）仍然是非线性的。

实际操作步骤：

1. 将状态转移函数改为四元数积分：q_k = exp(0.5·Ω(ω)·Δt)·q_{k-1}，并计算雅可比 F_k。
2. 观测模型使用加速度计和磁力计的参考向量与预测姿态的旋转对比，计算雅可比 H_k。
3. 针对磁力计干扰，在更新步中动态调整观测噪声协方差 R：当检测到磁场异常（模长偏离 1 超过 15%）时，将磁力计对应的 R 增大 10 倍。
4. 在 STM32 上使用单精度浮点运算，将雅可比矩阵的稀疏结构预计算，减少运行时开销。

结果对比：切换 EKF 后，快速旋转后的稳态误差从 5° 降至 0.8°，振荡幅度从 2° 降至 0.3°。在 500Hz 更新率下，EKF 单次迭代耗时约 85μs（含雅可比计算），CPU 占用率约 4.3%，满足实时性要求。

风险与代价：EKF 对初始协方差 P₀ 的设定较敏感，若 P₀ 过小，滤波器可能“过于自信”而无法快速收敛；若过大，则初期估计噪声较大。团队通过多次试验确定了 P₀ = diag(0.01, 0.01, 0.01, 0.01) 的合理值。

复盘经验：在 Edge AI 传感器融合中，EKF 的工程实现难点往往不在数学推导，而在于：①选择合适的状态表示（四元数 vs 欧拉角）；②正确处理传感器异常（动态调整 R）；③在有限算力下平衡精度与实时性。这些经验对后续章节的 UKF 和粒子滤波同样适用。

3.4 总结：EKF 的适用边界与选型建议

本章从非线性系统的本质挑战出发，详细阐述了 EKF 的线性化思想及其工程实现。总结如下：