4、扩展卡尔曼滤波：非线性系统线性化、EKF算法流程、雅可比矩阵计算

各位同学，欢迎来到第四章。说实话，讲到这里，才是组合导航真正开始「硬核」的地方。

前面我们聊了标准卡尔曼滤波，那个东西很漂亮，公式简洁，性能也棒。但有个致命的前提——系统必须是线性的。你想想看，现实世界里哪有那么多线性系统？

拿我们组合导航来说，IMU输出的姿态角、地球的曲率、卫星的几何构型……哪个不是非线性的？你要是硬拿标准KF去套，结果就是滤波器发散，定位直接飘到天上去。

所以，这一章我们来解决这个问题。核心思路就一句话：把非线性系统近似成线性系统，然后再用卡尔曼滤波。这个方法，就叫扩展卡尔曼滤波（EKF）。

4.1 非线性系统线性化：泰勒展开的妙用

先问一个问题：一个非线性函数，比如 f(x) = sin(x)，怎么用线性模型去近似它？

答案其实你高中就学过——泰勒展开。在一阶近似下，我们只保留线性项，忽略高阶项。数学上就是：

f(x) ≈ f(x₀) + f'(x₀) * (x - x₀)

这个公式看着简单，但它是EKF的基石。说白了，就是在状态估计值附近，把非线性函数「掰直」了。

我在项目中遇到过一个问题：某次跑车实验，车辆在高速过弯时，EKF突然发散。排查了半天，发现是转弯半径太小，导致非线性太强，一阶泰勒展开的近似误差太大了。后来我改成了二阶EKF，才稳住。嗯，这里要提醒大家：线性化是有代价的，近似误差会直接影响滤波精度。

具体到组合导航，我们有两个地方需要线性化：

• 状态方程：描述系统状态如何随时间演化。比如位置、速度、姿态的递推。
• 观测方程：描述观测值（比如GPS伪距）与状态之间的关系。

这两个方程，在标准KF里都是线性的。但在EKF里，它们可以是任意非线性函数。我们只需要在每一步，对它们做一阶泰勒展开，得到雅可比矩阵，然后套用KF的框架。

4.2 EKF算法流程：五步走

好，理论说完了，我们来看看EKF到底怎么跑。其实流程和标准KF非常像，只是多了个「线性化」的步骤。

我个人习惯把EKF分成五个步骤，记起来很方便：

1. 状态预测：用非线性状态方程，从 k-1 时刻的状态，预测 k 时刻的状态。
2. 协方差预测：用线性化的状态转移矩阵（雅可比），预测协方差矩阵。
3. 卡尔曼增益计算：用线性化的观测矩阵（雅可比），计算增益。
4. 状态更新：用实际观测值，修正预测的状态。
5. 协方差更新：更新协方差矩阵，为下一时刻做准备。

你看，是不是和标准KF一模一样？唯一的区别就是：状态转移矩阵 F 和观测矩阵 H 不再是固定的，而是每一步都要重新计算的雅可比矩阵。

下面我用伪代码展示一下这个流程，方便你理解：

// EKF 单步迭代
// 输入：上一时刻状态 x_{k-1}，协方差 P_{k-1}，控制量 u_{k-1}，观测值 z_k

// 1. 状态预测
x_pred = f(x_{k-1}, u_{k-1})   // 非线性状态方程

// 2. 协方差预测
F = df/dx | x_{k-1}            // 计算状态转移雅可比矩阵
P_pred = F * P_{k-1} * F^T + Q

// 3. 卡尔曼增益计算
H = dh/dx | x_pred             // 计算观测雅可比矩阵
K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}

// 4. 状态更新
z_pred = h(x_pred)             // 非线性观测方程
x_k = x_pred + K * (z_k - z_pred)

// 5. 协方差更新
P_k = (I - K * H) * P_pred

// 输出：当前时刻状态 x_k，协方差 P_k

这段代码我建议你多看几遍。我曾经在调试一个组合导航系统时，发现协方差一直不收敛，最后发现是雅可比矩阵的计算顺序搞反了——应该用预测值 x_pred 去算 H，我却用了上一时刻的 x_{k-1}。这种错误，排查起来非常痛苦。

4.3 雅可比矩阵计算：实战中的关键

雅可比矩阵，说白了就是一个偏导数矩阵。它描述了系统输出对输入的敏感程度。

假设你的状态向量有 n 个维度，那么状态转移雅可比 F 就是一个 n×n 的矩阵。它的第 i 行第 j 列元素是：

F[i][j] = ∂f_i / ∂x_j

其中 f_i 是状态方程的第 i 个分量，x_j 是状态向量的第 j 个分量。

听起来很抽象？我们举个具体的例子。假设一个简单的二维系统：

状态向量：x = [p, v]^T   // 位置和速度
状态方程：
  p_k = p_{k-1} + v_{k-1} * dt
  v_k = v_{k-1} + a * dt    // a 是加速度，假设已知

这个系统其实是线性的，但为了演示，我们把它当成非线性来算雅可比。状态方程 f 有两个分量：

f_1 = p_{k-1} + v_{k-1} * dt
f_2 = v_{k-1} + a * dt

那么雅可比矩阵 F 就是：

F = [ ∂f_1/∂p, ∂f_1/∂v ]   = [ 1,  dt ]
    [ ∂f_2/∂p, ∂f_2/∂v ]     [ 0,  1  ]

你看，结果就是一个常数矩阵。但在真正的组合导航中，状态方程往往包含姿态、四元数、地球自转等，雅可比矩阵会非常复杂，通常需要借助链式法则和数值微分来辅助计算。

我个人建议，在工程实现中，先用解析法推导出雅可比矩阵的表达式，再用数值法验证。这样可以避免推导错误，也能保证计算效率。

4.4 知识体系总览

为了让你对本章内容有一个整体的把握，我画了一张流程图，展示了EKF的核心逻辑：

这张图把EKF的整个流程串起来了。你可以看到，非线性系统经过线性化后，就变成了标准KF可以处理的形式。这就是EKF的精髓。

4.5 实战中的几点建议

最后，结合我多年的工程经验，给你几个实用建议：

• 雅可比矩阵一定要验证：用数值差分法对比解析结果，能发现很多推导错误。
• 注意线性化点的选择：EKF的线性化点就是当前的状态估计值。如果估计值偏差太大，线性化误差也会变大，可能导致发散。
• 考虑使用迭代EKF（IEKF）：在观测更新步骤中，可以多次迭代线性化和更新，提高精度。我在高精度组合导航中经常用这个方法。
• 不要盲目相信EKF：当系统非线性非常强时（比如大角度姿态变化），EKF的表现可能不如UKF或粒子滤波。要根据实际情况选择算法。