第4章非线性系统的挑战

第4章：非线性系统的挑战：泰勒展开与线性化思想

说实话，我第一次接触卡尔曼滤波时，心里想的是：这玩意儿真漂亮，公式简洁，逻辑完美。但一放到真实项目中，立马就碰壁了。

为什么？因为现实世界是非线性的。

你想想看，我们之前推导的卡尔曼滤波，核心假设是系统是线性的——状态转移和观测都是线性变换。但实际中，机器人转弯、传感器测距、卫星定位……哪个不是非线性？

嗯，这就是本章要解决的问题。

4.1 非线性系统到底难在哪？

先看一个最简单的例子。

假设你有一个机器人，它沿着圆弧运动。状态是位置 (x, y) 和朝向角 θ。运动模型是：

x_k = x_{k-1} + v * cos(θ_{k-1}) * dt
y_k = y_{k-1} + v * sin(θ_{k-1}) * dt
θ_k = θ_{k-1} + ω * dt

这里出现了 cos 和 sin。它们是非线性函数。

标准卡尔曼滤波要求状态转移是线性的，即：

x_k = A * x_{k-1} + B * u_k + w_k

但我们的模型里，x_k 依赖于 cos(θ)，这没法写成矩阵乘法。

怎么办？

我早期做无人机姿态估计时，就踩过这个坑。当时直接用标准卡尔曼滤波处理加速度计和陀螺仪数据，结果滤波结果发散得一塌糊涂。后来才意识到——非线性系统不能用线性滤波器硬套。

4.2 泰勒展开：用直线逼近曲线

核心思想其实很简单：

既然非线性函数不好处理，那就在当前估计点附近，用一条直线去近似它。

这就是泰勒展开的一阶近似。

对于任意函数 f(x)，在点 a 附近的一阶泰勒展开是：

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

说白了，就是用切线代替原曲线。

举个例子。假设 f(x) = sin(x)，在 x = 0 处展开：

sin(x) ≈ sin(0) + cos(0) * (x - 0) = 0 + 1 * x = x

你看，sin(x) 在 0 附近近似成一条直线 y = x。误差有多大？当 x = 0.1 时，sin(0.1) ≈ 0.0998，近似值 0.1，误差只有 0.0002。可以接受。

但 x 越大，误差越大。当 x = 1.0 时，sin(1.0) ≈ 0.8415，近似值 1.0，误差 0.1585。这就不能忍了。

注意： 泰勒展开只在展开点附近有效。如果系统状态变化剧烈，或者预测步长太大，线性化误差会迅速累积，导致滤波发散。

4.3 扩展到多维：雅可比矩阵

实际系统中，状态往往是多维向量。比如机器人状态是 [x, y, θ]^T，运动模型是三个非线性函数。

这时候，我们需要对每个函数求偏导，组成一个矩阵——雅可比矩阵。

假设状态向量 x = [x₁, x₂, ..., xₙ]^T，非线性函数 f(x) 输出 m 维向量。雅可比矩阵 J 是 m×n 的矩阵，其中第 i 行第 j 列元素是：

J_{ij} = ∂f_i / ∂x_j

回到机器人的例子。状态 x = [x, y, θ]^T，运动模型 f 输出也是三维：

f₁ = x + v * cos(θ) * dt
f₂ = y + v * sin(θ) * dt
f₃ = θ + ω * dt

雅可比矩阵 J 是 3×3：

J = [
  [∂f₁/∂x, ∂f₁/∂y, ∂f₁/∂θ],
  [∂f₂/∂x, ∂f₂/∂y, ∂f₂/∂θ],
  [∂f₃/∂x, ∂f₃/∂y, ∂f₃/∂θ]
]

计算一下：

∂f₁/∂x = 1,  ∂f₁/∂y = 0,  ∂f₁/∂θ = -v * sin(θ) * dt
∂f₂/∂x = 0,  ∂f₂/∂y = 1,  ∂f₂/∂θ =  v * cos(θ) * dt
∂f₃/∂x = 0,  ∂f₃/∂y = 0,  ∂f₃/∂θ = 1

所以：

J = [
  [1, 0, -v * sin(θ) * dt],
  [0, 1,  v * cos(θ) * dt],
  [0, 0,  1]
]

这个雅可比矩阵，就是扩展卡尔曼滤波中状态转移矩阵 A 的替代品。

核心要点： 在 EKF 中，我们用雅可比矩阵代替标准卡尔曼滤波中的 A 矩阵和 H 矩阵。预测和更新公式中的非线性函数 f 和 h 仍然使用原始非线性函数，但协方差传播使用线性化的雅可比矩阵。

4.4 观测模型也要线性化

不只是运动模型，观测模型同样需要处理。

比如，你用一个激光雷达测量机器人的距离和角度。观测模型是：

r = sqrt(x² + y²)
φ = atan2(y, x)

这两个函数也是非线性的。同样需要求雅可比矩阵。

对于观测函数 h(x)，雅可比矩阵 H 是：

H = [
  [∂r/∂x, ∂r/∂y, ∂r/∂θ],
  [∂φ/∂x, ∂φ/∂y, ∂φ/∂θ]
]

计算一下：

∂r/∂x = x / sqrt(x² + y²)
∂r/∂y = y / sqrt(x² + y²)
∂r/∂θ = 0

∂φ/∂x = -y / (x² + y²)
∂φ/∂y =  x / (x² + y²)
∂φ/∂θ = 0

注意，这里 ∂φ/∂θ = 0，因为观测角度不依赖于机器人的朝向。但实际中，如果传感器有安装偏角，就需要考虑。

我曾经做过一个项目，激光雷达安装时没校准，偏了 2 度。结果滤波结果一直有系统偏差。后来在观测模型里加了一个安装偏角参数，问题就解决了。嗯，细节决定成败。

4.5 EKF 的完整流程

现在我们把所有东西串起来。EKF 的流程和标准卡尔曼滤波很像，只是多了线性化这一步。

预测步骤：

用非线性函数 f 预测状态：x̂_k|k-1 = f(x̂_{k-1|k-1}, u_k)
计算雅可比矩阵 F_k = ∂f/∂x 在 x̂_{k-1|k-1} 处的值
预测协方差：P_k|k-1 = F_k * P_{k-1|k-1} * F_k^T + Q_k

更新步骤：

计算观测残差：ỹ_k = z_k - h(x̂_k|k-1)
计算雅可比矩阵 H_k = ∂h/∂x 在 x̂_k|k-1 处的值
计算卡尔曼增益：K_k = P_k|k-1 * H_k^T * (H_k * P_k|k-1 * H_k^T + R_k)^{-1}
更新状态：x̂_k|k = x̂_k|k-1 + K_k * ỹ_k
更新协方差：P_k|k = (I - K_k * H_k) * P_k|k-1

你看，和标准卡尔曼滤波的公式几乎一样，只是把 A 换成了 F_k，把 H 换成了 H_k。

我的建议： 在实际编码时，先把非线性函数 f 和 h 写清楚，再手动推导雅可比矩阵。不要偷懒用数值微分，数值微分容易引入额外误差，而且计算量大。我习惯用符号推导工具（比如 SymPy）先算好解析式，再硬编码到代码里。

4.6 一个完整的 Python 示例

下面是一个简单的 EKF 实现，用于二维目标跟踪。状态是 [x, y, vx, vy]^T，运动模型是匀速模型（其实是线性的，但为了演示，我们故意加一个非线性项）。

import numpy as np

class EKF:
    def __init__(self):
        # 状态向量 [x, y, vx, vy]
        self.x = np.array([0.0, 0.0, 1.0, 0.5])
        # 协方差矩阵
        self.P = np.eye(4) * 0.1
        # 过程噪声协方差
        self.Q = np.eye(4) * 0.01
        # 观测噪声协方差
        self.R = np.eye(2) * 0.1
        # 时间步长
        self.dt = 0.1

    def f(self, x):
        """非线性状态转移函数"""
        # 这里故意加一个非线性项：速度的平方项
        v = np.sqrt(x[2]**2 + x[3]**2)
        return np.array([
            x[0] + x[2] * self.dt + 0.1 * v * self.dt,
            x[1] + x[3] * self.dt + 0.1 * v * self.dt,
            x[2],
            x[3]
        ])

    def F_jacobian(self, x):
        """状态转移雅可比矩阵"""
        v = np.sqrt(x[2]**2 + x[3]**2)
        if v < 1e-6:
            dv_dvx = 0
            dv_dvy = 0
        else:
            dv_dvx = x[2] / v
            dv_dvy = x[3] / v
        return np.array([
            [1, 0, self.dt + 0.1 * dv_dvx * self.dt, 0.1 * dv_dvy * self.dt],
            [0, 1, 0.1 * dv_dvx * self.dt, self.dt + 0.1 * dv_dvy * self.dt],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])

    def h(self, x):
        """观测函数：测量位置 (x, y)"""
        return np.array([x[0], x[1]])

    def H_jacobian(self, x):
        """观测雅可比矩阵"""
        return np.array([
            [1, 0, 0, 0],
            [0, 1, 0, 0]
        ])

    def predict(self):
        F = self.F_jacobian(self.x)
        self.x = self.f(self.x)
        self.P = F @ self.P @ F.T + self.Q

    def update(self, z):
        H = self.H_jacobian(self.x)
        y = z - self.h(self.x)
        S = H @ self.P @ H.T + self.R
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ H) @ self.P

# 使用示例
ekf = EKF()
for i in range(100):
    ekf.predict()
    # 模拟观测
    z = np.array([i*0.1 + np.random.normal(0, 0.1),
                  i*0.05 + np.random.normal(0, 0.1)])
    ekf.update(z)
    print(f"Step {i}: x={ekf.x[0]:.2f}, y={ekf.x[1]:.2f}")

这个例子中，我故意在运动模型里加了速度的平方项，制造非线性。你看，EKF 依然能正常工作。

4.7 调参技巧与避坑指南

最后，分享几个实战中总结的经验。

关于雅可比矩阵：

一定要在正确的点处求值。预测步骤用 x̂_{k-1|k-1}，更新步骤用 x̂_k|k-1。搞混了会出大问题。
如果系统强非线性，一阶泰勒展开不够用。可以考虑二阶 EKF 或 UKF。

关于协方差初始化：

P 矩阵初始值不要设太大。我见过有人设成 100*I，结果滤波半天不收敛。
Q 和 R 的比值决定了滤波器的响应速度。Q/R 越大，滤波器越相信观测，响应越快，但噪声也大。

我曾经踩过的坑： 有一次做车辆定位，运动模型里用了大角度转弯。雅可比矩阵在 θ 接近 90 度时，sin 和 cos 的导数变化剧烈。我忘了检查雅可比矩阵的数值稳定性，结果协方差矩阵变得不正定，滤波直接崩溃。后来加了数值检查，发现异常就重置协方差，才解决问题。

关于线性化误差：

如果系统状态变化快，减小时间步长 dt 可以降低线性化误差。
如果观测更新频率低，预测步长太长，误差会累积。这时候可以考虑用迭代 EKF（IEKF），在更新步骤中多次线性化。

嗯，这一章的内容就到这里。泰勒展开和线性化是 EKF 的基石，理解透了，后面的内容就顺了。下一章我们聊聊实际项目中怎么调参，以及那些让你头疼的数值稳定性问题。

📚 课程目录