IMU Propagation

一、概述

IMU Propagation利用两帧图像之间的IMU数据进行预测步骤，主要分为两部分内容：

• 状态向量预测：直接根据mid-point或者RK4进行状态预测

• 协方差预测：根据误差状态进行协方差更新

二、状态向量预测

Euler积分、Mid-Point积分、RK4、smsckf使用的方法(v、p使用RK4，q使用四元数零阶积分)

分析：

Euler方法精度最差，smsckf精度最高（只使用2个测试用例，结果存在一定偶然性）

计算时间复杂度：Euler积分<Mid-Point积分<smsckf使用的方法<RK4，RK4电脑运行时间0.000002s

综上所述，使用smsckf使用的方法(v、p使用RK4，q使用四元数零阶积分)

方法：

三、协方差预测

四元数表示：q_i_w：w在i下的姿态

MSCKF论文一般使用JPL表示，即左手系，此时：[qv, qw]

我们是在IMU坐标系下添加扰动，所以使用左乘

方法一：先离散化，再求转移方程

先离散化，再求转移方程

过程噪声：

目标是计算：



误差状态转移方程：

其中$\text{n}$为IMU传感器噪声，通过allen方差曲线标定得到。$\text{x}$为误差状态变量。

协方差更新为：

其中Qd为离散时间噪声协方差

推到细节见：https://docs.openvins.com/propagation.html

方法二：连续方程求解F,在求解$\Phi$

推导IMU误差状态的连续运动模型

根据线性微分方程的求解公式：

进而得到：

S-MSCKF代码采用这种实现。F和G如下：

四、FEJ

• 旋转误差左乘改为右乘

• 每个状态值使用其第一次的估计值,用x_k|k-1代替x_k|k
  通常EKF我们的状态转移矩阵连乘是不成立的：
  

用x_k|k-1代替x_k|k，这样可以保证下式成立

本质上就是使用每个状态的第一次估计值计算雅可比，对应到MSCKF中就是只是用传播的v和p计算雅可比，而不是用更新之后的v和p。代码中姿态也FEJ了。

分析：

• 大多数情况加入FEJ会有比较好的RMSE精度提升，少数情况反而会变差，分析可能原因，FEJ若第一次估计的位姿并不准确，效果会变差，且滤波方法一次估计偏了，后续很难调回来

• 这里用groundtruth进行初始化，所以起点是完全对齐的，RMSE结果更合理（原点对齐）

• evo_rpe取得距离很近，小于1m，所以结果值很小，evo_ape进行了配准，结果会比真实好一些

五、OC-EKF

基本思想：强制修改Phi和H，使其满足观测矩阵零空间为4

观测矩阵M、右零空间N1

我们想要设计合适的Phi和H使得下式成立：

我们修改Phi和H

第k步零空间定义：

其中$\text{f}$是特征位置的第一次估计，在第l步三角化得到。

要满足
即下式成立：

得到：


后两项可求解这个最小化问题得到：


因此，根据拉格朗日乘数法得到闭式解：

H需要满足
，即

如果特征用全局坐标系位置表示，
上式可写为：

这是一个Au=0，同理，相当于上面最小化问题w=0，闭式解为

恢复测量Jacobian

<vec: ref: https://zhuanlan.zhihu.com/p/328940891>

分析：

• OC_EKF对精度还是有一定提升的

六、小结

1. 整体而言，FEJ和OC方法对一致性都有改善，但在实际使用过程中，个人更倾向于FEJ,前面提到实现相对简单，v和q直接替换第一次观测值即可，姿态上有些变换技巧(详情可以参见OpenVINS实现代码),但也比较好理解
2. FEJ由于使用第一次的线性化点，可能存在线性化点离真实状态更远的情况，因此实际测试过程中精度未必会有提升；
3. FEJ和OC虽然未必一定会提升精度，但对于滤波系统而言，可以让协方差更为合理，这对于需要合理置信度输出的系统而言还是有一定意义的。