别再混淆了！一张图讲清EsKF、IEKF和EsIKF在VIO/SLAM中的区别与联系

误差状态迭代卡尔曼滤波（EsIKF）在SLAM中的核心优势与实践解析

卡尔曼滤波算法在SLAM领域的发展历程就像一部不断进化的技术史诗，从最初的基础KF到应对非线性系统的EKF，再到如今广泛应用的EsKF和IEKF，每一次迭代都解决了特定场景下的痛点问题。而误差状态迭代卡尔曼滤波（EsIKF）作为这一演进路线上的最新成果，正在VIO和LiDAR SLAM系统中展现出独特的价值。本文将带您深入理解这三种滤波方法的本质区别，以及EsIKF如何巧妙结合前两者的优势，成为处理强非线性观测问题的利器。

1. 卡尔曼滤波演进史：从EsKF到IEKF的核心突破

1.1 误差状态卡尔曼滤波（EsKF）的数值稳定性之道

EsKF采用了一种独特的"误差状态"建模方式，与传统卡尔曼滤波直接估计系统状态不同，它只估计真实状态与名义状态之间的误差量。这种方法的优势主要体现在：

• 数值稳定性：对于姿态估计等场景，误差状态通常是小量，避免了直接处理大角度带来的奇异性问题
• 计算效率：误差状态维度往往低于完整状态空间，如三维姿态误差仅需3参数而非四元数的4参数
• 更新策略：采用"预测-校正-注入"的三步流程，其中误差状态在更新后会被"注入"到名义状态并重置

# EsKF的典型更新流程示例 def eskf_update(nominal_state, error_state, P, z_meas): # 预测步骤 error_state_pred = F @ error_state P_pred = F @ P @ F.T + Q # 校正步骤 K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R) error_state_updated = error_state_pred + K @ (z_meas - h(nominal_state)) P_updated = (I - K @ H) @ P_pred # 注入步骤 nominal_state_updated = nominal_state + injection(error_state_updated) return nominal_state_updated, P_updated

1.2 迭代扩展卡尔曼滤波（IEKF）的非线性征服之路

IEKF针对EKF在强非线性系统中的局限性，引入了迭代重新线性化的机制：

IEKF的核心突破在于每次迭代都基于当前最优估计重新计算观测方程的雅可比矩阵，有效缓解了EKF因线性化点不准确导致的"一阶近似陷阱"。在VINS-Mono等视觉惯性系统中，IEKF对特征点重投影误差的优化效果尤为显著。

注意：IEKF的迭代过程仅在校正阶段进行，预测阶段仍保持传统EKF形式。通常3-5次迭代即可达到满意精度，过多迭代会导致边际效益递减。

2. EsIKF：误差状态与迭代优化的完美融合

2.1 算法架构设计哲学

EsIKF的诞生源于对两类问题的深刻洞察：

1. 状态空间的数值特性：机器人状态中的姿态等分量需要特殊处理以避免奇异性
2. 观测模型的非线性强度：视觉观测等非线性函数需要更精确的线性化策略

// EsIKF的伪代码实现框架 for each iteration { // 在最新优化点x_op处线性化观测模型 H_k = compute_jacobian_at(x_op); z_pred = observation_model(x_op); // 误差状态更新 delta_x = K * (z_meas - z_pred - H_k*(x_hat - x_op)); // 状态更新与迭代点调整 x_op = x_hat + delta_x; if(converged) break; }

2.2 数学形式化解析

EsIKF的预测阶段沿用EsKF框架：

$$ \begin{aligned} \delta \mathbf{x}k &= \mathbf{F} \delta \mathbf{x}{k-1} \ \mathbf{P}k &= \mathbf{F} \mathbf{P}{k-1} \mathbf{F}^T + \mathbf{Q} \end{aligned} $$

校正阶段则引入IEKF的迭代思想，关键改进在于：

1. 动态雅可比矩阵计算：每次迭代在最新估计点$x_{op}^{(i)}$处重新计算$H_k^{(i)}$
2. 误差状态观测模型：保持误差状态的小量特性，同时享受迭代优化的精度提升
3. 混合状态更新：最终将误差状态注入名义状态时进行完整性约束处理

2.3 实际系统中的应用对比

以FAST-LIO2和VINS-Mono为例：

• FAST-LIO2：采用EsIKF处理LiDAR点云配准，通过误差状态避免数值问题，同时迭代优化匹配精度
• VINS-Mono：在视觉重投影环节使用类似技术，将特征点匹配误差降至像素级以下

实验数据显示，在高速运动场景下，EsIKF相比传统方法可将位置估计误差降低40-60%，特别是在急转弯等强机动情况下优势更为明显。

3. 关键场景下的算法选型指南

3.1 何时选择EsKF？

• 状态估计本身具有良好的线性特性
• 系统对计算实时性要求极高
• 主要状态变量不存在奇异性问题
• 典型场景：低动态IMU的纯惯性导航

3.2 何时选择IEKF？

• 观测模型呈现强非线性特性
• 系统有足够的计算余量
• 状态空间本身数值特性良好
• 典型场景：基于单目相机的位姿估计

3.3 何时选择EsIKF？

• 同时存在状态奇异性和观测非线性问题
• 系统需要毫米级精度保障
• 计算资源相对充足
• 典型场景：视觉-惯性紧耦合SLAM、LiDAR-惯性融合系统

实践建议：在无人机视觉导航等场景中，可对不同的状态分量采用差异化策略——姿态估计使用EsIKF，而位置估计使用标准IEKF，实现精度与效率的最佳平衡。

4. 实现细节与性能优化技巧

4.1 收敛性保障机制

EsIKF的迭代过程需要精心设计停止条件：

1. 残差阈值：$|z_{meas} - h(x_{op})| < \epsilon$
2. 增量阈值：$|\delta x| < \delta$
3. 最大迭代次数：通常3-5次
4. 协方差监测：防止迭代过程中不确定性异常增大

4.2 计算效率优化

• 稀疏性利用：SLAM问题的雅可比矩阵通常具有块稀疏结构
• 并行化策略：观测项的雅可比计算可并行化
• 缓存机制：重复使用的中间结果可缓存

# 利用numpy的einsum加速矩阵运算 J = np.einsum('ijk,ikl->ijl', A, B) # 比np.dot更高效的大矩阵乘法

4.3 常见陷阱与规避方案

1. 线性化点漂移：定期重置到最优估计点
2. 数值不稳定：对四元数等采用特殊处理
3. 观测异常值：结合鲁棒核函数（Huber、Cauchy）
4. 计算累积误差：关键帧机制与定期全局优化

在实际工程中，EsIKF的实现往往需要与前端预处理、后端优化等模块协同设计，形成一个完整的状态估计流水线。例如在LiDAR SLAM中，点云特征提取的质量会直接影响EsIKF的收敛性和最终精度。