1 预积分中关于IMU零偏的建模
从之前的学习中知道了IMU的预积分计算公式为:
从公式中可以看到,IMU预积分时除了受噪声
n
n
n的影响,还有IMU零偏
b
b
b 带来的影响。
而VINS-mono作为一个紧耦合的SLAM方案,其待估计和优化的状态变量除了
p
v
q
pvq
pvq 之外呢,还有加速度计和陀螺仪的零偏
b
b
b,一共是
15
∗
1
15*1
15∗1维的向量。
那么,系统每优化迭代一次,零偏也会发生小小的变化调整。相应的当下次迭代时,作为新一轮优化需要的预积分约束要带入这些微调的零偏重新计算一遍吗?
显然这样做太浪费时间了,如果优化迭代50次,那就要重新计算50次预积分啊。那么怎么办呢?
我们利用增量的思维来考虑这个问题,也就是说更新后的零偏在原先的预积分基础上额外增加了哪些影响。
所以我们计算出预积分量对零偏的雅可比矩阵之后带入上述公式,就可以避免带入更新后的零偏重新计算所有预积分了 。
VINS-mono论文中给出了公式:
2 如何计算预积分量关于零偏的雅可比矩阵
从VINS-mono论文给的公式来看,
我们只需要求
J
b
a
α
,
J
b
w
α
,
J
b
a
β
,
J
b
w
β
,
J
b
w
γ
J^{\alpha}_{b_a},J^{\alpha}_{b_w},J^{\beta}_{b_a},J^{\beta}_{b_w},J^{\gamma}_{b_w}
Jbaα,Jbwα,Jbaβ,Jbwβ,Jbwγ,但是为什么要计算那个
15
∗
15
15*15
15∗15的看似冗余的矩阵形式呢?
前面我们推导基于中值积分的离散时间IMU预积分误差状态传递时,得到一个式子:
为什么说避免重复计算呢?
比如我们计算出了第 k k k帧的 J k J_k Jk,那么我们像上节那样计算完 (I+F_t δ \delta δt)后,再乘以 J k J_k Jk就可以直接得到 J k + 1 J_{k+1} Jk+1。
这样零偏的微小调整给整个系统带来的影响就可以这样向后传递。