从滤波到优化：手把手拆解VIO算法核心，看懂OpenVINS的MSCKF和ORB-SLAM3的BA到底差在哪

从滤波到优化：手把手拆解VIO算法核心，看懂OpenVINS的MSCKF和ORB-SLAM3的BA到底差在哪

视觉惯性里程计（VIO）技术近年来在机器人导航、增强现实等领域展现出强大的生命力。对于开发者而言，理解不同技术流派的核心差异，远比记住几个算法名字更有价值。本文将聚焦OpenVINS和ORB-SLAM3这两个代表性框架，深入剖析滤波与优化方法在VIO实现中的本质区别。

1. 状态估计的两大流派：滤波与优化

在VIO领域，状态估计方法主要分为滤波和优化两大阵营。滤波方法以卡尔曼滤波为代表，通过预测-更新的递推方式处理传感器数据；而优化方法则基于批量非线性优化，通过最小化重投影误差来求解状态。

滤波方法的核心特点：

• 递推式处理：每次只处理当前时刻的观测
• 马尔可夫性假设：当前状态只依赖于前一时刻状态
• 计算效率高，适合实时系统

优化方法的核心特点：

• 批量处理：同时优化多个时刻的状态
• 利用历史观测信息构建全局一致性
• 精度更高，但计算量相对较大

提示：虽然理论上滤波和优化在理想条件下能达到相同精度，但实际实现中的各种近似会导致性能差异。

2. OpenVINS的MSCKF：滤波方法的典型实现

OpenVINS采用了多状态约束卡尔曼滤波（MSCKF）作为其核心算法。这种滤波方法通过维护一个滑动窗口内的多个相机状态，利用特征点在多个帧间的观测构建几何约束。

2.1 MSCKF的核心机制

MSCKF的关键创新在于：

1. 状态扩增：维护一个包含多个相机位姿的状态向量
2. 特征参数化：使用逆深度等参数化方式表示路标点
3. 观测压缩：将多个观测压缩为一个等效观测，减少计算量

// MSCKF状态向量示例 StateVector { IMU状态: [位置, 速度, 姿态, 陀螺仪bias, 加速度计bias] 相机状态: [位姿1, 位姿2, ..., 位姿N] }

2.2 FEJ：保持线性化一致性

MSCKF采用首次估计雅可比（FEJ）技术来解决滤波器的线性化不一致问题。FEJ的核心思想是：

• 对所有观测使用相同的线性化点
• 避免重复线性化带来的误差
• 保持信息矩阵的一致性

FEJ实现效果对比：

3. ORB-SLAM3的BA：优化方法的精妙设计

ORB-SLAM3采用基于图优化的方法，通过捆绑调整（BA）来实现状态估计。其核心是通过构建视觉重投影误差和IMU预积分误差的联合优化问题。

3.1 BA的数学表达

ORB-SLAM3的优化问题可以表示为：

min_{X} (∑||z_vision - h_vision(X)||^2_Σv + ∑||z_imu - h_imu(X)||^2_Σi)

其中：

• X包含所有待优化状态（位姿、速度、bias、路标点）
• h_vision和h_imu分别是视觉和IMU的观测模型
• Σv和Σi是各观测的协方差矩阵

3.2 边缘化策略

ORB-SLAM3采用边缘化来处理历史信息：

1. 将旧状态转换为先验信息
2. 保留其对当前状态的影响
3. 避免直接优化所有历史状态

边缘化实现步骤：

1. 构建包含待边缘化状态的Hessian矩阵
2. 进行Schur补操作得到边缘化后的先验
3. 将先验信息加入后续优化问题

4. 滤波与优化的本质差异

虽然MSCKF和BA在理论上等价，但实际实现中存在几个关键差异点：

4.1 线性化处理方式

4.2 历史信息利用

MSCKF的处理方式：

• 滑动窗口内状态直接估计
• 窗口外信息通过边缘化丢弃
• 信息利用率较低

BA的处理方式：

• 通过边缘化保留历史信息影响
• 构建更完整的概率图模型
• 信息利用率高

4.3 计算效率对比

在EuRoC数据集上的实测表现：

5. 实际应用中的选择建议

根据我们在多个实际项目中的经验，滤波和优化方法各有最适合的场景：

选择MSCKF当：

• 硬件资源有限
• 需要极高实时性
• 运动较为平缓

选择BA当：

• 追求最高精度
• 有足够计算资源
• 运动复杂多变

一个实用的折中方案是采用基于优化的紧耦合VIO作为前端，配合滤波方法进行状态预测，这样既能保证精度，又能满足实时性要求。