第4章状态估计与滤波

状态估计与滤波：EKF、UKF、粒子滤波对比

做飞行器控制这些年，我最大的感触就是：传感器数据从来不会告诉你真相。GPS飘个几米、IMU零偏慢慢跑、气压计受风影响跳来跳去——这些噪声如果不处理，你的控制器根本没法用。

所以状态估计就成了必选项。今天咱们聊聊三种最常用的滤波方法：扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF）。

说白了，它们干的是同一件事：从带噪声的测量值里，把真实状态“猜”出来。但怎么猜、猜得多准、代价多大，差别就大了。

核心思路：任何滤波问题都可以拆成两步——预测（根据模型往前推）和更新（用测量值修正）。三种方法的区别，就在于“怎么处理非线性”和“怎么表示概率分布”。

1. 扩展卡尔曼滤波（EKF）—— 经典但需要小心

EKF 是卡尔曼滤波的非线性版本。它的想法很直接：把非线性函数做一阶泰勒展开，近似成线性。

我在项目中遇到过一个问题：用 EKF 做四旋翼的姿态估计，一开始效果还行，但一旦机动剧烈，估计值就开始发散。后来发现是雅可比矩阵算错了——那玩意儿一旦偏导求错，整个滤波器就崩了。

EKF 的流程其实不复杂：

预测步：用状态方程 f(x) 算先验状态，用线性化的雅可比矩阵 F 传播协方差
更新步：用观测方程 h(x) 算预测测量，用雅可比矩阵 H 计算卡尔曼增益
融合：用测量残差修正状态和协方差

// C++ 伪代码：EKF 预测步
void predict(Eigen::VectorXd &x, Eigen::MatrixXd &P, 
             const Eigen::VectorXd &u, double dt) {
    // 非线性状态传播
    x = f(x, u, dt);  
    // 线性化：计算雅可比矩阵 F
    Eigen::MatrixXd F = computeJacobianF(x, u, dt);
    // 协方差传播
    P = F * P * F.transpose() + Q;
}

注意：EKF 的线性化误差在高非线性系统中会累积。我曾经见过一个案例，EKF 在无人机大角度机动时直接“炸了”——因为泰勒展开只取一阶项，忽略了高阶非线性。

2. 无迹卡尔曼滤波（UKF）—— 更聪明的近似

UKF 的思路完全不同。它不去算雅可比矩阵，而是用一组确定的采样点（Sigma点）来近似概率分布。

你想想看：既然非线性函数不好处理，那我干脆多取几个点，让这些点通过非线性函数，再统计它们的均值和协方差。这不就绕开了求导吗？

我个人习惯用 UKF 做飞行器的姿态估计，尤其是四元数相关的滤波。因为四元数的雅可比矩阵推导太容易出错了，而 UKF 完全不用操心这个。

// Python 伪代码：UKF Sigma点生成
def generate_sigma_points(x, P, lambda_):
    n = len(x)
    sigma = np.zeros((2*n+1, n))
    sigma[0] = x
    # 计算矩阵平方根
    sqrt_P = np.linalg.cholesky((n + lambda_) * P)
    for i in range(n):
        sigma[i+1] = x + sqrt_P[:, i]
        sigma[n+i+1] = x - sqrt_P[:, i]
    return sigma

UKF 的好处很明显：不需要求导，精度比 EKF 高（能到三阶泰勒精度）。代价就是计算量稍微大一点——但以现在的硬件水平，这点开销完全可以接受。

经验之谈：如果你的系统非线性强，或者雅可比矩阵太难算，直接上 UKF。我在做固定翼无人机编队时，EKF 总是收敛慢，换成 UKF 后问题就解决了。

3. 粒子滤波（PF）—— 终极武器，但代价高

粒子滤波的思路更暴力：用一大堆随机粒子来近似任意分布。不管你的系统多非线性、噪声多非高斯，理论上粒子滤波都能处理。

但代价也很明显——计算量巨大。我见过一个项目，用 5000 个粒子做地面机器人的定位，跑在树莓派上帧率只有 5Hz。对于飞行器这种实时性要求高的场景，得慎重。

粒子滤波的核心步骤：

初始化：生成 N 个粒子，每个粒子代表一个可能的状态
预测：每个粒子根据状态方程加噪声往前推
权重更新：根据测量值计算每个粒子的权重
重采样：权重高的粒子多复制，权重低的淘汰

// Python 伪代码：粒子滤波重采样
def resample(particles, weights):
    N = len(particles)
    # 计算累积权重
    cumsum = np.cumsum(weights)
    cumsum[-1] = 1.0  # 避免浮点误差
    # 生成均匀随机数
    positions = (np.random.random(N) + np.arange(N)) / N
    # 重采样
    new_particles = np.zeros_like(particles)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumsum[j]:
            new_particles[i] = particles[j]
            i += 1
        else:
            j += 1
    return new_particles

避坑指南：我曾经在室内无人机定位中试过粒子滤波，结果粒子数量太少（200个）导致发散，太多（10000个）又跑不动。后来发现，对于飞行器这种高动态系统，粒子滤波更适合做全局定位初始化，一旦收敛就切到 EKF 或 UKF。

4. 三种方法对比

咱们直接看表格，一目了然：

特性	EKF	UKF	粒子滤波
非线性处理	一阶泰勒展开	Sigma点采样	随机粒子
精度	一阶	三阶（高斯噪声下）	理论上任意精度
计算量	低（O(n³)）	中（O(n³)）	高（O(N·n²)）
适用场景	弱非线性、实时性高	中等非线性、实时性高	强非线性、非高斯、低实时
实现难度	中（需算雅可比）	低（无需求导）	中（重采样逻辑）

5. 核心逻辑框架图

下面这张图展示了三种滤波方法的核心逻辑对比：

6. 实际工程中的选择建议

说了这么多，到底怎么选？我个人的经验是这样的：

做飞控姿态估计：首选 UKF。不用求导，精度够，实时性也满足。PX4 和 ArduPilot 里用的就是 UKF 变体。
做 GPS/INS 组合导航：EKF 仍然主流。因为 GPS 更新频率低，EKF 的计算量优势明显。
做全局定位初始化或 SLAM：粒子滤波有独特优势。尤其是非高斯噪声场景，比如室内 WiFi 定位。

一个小技巧：如果你不确定用哪个，先上 UKF。它比 EKF 鲁棒，比 PF 快。我几乎所有新项目都从 UKF 开始，除非遇到性能瓶颈才考虑换。

嗯，关于状态估计的三种方法就聊到这儿。记住一句话：没有最好的滤波器，只有最合适的。理解每种方法的假设和代价，你就能在工程中做出正确的选择。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

状态估计与滤波：EKF、UKF、粒子滤波对比

1. 扩展卡尔曼滤波（EKF）—— 经典但需要小心

2. 无迹卡尔曼滤波（UKF）—— 更聪明的近似

3. 粒子滤波（PF）—— 终极武器，但代价高

4. 三种方法对比

5. 核心逻辑框架图

6. 实际工程中的选择建议