VINS-Mono理论学习——IMU预积分 Pre-integration (Jacobian 协方差)

引言

VINS论文的IV-B. IMU Pre-integration介绍了IMU预积分模型,Foster的两篇论文对IMU预积分理论进行详细分析。

传统传统捷联惯性导航的递推算法,是在已知上一时刻的IMU状态量(姿态和速度、位移)情况下,利用IMU测量得到的线加速度和角速度,做积分运算得到当前时刻的状态量。

然而在基于非线性优化的VIO中,各个节点的状态量都是估计值(此处还包括Ba和Bg)。当这些状态量在优化算法中,会不断调整,每次调整都需要在它们之间重新积分,传递IMU测量值。因此提出了IMU预积分,希望对IMU的相对测量进行处理,使它与绝对位姿解耦,或者只要线性运算就可以进行矫正,从而避免绝对位姿被优化时进行重复积分。

所有式子中的旋转采用了右乘形式(from local to global),可能看起来不习惯。使用右乘形式的好处是,可以直接使用角速度在体坐标系下的坐标,而左乘形式需要将角速度转换到世界坐标系下。

因本人能力有限,本文有大量内容参考了崔华坤的《VINS论文推导及代码解析》、邱笑晨《预积分总结与公式推导》。

IMU模型

IMU测量值包括加速度计得到的线加速度 a ^ t \hat a_t a^t和陀螺仪得到的角速度 w ^ t \hat w_t w^t 【论文式(1)】
a ^ t = a t + b a t + R w t g w + n a \hat a_t=a_t+b_{a_t}+R^t_wg^w+n_a a^t=at+bat+Rwtgw+na

w ^ t = w t + b w t + n w \hat w_t=w_t+b_{w_t}+n_w w^t=wt+bwt+nw

其中 t t t 下标表示在体坐标系(IMU坐标系)下,并受到加速度偏置 b a b_a ba、陀螺仪偏置 b w b_w bw和附加噪声 n a 、 n w n_a、n_w nanw的影响。线加速度是重力加速度和物体加速度的合矢量。

假设附加噪声为高斯噪声:
n a ∼ N ( 0 , σ a 2 ) , n w ∼ N ( 0 , σ w 2 ) n_a∼N(0,σ_a^2) ,n_w∼N(0,σ_w^2) naN(0,σa2)nwN(0,σw2)

加速度计偏置和陀螺仪偏置被建模为随机游走,其导数为高斯性的: 【论文式(2)】
b ˙ a t = n b a , b ˙ w t = n b w \dot b_{a_t}=n_{b_a},\dot b_{w_t}=n_{b_w} b˙at=nbab˙wt=nbw

n b a ∼ N ( 0 , σ b a 2 ) , n b w ∼ N ( 0 , σ b w 2 ) n_{b_a}∼N(0,σ_{b_a}^2),n_{b_w}∼N(0,σ_{b_w}^2) nbaN(0,σba2)nbwN(0,σbw2)

对于图像帧k和k+1,体坐标系对应为 b k b_k bk b k + 1 b_{k+1} bk+1,位置、速度和方向状态值PVQ可以根据 [ t k , t k + 1 ] [t_k,t_{k+1}] [tk,tk+1]时间间隔内的IMU测量值,在世界坐标系下进行传递:【论文式(3)(4)】
p b k + 1 w = p b k w + v b k w Δ t k + ∬ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t 2 p^w_{b_{k+1}}=p^w_{b_k}+v^w_{b_k} \Delta t_k+\iint_{t\in[t_k,t_{k+1}]}(R^w_t(\hat a_t-b_{a_t}-n_a)-g^w)dt^2 pbk+1w=pbkw+vbkwΔtk+t[tk,tk+1](Rtw(a^tbatna)gw)dt2

v b k + 1 w = v b k w + ∫ t ∈ [ t k , t k + 1 ] ( R t w ( a ^ t − b a t − n a ) − g w ) d t v^w_{b_{k+1}}=v^w_{b_k}+\int_{t\in[t_k,t_{k+1}]}(R^w_t(\hat a_t-b_{a_t}-n_a)-g^w)dt vbk+1w=vbkw+t[tk,tk+1](Rtw(a^tbatna)gw)dt
q b k + 1 w = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ ( w ^ t − b w t − n w ) 0 ] d t q^w_{b_{k+1}}=q^w_{b_k}\otimes \int_{t\in[t_k,t_{k+1}]} \frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} (\hat w_t-b_{w_t}-n_w)\\ 0 \end{matrix} \right] dt qbk+1w=qbkwt[tk,tk+1]21qtbk[(w^tbwtnw)0]dt

          = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) q t b k d t ( ∗ ) \space\space\space\space\space\space\space\space\space=q^w_{b_k}\otimes \int_{t\in[t_k,t_{k+1}]} \frac{1}{2} \Omega(\hat w_t-b_{w_t}-n_w)q^{b_k}_tdt \tag{$*$}          =qbkwt[tk,tk+1]21Ω(w^tbwtnw)qtbkdt()

Ω ( ω ) = [ − [ ω ] × ω − ω T 0 ] , [ ω ] × = [ 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ] \Omega(\omega)=\left[\begin{matrix} -[\omega]_{\times} & \omega \\ -\omega^T & 0 \end{matrix} \right],[\omega]_{\times}=\left[ \begin{matrix} 0&-\omega_z&\omega_y\\ \omega_z&0&-\omega_x \\-\omega_y& \omega_x&0 \end{matrix} \right] Ω(ω)=[[ω]×ωTω0][ω]×=0ωzωyωz0ωxωyωx0

Δ t k \Delta t_k Δtk [ t k , t k + 1 ] [t_k,t_{k+1}] [tk,tk+1]之间的时间间隔, R t w R^w_t Rtw为t时刻本体坐标系相对于bk时刻本体坐标系的旋转矩阵, q t b k q^{b_k}_t qtbk为用四元数表示的t时刻从体坐标系到世界坐标系的旋转,这里的四元数实部在后,虚部在前。这里的 Ω ( ω ) \Omega(\omega) Ω(ω)表示四元数右乘。

可以看到,坐标系 b k + 1 b_{k+1} bk+1的状态( p b k + 1 w 、 v b k + 1 w 、 q b k + 1 w p^w_{b_{k+1}}、v^w_{b_{k+1}}、q^w_{b_{k+1}} pbk+1wvbk+1wqbk+1w)依赖于 b k b_k bk的状态。那么如果直接将体坐标系的状态作为变量放入非线性优化中,在进行后端优化时会迭代更新,这将导致我们需要根据每次迭代后的状态量重新进行状态传递,即需要重新传递IMU状态值,这样就会存在很大的计算量。


关于(*)的推导:
这里主要需要知道四元数的左乘右乘以及导数定理(这部分将参考我关于四元数的博客):
q ( t ) q(t) q(t)是一个单位四元数函数, ω ( t ) ω(t) ω(t) 是由 q ( t ) q(t) q(t)确定的角速度。则 q ( t ) q(t) q(t)的导数为:
q ˙ = 1 2 [ ω 0 ] ⊗ q ( 左 乘 ) = 1 2 q ⊗ [ ω 0 ] ( 右 乘 ) \dot q=\frac{1}{2}\left[\begin{matrix} \omega\\0 \end{matrix} \right] \otimes q(左乘)=\frac{1}{2}q \otimes \left[\begin{matrix} \omega\\0 \end{matrix} \right] (右乘) q˙=21[ω0]q=21q[ω0]

因此有:
q b k + 1 w = q b k w ⊗ q b k + 1 b k = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] q ˙ t b k d t = q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ ω t b k 0 ] d t q^w_{b_{k+1}}=q^w_{b_{k}}\otimes q^{b_k}_{b_{k+1}}=q^w_{b_{k}}\otimes \int_{t\in[t_k,t_{k+1}]} \dot q^{b_k}_t dt=q^w_{b_{k}}\otimes \int_{t\in[t_k,t_{k+1}]} \frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} \omega^{b_k}_t\\0 \end{matrix} \right] dt qbk+1w=qbkwqbk+1bk=qbkwt[tk,tk+1]q˙tbkdt=qbkwt[tk,tk+1]21qtbk[ωtbk0]dt

= q b k w ⊗ ∫ t ∈ [ t k , t k + 1 ] 1 2 q t b k ⊗ [ ( w ^ t − b w t − n w ) 0 ] d t =q^w_{b_k}\otimes \int_{t\in[t_k,t_{k+1}]} \frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} (\hat w_t-b_{w_t}-n_w)\\0 \end{matrix} \right] dt =qbkwt[tk,tk+1]21qtbk[(w^tbwtnw)0]dt


离散形式

下面还给出基于中值积分的离散形式公式,这在void Estimator::processIMU()函数中得以实现。 作用是将IMU积分出来第j时刻PVQ作为第j帧图像的优化初始值。

p b k + 1 w = p b k w + v b k w Δ t k + 1 2 a ^ t ˉ δ t 2 p^w_{b_{k+1}}=p^w_{b_k}+v^w_{b_k} \Delta t_k+ \frac{1}{2} \bar{\hat a_t} \delta t^2 pbk+1w=pbkw+vbkwΔtk+21a^tˉδt2

v b k + 1 w = v b k w + a ^ t ˉ δ t v^w_{b_{k+1}}=v^w_{b_k} +\bar{\hat a_t} \delta t vbk+1w=vbkw+a^tˉδt

q b k + 1 w = q b k w ⊗ [ 1 1 2 w ^ ˉ t δ t ] q^w_{b_{k+1}}=q^w_{b_k} \otimes \begin{bmatrix}1\\{\frac{1}{2} \bar{\hat w}_t \delta t}\end{bmatrix} qbk+1w=qbkw[121w^ˉtδt]
a ^ t ˉ = 1 2 [ q i ( a ^ t − b a i ) − g w + q i + 1 ( a ^ t + 1 − b a i ) − g w ] \bar{\hat a_t} =\frac{1}{2} [q_i(\hat a_t-b_{a_i})-g^w+q_{i+1}(\hat a_{t+1}-b_{a_i})-g^w] a^tˉ=21[qi(a^tbai)gw+qi+1(a^t+1bai)gw]

w ^ t ˉ = 1 2 ( w ^ t + w ^ t + 1 ) − b w i \bar{\hat w_t} = \frac{1}{2}(\hat w_t+\hat w_{t+1})-b_{w_i} w^tˉ=21(w^t+w^t+1)bwi


IMU预积分

IMU预积分的思想就是将参考坐标系从世界坐标系 w w w调整为第 k k k帧的体坐标系 b k b_k bk,即同时乘以 R w b k R^{b_k}_w Rwbk:【论文式(5)(6)】

R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R^{b_k}_w p^w_{b_{k+1}}=R^{b_k}_w (p^w_{b_k}+v^w_{b_k} \Delta t_k-\frac{1}{2} g^w\Delta t_k^2)+ \alpha^{b_k}_{b_{k+1}} Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk21gwΔtk2)+αbk+1bk

R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k R^{b_k}_w v^w_{b_{k+1}}=R^{b_k}_w (v^w_{b_k}- g^w\Delta t_k)+\beta^{b_k}_{b_{k+1}} Rwbkvbk+1w=Rwbk(vbkwgwΔtk)+βbk+1bk

q w b k ⊗ q b k + 1 w = γ b k + 1 b k q^{b_k}_w\otimes q^w_{b_{k+1}}=\gamma^{b_k}_{b_{k+1}} qwbkqbk+1w=γbk+1bk

其中:

α b k + 1 b k = ∬ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 \alpha^{b_k}_{b_{k+1}}=\iint_{t\in[t_k,t_{k+1}]}R^{b_k}_t(\hat a_t-b_{a_t}-n_a)dt^2 αbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt2

β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t \beta^{b_k}_{b_{k+1}}=\int_{t\in[t_k,t_{k+1}]}R^{b_k}_t(\hat a_t-b_{a_t}-n_a)dt βbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt

γ b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) γ t b k d t \gamma^{b_k}_{b_{k+1}}=\int_{t\in[t_k,t_{k+1}]} \frac{1}{2}\Omega(\hat w_t-b_{w_t}-n_w)\gamma^{b_k}_tdt γbk+1bk=t[tk,tk+1]21Ω(w^tbwtnw)γtbkdt

此时的积分结果 α b k + 1 b k 、 β b k + 1 b k 、 γ b k + 1 b k \alpha^{b_k}_{b_{k+1}}、\beta^{b_k}_{b_{k+1}}、\gamma^{b_k}_{b_{k+1}} αbk+1bkβbk+1bkγbk+1bk可以理解为 b k + 1 b_{k+1} bk+1 b k b_k bk的相对运动量, b k b_k bk的状态改变并不会对其产生影响,因此将其作为非线性优化变量,可以避免状态的重复传递。

注意,这是在假设IMU的偏置 b a 、 b w b_a、b_w babw已经确定的情况下,实际上偏置也是需要优化的变量,那么在每次迭代时, b a 、 b w b_a、b_w babw发生改变,得重新根据公式求得所有帧之间的IMU预积分。

当偏置变换很小时,可以将 α b k + 1 b k 、 β b k + 1 b k 、 γ b k + 1 b k \alpha^{b_k}_{b_{k+1}}、\beta^{b_k}_{b_{k+1}}、\gamma^{b_k}_{b_{k+1}} αbk+1bkβbk+1bkγbk+1bk 按其对偏置的一阶近似来调整,否则就进行重新传递。[论文式(12)](这部分暂时只是抛出一个概念,后面会讲为什么这样写)

α b k + 1 b k ≈ α ^ b k + 1 b k + J b a α δ b a + J b w α δ b w {\alpha}^{b_k}_{b_{k+1}} \approx \hat {\alpha}^{b_k}_{b_{k+1}}+J^{\alpha}_{b_a}\delta b_a+J^{\alpha}_{b_w}\delta b_w αbk+1bkα^bk+1bk+Jbaαδba+Jbwαδbw

β b k + 1 b k ≈ β ^ b k + 1 b k + J b a β δ b a + J b w β δ b w {\beta}^{b_k}_{b_{k+1}} \approx \hat {\beta}^{b_k}_{b_{k+1}}+J^{\beta}_{b_a}\delta b_a+J^{\beta}_{b_w}\delta b_w βbk+1bkβ^bk+1bk+Jbaβδba+Jbwβδbw

γ b k + 1 b k ≈ γ ^ b k + 1 b k ⊗ [ 1 1 2 J b w γ δ b w ] \gamma^{b_k}_{b_{k+1}} \approx \hat {\gamma}^{b_k}_{b_{k+1}} \otimes\begin{bmatrix}1\\{\frac{1}{2} J^{\gamma}_{b_w} \delta b_w}\end{bmatrix} γbk+1bkγ^bk+1bk[121Jbwγδbw]


离散形式

接下来讨论一下对于离散时间的具体实现。这里需要提出的是在VINS论文中使用的是欧拉积分,而代码中使用的是中值积分

在这里插入图片描述
欧拉法给出第i时刻与第i+1时刻的变量估计值的关系为:【论文式(7)】

α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 R ( γ ^ i b k ) ( a ^ i − b a i ) δ t 2 \hat {\alpha}^{b_k}_{i+1}=\hat {\alpha}^{b_k}_i+\hat {\beta}^{b_k}_i \delta t + \frac{1}{2}R(\hat {\gamma}^{b_k}_i)(\hat a_i-b_{a_i})\delta t^2 α^i+1bk=α^ibk+β^ibkδt+21R(γ^ibk)(a^ibai)δt2

β ^ i + 1 b k = β ^ i b k + R ( γ ^ i b k ) ( a ^ i − b a i ) δ t \hat {\beta}^{b_k}_{i+1}=\hat {\beta}^{b_k}_i+R(\hat {\gamma}^{b_k}_i)(\hat a_i-b_{a_i})\delta t β^i+1bk=β^ibk+R(γ^ibk)(a^ibai)δt

γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 1 2 ( w ^ i − b w i ) δ t ] \hat {\gamma}^{b_k}_{i+1} = \hat {\gamma}^{b_k}_i \otimes \hat {\gamma}^i_{i+1} =\hat {\gamma}^{b_k}_i \otimes \begin{bmatrix}1\\{\frac{1}{2} (\hat w_i-b_{w_i}) \delta t}\end{bmatrix} γ^i+1bk=γ^ibkγ^i+1i=γ^ibk[121(w^ibwi)δt]

而代码中采用的基于中值法的IMU预积分公式,在Estimator::processIMU()函数中的IntergrationBase::push_back()中得以实现。

α ^ i + 1 b k = α ^ i b k + β ^ i b k δ t + 1 2 a ^ ˉ i δ t 2 \hat {\alpha}^{b_k}_{i+1}=\hat {\alpha}^{b_k}_i+\hat {\beta}^{b_k}_i \delta t + \frac{1}{2}\bar{\hat {a}}_i \delta t^2 α^i+1bk=α^ibk+β^ibkδt+21a^ˉiδt2

β ^ i + 1 b k = β ^ i b k + a ^ ˉ i δ t \hat {\beta}^{b_k}_{i+1}=\hat {\beta}^{b_k}_i+\bar{\hat {a}}_i \delta t β^i+1bk=β^ibk+a^ˉiδt

γ ^ i + 1 b k = γ ^ i b k ⊗ γ ^ i + 1 i = γ ^ i b k ⊗ [ 1 1 2 w ^ ˉ i δ t ] \hat {\gamma}^{b_k}_{i+1} = \hat {\gamma}^{b_k}_i \otimes \hat {\gamma}^i_{i+1} =\hat {\gamma}^{b_k}_i \otimes \begin{bmatrix}1\\{\frac{1}{2} \bar{\hat {w}}_i \delta t}\end{bmatrix} γ^i+1bk=γ^ibkγ^i+1i=γ^ibk[121w^ˉiδt]

a ^ ˉ i = 1 2 [ q i ( a ^ i − b a i ) + q i + 1 ( a ^ i + 1 − b a i ) ] \bar{\hat {a}}_i=\frac{1}{2}[q_i(\hat a_i-b_{a_i})+q_{i+1}(\hat a_{i+1}-b_{a_{i}})] a^ˉi=21[qi(a^ibai)+qi+1(a^i+1bai)]

w ^ ˉ i = 1 2 ( w ^ i + w ^ i + 1 ) − b w i \bar{\hat {w}}_i= \frac{1}{2}(\hat {w}_i+\hat {w}_{i+1})-b_{w_i} w^ˉi=21(w^i+w^i+1)bwi

初始状态 α b k b k 、 β b k b k 为 0 , γ b k b k \alpha^{b_k}_{b_k}、\beta^{b_k}_{b_k}为0,\gamma^{b_k}_{b_k} αbkbkβbkbk0γbkbk为单位四元数, n a 、 n w n_a、n_w nanw被视为0, i i i为在 [ k , k + 1 ] [k,k+1] [k,k+1]中IMU测量值的某一时刻, δ t \delta t δt为IMU测量值i和i+1之间的时间间隔。


误差递推方程、协方差和雅可比矩阵

目前我们已经完成了IMU预积分测量值的推导,而要将IMU预积分运用到非线性优化中,需要建立线性高斯误差状态递推方程,由线性高斯系统的协方差,推导方程协方差矩阵,并求解对应的雅可比矩阵。

首先由于四元数 γ t b k {\gamma}^{b_k}_{t} γtbk被过参数化,我们将其误差项定义为围绕其均值的扰动:【论文式(8)】
γ t b k ≈ γ ^ t b k ⊗ [ 1 1 2 δ θ t b k ] {\gamma}^{b_k}_{t} \approx \hat{\gamma}^{b_k}_{t} \otimes \begin{bmatrix}1\\{\frac{1}{2} \delta \theta^{b_k}_t}\end{bmatrix} γtbkγ^tbk[121δθtbk]

我们给出在t时刻误差项的线性化递推方程为:【论文式(9)】
[ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ w t ] = [ 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] × − R t b k 0 0 0 − [ w ^ t − b w t ] × 0 − I 0 0 0 0 0 0 0 0 0 0 ] [ δ α t b k δ β t b k δ θ t b k δ b a t δ b w t ] + [ 0 0 0 0 − R t b k 0 0 0 0 − I 0 0 0 0 I 0 0 0 0 I ] [ n a n w n b a n b w ] = F t δ z t b k + G t n t (*) \begin{bmatrix}\delta \dot \alpha^{b_k}_t \\ \delta \dot \beta^{b_k}_t \\ \delta \dot \theta^{b_k}_t \\ \delta \dot b_{a_t} \\ \delta \dot b_{w_t} \end{bmatrix} = \begin{bmatrix}0&I&0&0&0\\0&0&-R^{b_k}_t {[\hat a_t-b_{a_t}]}_{\times} & -R^{b_k}_t&0\\0&0&- {[\hat w_t-b_{w_t}]}_{\times}&0&-I\\0&0&0&0&0\\0&0&0&0&0 \end{bmatrix} \begin{bmatrix}\delta \alpha^{b_k}_t \\ \delta \beta^{b_k}_t \\ \delta \theta^{b_k}_t \\ \delta b_{a_t} \\ \delta b_{w_t} \end{bmatrix} + \begin{bmatrix}0&0&0&0\\-R^{b_k}_t&0&0&0\\0&-I&0&0\\0&0&I&0\\0&0&0&I \end{bmatrix} \begin{bmatrix}n_{a}\\n_w\\n_{b_a}\\n_{b_w} \end{bmatrix} \\ =F_t\delta z^{b_k}_t+G_tn_t \tag{*} δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙wt=00000I00000Rtbk[a^tbat]×[w^tbwt]×000Rtbk00000I00δαtbkδβtbkδθtbkδbatδbwt+0Rtbk00000I00000I00000Inanwnbanbw=Ftδztbk+Gtnt(*)

可以简写为:
δ z ˙ t b k = F t δ z t b k + G t n t \delta \dot z^{b_k}_t=F_t\delta z^{b_k}_t+G_tn_t δz˙tbk=Ftδztbk+Gtnt
其中: F t F_t Ft是15X15, G t G_t Gt是15X12, δ z t b k \delta z^{b_k}_t δztbk是15X1, n t n_t nt是12X1。


关于(*)的推导:

先把之前推导的预积分再写一遍,看的更清晰:

α b k + 1 b k = ∬ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t 2 \alpha^{b_k}_{b_{k+1}}=\iint_{t\in[t_k,t_{k+1}]}R^{b_k}_t(\hat a_t-b_{a_t}-n_a)dt^2 αbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt2

β b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] R t b k ( a ^ t − b a t − n a ) d t \beta^{b_k}_{b_{k+1}}=\int_{t\in[t_k,t_{k+1}]}R^{b_k}_t(\hat a_t-b_{a_t}-n_a)dt βbk+1bk=t[tk,tk+1]Rtbk(a^tbatna)dt

γ b k + 1 b k = ∫ t ∈ [ t k , t k + 1 ] 1 2 Ω ( w ^ t − b w t − n w ) γ t b k d t \gamma^{b_k}_{b_{k+1}}=\int_{t\in[t_k,t_{k+1}]} \frac{1}{2}\Omega(\hat w_t-b_{w_t}-n_w)\gamma^{b_k}_tdt γbk+1bk=t[tk,tk+1]21Ω(w^tbwtnw)γtbkdt

1)首先对于 δ α ˙ t b k \delta \dot \alpha^{b_k}_t δα˙tbk δ b ˙ a t \delta \dot b_{a_t} δb˙at δ b ˙ w t \delta \dot b_{w_t} δb˙wt,显然根据定义有:

δ α ˙ t b k = α ˙ ^ t b k − α ˙ t b k = β ^ t b k − β t b k = δ β t b k \delta \dot \alpha^{b_k}_t=\hat {\dot \alpha}^{b_k}_t-\dot \alpha^{b_k}_t=\hat \beta^{b_k}_t - \beta^{b_k}_t=\delta \beta^{b_k}_t δα˙tbk=α˙^tbkα˙tbk=β^tbkβtbk=δβtbk

δ b ˙ a t = b ˙ a t − 0 = n b a \delta \dot b_{a_t}= \dot b_{a_t}-0=n_{b_a} δb˙at=b˙at0=nba

δ b ˙ w t = b ˙ w t − 0 = n b w \delta \dot b_{w_t} =\dot b_{w_t}-0= n_{b_w} δb˙wt=b˙wt0=nbw

2)然后是 δ β ˙ t b k \delta \dot \beta^{b_k}_t δβ˙tbk

β ˙ t b k \dot \beta^{b_k}_t β˙tbk的理论值,即不考虑存在噪声 n a n_a na n b a n_{b_a} nba时应为: β ˙ t b k = R t b k ( a ^ t − b a t ) \dot \beta^{b_k}_t=R^{b_k}_t(\hat a_t-b_{a_t}) β˙tbk=Rtbk(a^tbat)

β ˙ t b k \dot \beta^{b_k}_t β˙tbk的实际测量值,即考虑噪声时应为:

β ˙ ^ t b k = R ^ t b k ( a ^ t − b ^ a t − n a ) = R t b k e x p ( [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) \hat{ \dot \beta}^{b_k}_t = \hat R^{b_k}_t(\hat a_t-\hat b_{a_t}-n_a)=R^{b_k}_texp([\delta \theta]_{\times} )(\hat a_t-n_a- b_{a_t}-\delta b_{a_t}) β˙^tbk=R^tbk(a^tb^atna)=Rtbkexp([δθ]×)(a^tnabatδbat)

= R t b k ( 1 + [ δ θ ] × ) ( a ^ t − n a − b a t − δ b a t ) =R^{b_k}_t(1+[\delta \theta]_{\times} )(\hat a_t-n_a- b_{a_t}-\delta b_{a_t}) =Rtbk(1+[δθ]×)(a^tnabatδbat)

= R t b k ( a ^ t − n a − b a t − δ b a t + [ δ θ ] × ( a ^ t − b a t ) ) =R^{b_k}_t(\hat a_t-n_a- b_{a_t}-\delta b_{a_t}+[\delta \theta]_{\times}(\hat a_t- b_{a_t})) =Rtbk(a^tnabatδbat+[δθ]×(a^tbat))

= R t b k ( a ^ t − n a − b a t − δ b a t − [ a ^ t − b a t ] × δ θ ) =R^{b_k}_t(\hat a_t-n_a- b_{a_t}-\delta b_{a_t}-[\hat a_t- b_{a_t}]_{\times}\delta \theta) =Rtbk(a^tnabatδbat[a^tbat]×δθ)

则:

δ β ˙ t b k = β ˙ ^ t b k − β ˙ t b k = − R t b k [ a ^ t − b a t ] × δ θ − R t b k δ b a t − R t b k n a \delta \dot \beta^{b_k}_t = \hat{ \dot \beta}^{b_k}_t-\dot \beta^{b_k}_t=-R^{b_k}_t[\hat a_t- b_{a_t}]_{\times}\delta \theta-R^{b_k}_t \delta b_{a_t} -R^{b_k}_t n_a δβ˙tbk=β˙^tbkβ˙tbk=Rtbk[a^tbat]×δθRtbkδbatRtbkna

证毕。

3)接下来是 δ θ ˙ t b k \delta \dot \theta^{b_k}_t δθ˙tbk,先推个 δ q ˙ t b k \delta \dot q^{b_k}_t δq˙tbk

q ˙ t b k \dot q^{b_k}_t q˙tbk的理论值,即不考虑噪声 n w n_w nw n b w n_{b_w} nbw
q ˙ t b k = 1 2 Ω ( w ^ t − b w t ) q t b k = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] \dot q^{b_k}_t=\frac{1}{2}\Omega(\hat w_t-b_{w_t})q^{b_k}_t=\frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} (\hat w_t-b_{w_t})\\0 \end{matrix} \right] q˙tbk=21Ω(w^tbwt)qtbk=21qtbk[(w^tbwt)0]

q ˙ t b k \dot q^{b_k}_t q˙tbk的真实测量值: q ˙ ^ t b k = 1 2 q ^ t b k ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] = 1 2 q t b k ⊗ δ q t b k ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] \hat {\dot q}^{b_k}_t=\frac{1}{2} \hat q^{b_k}_t \otimes \left[\begin{matrix} \hat w_t-b_{w_t}-n_w-\delta b_{w_t}\\0 \end{matrix} \right] \\ =\frac{1}{2} q^{b_k}_t \otimes \delta q^{b_k}_t \otimes \left[\begin{matrix} \hat w_t-b_{w_t}-n_w-\delta b_{w_t}\\0 \end{matrix} \right] q˙^tbk=21q^tbk[w^tbwtnwδbwt0]=21qtbkδqtbk[w^tbwtnwδbwt0]

根据导数性质,有:
q ˙ ^ t b k = ( q t b k ⊗ ˙ δ q t b k ) = q ˙ t b k ⊗ δ q t b k + q t b k ⊗ δ q t b k ˙ = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q t b k ˙ \hat {\dot q}^{b_k}_t=(q^{b_k}_t \dot \otimes \delta q^{b_k}_t)=\dot q^{b_k}_t \otimes \delta q^{b_k}_t+q^{b_k}_t \otimes \dot{ \delta q^{b_k}_t}=\frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} (\hat w_t-b_{w_t})\\0 \end{matrix} \right]\otimes \delta q^{b_k}_t+q^{b_k}_t \otimes \dot{ \delta q^{b_k}_t} q˙^tbk=(qtbk˙δqtbk)=q˙tbkδqtbk+qtbkδqtbk˙=21qtbk[(w^tbwt)0]δqtbk+qtbkδqtbk˙

即得到等式:

1 2 q t b k ⊗ δ q t b k ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] = 1 2 q t b k ⊗ [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k + q t b k ⊗ δ q t b k ˙ \frac{1}{2} q^{b_k}_t \otimes \delta q^{b_k}_t \otimes \left[\begin{matrix} \hat w_t-b_{w_t}-n_w-\delta b_{w_t}\\0 \end{matrix} \right]=\frac{1}{2}q^{b_k}_t \otimes \left[\begin{matrix} (\hat w_t-b_{w_t})\\0 \end{matrix} \right]\otimes \delta q^{b_k}_t+q^{b_k}_t \otimes \dot{ \delta q^{b_k}_t} 21qtbkδqtbk[w^tbwtnwδbwt0]=21qtbk[(w^tbwt)0]δqtbk+qtbkδqtbk˙

2 δ q t b k ˙ = δ q t b k ⊗ [ w ^ t − b w t − n w − δ b w t 0 ] − [ ( w ^ t − b w t ) 0 ] ⊗ δ q t b k 2 \dot{ \delta q^{b_k}_t} = \delta q^{b_k}_t \otimes \left[\begin{matrix} \hat w_t-b_{w_t}-n_w-\delta b_{w_t}\\0 \end{matrix} \right]-\left[\begin{matrix} (\hat w_t-b_{w_t})\\0 \end{matrix} \right]\otimes \delta q^{b_k}_t 2δqtbk˙=δqtbk[w^tbwtnwδbwt0][(w^tbwt)0]δqtbk

2 δ q t b k ˙ = R ( [ w ^ t − b w t − n w − δ b w t 0 ] ) δ q t b k − L ( [ ( w ^ t − b w t ) 0 ] ) δ q t b k 2 \dot{ \delta q^{b_k}_t} =\mathcal R(\left[\begin{matrix} \hat w_t-b_{w_t}-n_w-\delta b_{w_t}\\0 \end{matrix} \right])\delta q^{b_k}_t-\mathcal L(\left[\begin{matrix} (\hat w_t-b_{w_t})\\0 \end{matrix} \right] ) \delta q^{b_k}_t 2δqtbk˙=R([w^tbwtnwδbwt0])δqtbkL([(w^tbwt)0])δqtbk

2 δ q t b k ˙ = [ δ θ t b k ˙ 0 ] = [ − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × − n w − δ b w t ( n w + δ b w t ) T 0 ] [ 1 2 δ θ t b k 1 ] 2 \dot{ \delta q^{b_k}_t} = \left[\begin{matrix} \dot{\delta \theta^{b_k}_t} \\0 \end{matrix} \right] = \left[\begin{matrix} -[2\hat w_t-2b_{w_t}-n_w-\delta b_{w_t}]_{\times}& -n_w-\delta b_{w_t}\\(n_w+\delta b_{w_t} )^T&0 \end{matrix} \right] \left[\begin{matrix} \frac{1}{2}\delta \theta^{b_k}_t\\1 \end{matrix} \right] 2δqtbk˙=[δθtbk˙0]=[[2w^t2bwtnwδbwt]×(nw+δbwt)Tnwδbwt0][21δθtbk1]

δ θ t b k ˙ = − [ 2 w ^ t − 2 b w t − n w − δ b w t ] × 1 2 δ θ t b k − n w − δ b w t ≈ − [ w ^ t − b w t ] × δ θ t b k − n w − δ b w t \dot{\delta \theta^{b_k}_t}= -[2\hat w_t-2b_{w_t}-n_w-\delta b_{w_t}]_{\times}\frac{1}{2}\delta \theta^{b_k}_t-n_w-\delta b_{w_t}\\\approx-[\hat w_t-b_{w_t}]_{\times}\delta \theta^{b_k}_t-n_w-\delta b_{w_t} δθtbk˙=[2w^t2bwtnwδbwt]×21δθtbknwδbwt[w^tbwt]×δθtbknwδbwt

证毕。

注:这部分的推导完全参考了崔博的论文推导。


那我们继续,上面已经推到了: δ z ˙ t b k = F t δ z t b k + G t n t \delta \dot z^{b_k}_t=F_t\delta z^{b_k}_t+G_tn_t δz˙tbk=Ftδztbk+Gtnt
根据导数定义有:
δ z ˙ t b k = lim ⁡ δ t → 0 δ z t + δ t b k − δ z t b k δ t \delta \dot z^{b_k}_t={\lim_{\delta t \to 0}} \frac {\delta z^{b_k}_{t+\delta t}-\delta z^{b_k}_{t}}{\delta t} δz˙tbk=δt0limδtδzt+δtbkδztbk

即:
δ z t + δ t b k = δ z t b k + δ z ˙ t b k δ t = δ z t b k + ( F t δ z t b k + G t n t ) δ t = ( I + F t δ t ) δ z t b k + G t δ t n t \delta z^{b_k}_{t+\delta t}=\delta z^{b_k}_{t}+\delta \dot z^{b_k}_t {\delta t}=\delta z^{b_k}_{t}+(F_t\delta z^{b_k}_t+G_tn_t) {\delta t}=(I+F_t\delta t)\delta z^{b_k}_{t}+G_t{\delta t}n_t δzt+δtbk=δztbk+δz˙tbkδt=δztbk+(Ftδztbk+Gtnt)δt=(I+Ftδt)δztbk+Gtδtnt

F = ( I + F t δ t ) F=(I+F_t\delta t) F=(I+Ftδt) V = G t δ t V=G_t{\delta t} V=Gtδt,有:
δ z t + δ t b k = F δ z t b k + V n t \delta z^{b_k}_{t+\delta t}=F\delta z^{b_k}_{t}+Vn_t δzt+δtbk=Fδztbk+Vnt

由此我们得到了该非线性系统的线性化递推方程,那么便可以根据当前时刻的值,预测下一时刻的协方差:【论文式(10)】
P t + δ t b k = ( I + F t δ t ) P t b k ( I + F t δ t ) T + ( G t δ t ) Q ( G t δ t ) T P^{b_k}_{t+\delta t}=(I+F_t\delta t)P^{b_k}_{t}(I+F_t\delta t)^T+(G_t{\delta t})Q(G_t{\delta t})^T Pt+δtbk=(I+Ftδt)Ptbk(I+Ftδt)T+(Gtδt)Q(Gtδt)T

其中初始协方差 P b k b k = 0 P^{b_k}_{b_k}=0 Pbkbk=0,Q代表噪声项的对角协方差矩阵:
Q 12 × 12 = [ σ a 2 0 0 0 0 σ w 2 0 0 0 0 σ b a 2 0 0 0 0 σ b w 2 ] Q^{12 \times 12}=\left[\begin{matrix} \sigma^2_a&0&0&0 \\0&\sigma^2_w&0&0\\0&0&\sigma^2_{b_a}&0\\0&0&0&\sigma^2_{b_w} \end{matrix} \right] Q12×12=σa20000σw20000σba20000σbw2

同时也可以根据递推方程得到对应的Jacobian迭代公式:【论文式11】
J t + δ t = ( I + F t δ t ) J t J_{t+\delta t}=(I+F_t\delta t)J_t Jt+δt=(I+Ftδt)Jt

其中初始Jacobian为: J b k = I J_{b_k}=I Jbk=I


离散形式

根据连续形式的增量误差递推方程,同理我们可以推导出离散形式的方程,这里先直接给出结果。这在vins_estimator/src/factor/integration_base.h中的void IntegrationBase::midPointIntegration()函数中得以实现。

[ δ α k + 1 δ θ k + 1 δ β k + 1 δ b a k + 1 δ b w k + 1 ] = [ I f 01 δ t f 03 f 04 0 f 11 0 0 − δ t 0 f 21 I f 23 f 24 0 0 0 I 0 0 0 0 0 I ] [ δ α k δ θ k δ β k δ b a k δ b w k ] + [ v 00 v 01 v 02 v 03 0 0 0 − 1 2 δ t 0 − 1 2 δ t 0 0 − 1 2 R k δ t v 21 − 1 2 R k + 1 δ t v 23 0 0 0 0 0 0 δ t 0 0 0 0 0 0 δ t ] [ n a k n w k n a k + 1 n w k + 1 n b a n b w ] \begin{bmatrix}\delta \alpha_{k+1} \\ \delta\theta_{k+1} \\ \delta \beta_{k+1}\\ \delta b_{a_{k+1}} \\ \delta b_{w_{k+1}} \end{bmatrix} = \begin{bmatrix}I&f_{01}&\delta t&f_{03}&f_{04}\\0&f_{11}&0&0&-\delta t\\0&f_{21}&I&f_{23}&f_{24}\\0&0&0&I&0\\0&0&0&0&I \end{bmatrix} \begin{bmatrix}\delta \alpha_k \\ \delta \theta_k\\ \delta \beta_k \\ \delta b_{a_k} \\ \delta b_{w_k} \end{bmatrix} + \begin{bmatrix} v_{00}&v_{01}&v_{02}&v_{03}&0&0\\ 0&-\frac{1}{2}\delta t&0&-\frac{1}{2}\delta t&0&0\\ -\frac{1}{2}R_k\delta t&v_{21}&-\frac{1}{2}R_{k+1}\delta t&v_{23}&0&0\\ 0&0&0&0&\delta t&0\\ 0&0&0&0&0&\delta t \end{bmatrix} \begin{bmatrix}n_{a_k}\\n_{w_k}\\n_{a_{k+1}}\\n_{w_{k+1}}\\n_{b_a}\\n_{b_w} \end{bmatrix} δαk+1δθk+1