理论三、 MSCKF 数学基础

感谢：紫薯萝卜 https://zhuanlan.zhihu.com/p/76793847

 

1. 数学定义

后续将进行MSCKF数学基础介绍，先总结一下数据符号定义

坐标系定义

物理量定义

估计量定义

四元数定义：四元数有Hamilton和JPL两种形式，不同论文定义不一致，后面统一使用的JPL形式，区别如下

 

2. 误差状态向量

MSCKF后端的融合框架用的error-state EKF，其状态向量包括：IMU状态和N个历史相机状态，误差状态向量如下：

这里波浪号代表误差量，除了旋转量以外，其他误差项都是直接相加(真实值=估计值+误差值)，旋转量的误差项的叠加方式为：

状态协方差矩阵为：

注意：这里的只讨论了最基本的15维IMU状态，很多论文为了获得更高的精度，会将状态向量扩展到更高维，如将相机内外参、时间同步误差等状态量加入到状态向量中，在后续篇章中会进行单独介绍。

 

3. Error-State EKF

先回顾EKF(Extendec Kalman Filter)：

• 非线性模型：

其中

 

EKF预测：

EKF更新：

协方差更新有三种等价形式[2]：

可以推导出3种形式是相等的，推导如下：

MSCKF 中用的ESKF(Error-State Kalman Filter)，为什么要用ESKF而不是EKF呢? 参考文献[2]中解释了几点理由, 翻译过来大体是：

• 朝向(姿态)的误差状态是最小参数描述, 避免了冗余参数化导致协方差矩阵奇异;
• 误差状态系统通常在原点附近, 参数远离奇异点或万向节锁等问题, 从而确保EKF线性化一直有效;
• 误差状态通常很小, 意味着二阶项可以忽略. 这使得Jacobian能够快速容易计算;
• 误差状态的动态变化较慢, 这是因为大的信号动态性被积分到了额定状态. 这意味着EKF更新频率可以比预测频率低.(VIO中通常IMU数据频率为100-500HZ, 而图像频率只有20-30HZ左右)

 

MSCKF中的运动模型和观测模型如下：

运动模型和观测模型都是关于误差状态向量 的，其中 是线性化的Jacobian矩阵。需要注意的是，运动模型只与IMU状态有关，相机状态不发生改变，但是相机与IMU状态之间的协方差会发生改变。观测模型其实至于相机状态有关，但会通过协方差同步对IMU状态进行修正。

MSCKF Propagation/Predict

MSCKF中状态向量Propagation是利用IMU数值积分对原IMU状态（而不是误差状态）进行Propagation，数值积分方法有Euler法，Mid-Point法，Runge-Kutta法等。协方差迭代公式为:

MSCKF Update

MSCKF更新步骤与EKF更新步骤基本类似

与常规EKF更新公式唯一的区别在状态向量更新量直接是 ，协方差更新公式使用的对称正定式版本。有了MSCKF的预测和更新步骤，后面我们需要做得就是将运动模型和观测模型推导出来，其实就是求 这几个Jacobian矩阵，然后套用EKF公式迭代计算状态向量及其对应的协方差。

 

参考文献

1. Indirect Kalman Filter for 3D Attitude Estimation
2. Quaternion kinematics for the error-state Kalman filter
3. Mourikis A I, Roumeliotis S I. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation[C]// IEEE International Conference on Robotics and Automation. IEEE, 2007:3565-3572