4、扩展卡尔曼滤波：非线性系统线性化、雅可比矩阵计算、风速风向融合案例

各位同学，欢迎来到第四讲。

前面我们聊了标准卡尔曼滤波，它很强大，但有个硬伤——只能处理线性系统。可咱们的气象站，偏偏就是个非线性世界。风速和风向的关系，可不是简单的加减乘除。今天，我们就来啃这块硬骨头：扩展卡尔曼滤波（EKF）。

4.1 为什么需要扩展卡尔曼滤波？

标准卡尔曼滤波假设系统是线性的，噪声是高斯分布的。但现实很骨感。你想想看，风速传感器测的是标量风速，风向标测的是角度。这两个东西怎么融合？直接相加？那风向 350° 和 10° 的平均值，按线性算出来是 180°，但实际应该是 0° 或 360°。这明显不对。

我早年做第一个气象站项目时，就踩过这个坑。当时天真地用了标准卡尔曼，结果风向融合结果一塌糊涂，尤其是在风速很低、风向乱飘的时候。后来才明白，非线性系统必须用非线性滤波器。

EKF 的核心思想很简单：把非线性函数在当前估计点附近做一阶泰勒展开，近似成线性系统。说白了，就是用切线去逼近曲线。

这里的 F 就是雅可比矩阵，它描述了状态转移函数 f 对每个状态变量的偏导数。

4.2 雅可比矩阵：EKF 的“翻译官”

雅可比矩阵，听起来很高大上，其实它就是多变量函数的导数矩阵。对于 EKF，我们需要计算两个雅可比矩阵：

• 状态转移雅可比矩阵 F：描述状态如何随时间变化。
• 观测雅可比矩阵 H：描述状态如何映射到观测值。

我个人的习惯是，先写出系统的非线性方程，然后逐项求偏导。别怕，大部分情况下，这些偏导都有解析解，不需要数值微分。

4.2.1 状态转移雅可比矩阵 F

假设我们的状态向量是 [风速, 风向]ᵀ。风速变化可以用一个随机游走模型，风向变化则复杂一些。一个简化的模型是：

// 状态方程（非线性）
风速(k) = 风速(k-1) + w_speed
风向(k) = 风向(k-1) + w_dir

// 雅可比矩阵 F（对状态求偏导）
F = [ ∂f1/∂风速  ∂f1/∂风向 ]
    [ ∂f2/∂风速  ∂f2/∂风向 ]

// 对于这个简单模型，F 就是单位矩阵
F = [ 1  0 ]
    [ 0  1 ]

嗯，这里要注意，如果风向模型里包含了风速对风向的影响（比如低风速时风向更不稳定），那 F 就不是单位矩阵了。我在实际项目中，通常会加入一个风速相关的噪声项，让低风速时的过程噪声 Q 更大。

4.2.2 观测雅可比矩阵 H

这是 EKF 的关键。我们的观测值是什么？是风速传感器测的标量风速 v，和风向标测的角度 θ。但这两个观测值，与状态向量 [风速, 风向]ᵀ 的关系是线性的吗？

是的，在这个例子里，观测方程是线性的：

// 观测方程（线性）
z_v = 风速 + v_speed_noise
z_θ = 风向 + v_dir_noise

// 观测雅可比矩阵 H
H = [ ∂h1/∂风速  ∂h1/∂风向 ]
    [ ∂h2/∂风速  ∂h2/∂风向 ]

// 对于线性观测，H 就是
H = [ 1  0 ]
    [ 0  1 ]

等等，你可能会问：“既然观测是线性的，那为什么还要用 EKF？” 问得好！因为状态转移可能是非线性的。比如，当风速很低时，风向的随机游走模型会变得非常不稳定，甚至出现“跳变”。这时候，我们需要在状态转移中引入非线性函数，比如一个风速相关的阻尼项。

4.3 风速风向融合案例：从理论到代码

好了，理论讲完了，我们来看一个完整的案例。假设我们有一个超声波风速计，它能同时输出风速和风向。但这两个信号都有噪声，而且噪声特性不同。我们的目标是融合它们，得到更平滑、更可靠的风速风向估计。

4.3.1 系统模型

4.3.2 EKF 迭代步骤（C 语言伪代码）

// 1. 预测步骤
void ekf_predict(EKF_t *ekf) {
    // 状态预测：简单模型，直接复制
    ekf->x[0] = ekf->x[0];  // 风速
    ekf->x[1] = ekf->x[1];  // 风向

    // 雅可比矩阵 F（这里假设是单位阵）
    float F[2][2] = {{1, 0}, {0, 1}};

    // 协方差预测：P = F * P * F' + Q
    // 由于 F 是单位阵，简化为 P = P + Q
    ekf->P[0][0] += ekf->Q[0][0];
    ekf->P[1][1] += ekf->Q[1][1];
}

// 2. 更新步骤
void ekf_update(EKF_t *ekf, float z_v, float z_theta) {
    // 观测雅可比矩阵 H（线性观测，单位阵）
    float H[2][2] = {{1, 0}, {0, 1}};

    // 计算卡尔曼增益 K = P * H' * inv(H * P * H' + R)
    // 这里 H 是单位阵，简化计算
    float S[2][2];
    S[0][0] = ekf->P[0][0] + ekf->R[0][0];
    S[1][1] = ekf->P[1][1] + ekf->R[1][1];

    float K[2][2];
    K[0][0] = ekf->P[0][0] / S[0][0];
    K[1][1] = ekf->P[1][1] / S[1][1];

    // 计算残差（注意风向角度归一化！）
    float y_v = z_v - ekf->x[0];
    float y_theta = z_theta - ekf->x[1];
    // 关键：将风向残差归一化到 [-PI, PI]
    y_theta = atan2f(sinf(y_theta), cosf(y_theta));

    // 状态更新
    ekf->x[0] += K[0][0] * y_v;
    ekf->x[1] += K[1][1] * y_theta;

    // 协方差更新
    ekf->P[0][0] = (1 - K[0][0]) * ekf->P[0][0];
    ekf->P[1][1] = (1 - K[1][1]) * ekf->P[1][1];
}

4.4 实战中的坑与经验

最后，分享几个我亲身踩过的坑：

• 初始化很重要：EKF 对初始状态和协方差很敏感。我建议用前 10 个观测值的均值作为初始状态，协方差设得大一些（比如传感器噪声方差的 10 倍），让滤波器自己收敛。
• Q 矩阵不是常数：风速稳定时，Q 可以小一点；风速剧烈变化时，Q 要相应增大。我习惯用一个风速的方差来动态调整 Q。
• 风向的“跳变”问题：当风向在 0° 附近时，测量值可能在 359° 和 1° 之间跳变。如果不做处理，EKF 会认为这是巨大误差，导致发散。解决方案就是我在代码里写的：残差归一化。

好了，这一讲的内容就到这里。EKF 是处理非线性系统的利器，但也不是万能的。如果非线性太强，或者系统维度太高，你可能需要考虑无迹卡尔曼滤波（UKF）或粒子滤波。那是后话了。

下一讲，我们会讨论无迹卡尔曼滤波，看看它是如何用“采样点”来避免雅可比矩阵计算的。敬请期待。