从KF到ESKF：五大滤波算法核心思想与工程选型指南
1. 滤波算法入门：从贝叶斯到马尔科夫
第一次接触滤波算法时，我被各种缩写搞得晕头转向。直到把KF、EKF这些算法实际用在机器人定位项目上，才真正理解它们的精妙之处。所有滤波算法的核心都在解决同一个问题：如何从带有噪声的观测数据中，尽可能准确地估计系统状态。
想象你正在玩一个蒙眼走迷宫的游戏。每走一步，你只能通过脚下的震动（系统运动模型）和同伴的语音提示（观测模型）来判断位置。滤波算法就是帮你把这两种不确定信息融合起来的大脑。这里涉及两个关键数学工具：贝叶斯概率告诉我们如何用新证据更新信念，马尔科夫假设则简化了问题，认为当前状态只与上一时刻有关。
在实际工程中，我常用一个简单的例子来解释这个过程：用GPS和里程计估算车辆位置。GPS数据虽然全局准确但更新频率低，里程计高频但存在累积误差。滤波算法就像个聪明的调解员，实时平衡这两种传感器的优缺点。这也是为什么在自动驾驶和机器人领域，滤波算法会成为多传感器融合的基石。
2. 五大滤波算法核心思想解析
2.1 经典KF：线性世界的完美解
卡尔曼滤波(KF)是所有滤波算法的鼻祖，我在早期做四轴飞行器控制时深刻体会了它的魅力。KF要求系统必须满足两个强约束：状态转移和观测模型都必须是线性的，噪声必须符合高斯分布。这种情况下，KF能给出最优估计。
举个例子，假设我们要估计匀速运动的小车位置。状态方程可以表示为：
# 状态向量 [位置, 速度] x_k = F * x_{k-1} + B * u_k # 状态转移 z_k = H * x_k # 观测模型其中F是状态转移矩阵，H是观测矩阵。KF通过预测-更新的循环，不断修正估计值。但现实世界充满非线性，这就引出了它的改进版本。
2.2 EKF：非线性问题的暴力解法
扩展卡尔曼滤波(EKF)是我在无人机姿态估计中第一个用到的非线性滤波算法。它的聪明之处在于对非线性函数进行泰勒展开，保留一阶项实现局部线性化。就像用无数个小直线段来逼近曲线。
但这里有个坑：雅可比矩阵计算。记得第一次实现EKF时，我手工推导了IMU模型的雅可比矩阵，整整花了三天时间。后来发现现代自动微分工具可以解决这个问题，比如使用Ceres Solver：
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ImuError, 3, 4>(new ImuError(measurement));EKF的主要问题是线性化误差会累积，特别是在快速机动时。有次测试无人机翻滚动作，EKF估计很快就发散了，这促使我寻找更稳定的方案。
2.3 UKF：用特征点捕捉非线性
无迹卡尔曼滤波(UKF)采用了完全不同的思路。它不像EKF那样强行线性化，而是精心选取一组sigma点来捕捉概率分布特征。这就像用几个关键采样点来描述整个分布形状。
在激光雷达SLAM项目中，我对比过EKF和UKF的表现。UKF对非线性强的系统（如剧烈转弯时的车辆模型）估计更稳定，但计算量也更大。一个实用的经验是：当系统非线性程度超过30°时，UKF开始显现优势。
2.4 PF：蒙特卡洛的力量
粒子滤波(PF)是另一种思路，用大量粒子来近似概率分布。我在做视觉定位时用过PF，设置2000个粒子时定位精度能达到厘米级，但CPU占用率直接飙到80%。这引出了PF的最大挑战：粒子退化问题。
实践中我采用了两项优化：
- 系统重采样策略避免粒子贫化
- 自适应粒子数调整
def resample(particles, weights): indices = np.random.choice(len(particles), size=len(particles), p=weights) return particles[indices], np.ones_like(weights)/len(weights)2.5 ESKF：误差状态的智慧
误差状态卡尔曼滤波(ESKF)是我现在最常用的算法，它巧妙地将状态分解为名义状态和误差状态。就像手表的时间显示（名义状态）和每日误差（误差状态）分开管理。这种设计带来了三个优势：
- 误差状态量通常很小，线性近似更准确
- 避免了过参数化问题
- 计算效率高，适合嵌入式系统
在IMU预积分实验中，ESKF的运算速度比EKF快3倍，精度却相当。这是因为它只需要在误差状态这个小范围内做KF，而不必处理整个非线性系统。
3. 工程选型指南
3.1 算法对比矩阵
| 维度 | KF | EKF | UKF | PF | ESKF |
|---|---|---|---|---|---|
| 线性要求 | 严格 | 弱非线性 | 强非线性 | 任意 | 弱非线性 |
| 计算复杂度 | O(n²) | O(n³) | O(n³) | O(N·n²) | O(n²) |
| 内存占用 | 低 | 中 | 中 | 高 | 低 |
| 实现难度 | ★★☆ | ★★★☆ | ★★★★ | ★★★☆ | ★★★☆ |
| 适用频率 | 1kHz+ | 100Hz | 50Hz | 10Hz | 500Hz |
3.2 选型决策树
根据我的项目经验，可以按以下流程选择：
- 系统是否线性？是→KF，否→下一步
- 计算资源是否充足？否→ESKF，是→下一步
- 非线性程度如何？弱→EKF，强→下一步
- 需要多模态估计？是→PF，否→UKF
有个实际案例：在农业机器人项目中，我们最终选择了ESKF。因为既要处理IMU的非线性，又要在树莓派上实时运行。ESKF在10% CPU占用下达到了2cm的定位精度。
4. ESKF实现细节
4.1 状态分解技巧
ESKF的核心在于状态分解。以IMU为例：
- 名义状态：q(四元数), v(速度), p(位置)
- 误差状态：δθ(角度误差), δv(速度误差), δp(位置误差)
这种分解使得误差状态始终在零点附近波动，保证了线性近似的有效性。我在代码中是这样实现的：
struct State { Quaterniond q; // 名义姿态 Vector3d v; // 名义速度 Vector3d p; // 名义位置 }; struct ErrorState { Vector3d delta_theta; Vector3d delta_v; Vector3d delta_p; };4.2 预测-更新流程
ESKF的工作流程比传统KF多一个注入步骤：
- 预测名义状态（非线性）
- 预测误差状态（线性KF）
- 观测更新误差状态
- 将误差状态注入名义状态
- 重置误差状态
这个流程在IMU-GPS融合中表现优异。实测数据显示，纯IMU积分10秒位置误差达5米，而ESKF融合后控制在0.3米内。
4.3 参数调优经验
经过多个项目积累，我总结出ESKF调参的三个要点：
- 过程噪声Q：反映系统模型置信度，通常取IMU厂商提供的噪声参数
- 观测噪声R：与传感器精度相关，可通过静态测试标定
- 误差状态重置时机：建议每次更新后立即重置，避免误差累积
一个实用的调试技巧是监控新息序列（innovation sequence），它应该符合零均值白噪声特性。如果出现系统性偏差，说明模型或噪声参数需要调整。
5. 前沿发展与实用建议
虽然深度学习在状态估计领域很火热，但滤波算法仍是工业界的首选。最近我在研究一种混合方案：用神经网络预测观测噪声参数，再输入ESKF。这种方法在视觉惯性里程计中取得了15%的精度提升。
对于刚入门的工程师，我的建议是：
- 从KF开始，彻底理解预测-更新框架
- 使用Eigen等矩阵库避免重复造轮子
- 可视化中间结果，便于调试
- 在实际系统中预留足够的计算余量
记得第一次实现ESKF时，因为没有正确处理四元数更新，导致无人机在翻滚时估计发散。这个教训让我明白：理论理解必须配合充分的仿真验证。现在我的开发流程总是遵循"仿真-实验室-实地"三步走，节省了大量调试时间。