1. 姿态估计中的卡尔曼滤波演进:从KF到ESKF
姿态估计是机器人、无人机和AR/VR等领域的基础问题。当我们使用陀螺仪等惯性传感器测量物体姿态时,原始数据总是包含噪声。卡尔曼滤波提供了一种优雅的解决方案,但其不同变种在姿态估计中的表现差异显著。
1.1 问题的本质:噪声与几何结构
陀螺仪测量的角速度数据通常呈现高频跳动,这是由多种微小扰动叠加形成的。根据中心极限定理,我们可以将这些噪声建模为均值为零的高斯分布。滤波器的核心任务就是通过数据融合,找到方差最小的最优估计。
姿态描述的特殊性在于它属于SO(3)李群结构,这意味着:
- 旋转操作不满足交换律
- 旋转叠加应通过乘法而非加法实现
- 欧拉角的简单加法会破坏几何结构
1.2 卡尔曼滤波家族三大成员
- 线性卡尔曼滤波器(KF):适用于状态转移和观测均为线性的系统
- 扩展卡尔曼滤波器(EKF):通过雅可比矩阵局部线性化处理非线性系统
- 误差状态卡尔曼滤波器(ESKF):在切空间维护误差状态,严格尊重流形结构
需要模型API调用? 免费领10W Token,多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 线性卡尔曼滤波器(KF)的局限
2.1 KF的基本原理
KF建立在两个关键假设上:
- 马尔可夫性:当前状态仅与前一时刻状态相关
- 所有误差均为高斯噪声
其核心方程包括:
- 状态预测:$\hat x_k^- = Fx_{k-1} + Gu_k + w_k$
- 协方差预测:$P_k^- = FP_{k-1}F^T + Q$
- 卡尔曼增益:$K_k = P_k^-H^T(HP_k^-H^T + R)^{-1}$
- 状态更新:$\hat x_k = \hat x_k^- + K_k(z_k - H\hat x_k^-)$
- 协方差更新:$P_k = (I - K_kH)P_k^-$
2.2 在姿态估计中的问题
KF直接对欧拉角进行线性运算,这违背了旋转的几何本质。实验表明:
- 静态条件下KF表现尚可
- 动态情况下会出现严重发散
- 根本原因是小角度假设在剧烈运动时失效
实际工程中,许多开源陀螺仪库仍使用KF,这是通过提高采样频率来缓解不稳定性的妥协方案。但从理论角度看,KF并不适合姿态估计。
3. 扩展卡尔曼滤波器(EKF)的改进与局限
3.1 四元数基础
E
