Bootstrap

Vins-mono中的IMU预积分【SLAM】

世界系下连续时间的IMU积分

w w w代表世界系, b k b_{k} bk代表第k帧图像。
[ t k , t k + 1 ] [t_{k}, t_{k+1}] [tk,tk+1]时间段内,有通过加速度和角速度在连续时间下的积分:
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 Ω ( ω ^ t − b ω t − n ω ) q t b k d t q^{w}_{b_{k+1}} = q^{w}_{b_{k}} \otimes \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{\omega}_{t}-b_{\omega_{t}}-n_{\omega})q^{b_{k}}_{t} dt qbk+1w=qbkwt[tk,tk+1]21Ω(ω^tbωtnω)qtbkdt

其中 Δ t k = t k + 1 − t k \Delta t_{k} = t_{k+1} - t_{k} Δtk=tk+1tk, Ω ( ⋅ ) \Omega (\cdot) Ω()是四元数乘法右乘矩阵的虚数部分, Ω ( ω ) = [ − [ ω ] × ω − ω T 0 ] \Omega(\omega) = \begin{bmatrix} -[\omega]_{\times} & \omega \\ -\omega^{T} & 0 \end{bmatrix} Ω(ω)=[[ω]×ωTω0]

IMU预积分的形成

对上一部分的三个积分公式转换坐标系就可以形成预积分的定义式:
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
其中 α 、 β 、 γ \alpha、\beta、\gamma αβγ就是IMU预积分量。
α 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 Ω ( ω ^ t − b ω t − n ω ) γ t b k d t \gamma^{b_{k}}_{b_{k+1}} = \int_{t \in [t_{k}, t_{k+1}]} \frac{1}{2} \Omega(\hat{\omega}_{t}-b_{\omega_{t}}-n_{\omega})\gamma^{b_{k}}_{t} dt γbk+1bk=t[tk,tk+1]21Ω(ω^tbωtnω)γtbkdt
可以看出预积分量只和状态量中的零偏有关,如果零偏更新了,就用预积分对零偏的一阶雅可比更新预积分。

离散时间下的IMU预积分递推公式

实际IMU信息是离散的,所以要得到离散时间下的IMU预积分公式,可以用Euler、中值积分、RK4进行数值积分求IMU预积分。
离散时间下IMU预积分量的递推公式为:
α ^ 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 ⊗ [ 1 1 2 ( ω ^ i − b ω i ) δ t ] \hat{\gamma}^{b_{k}}_{i+1} = \hat{\gamma}^{b_{k}}_{i} \otimes \begin{bmatrix} 1 \\ \frac{1}{2}(\hat{\omega}_{i} - b_{\omega_{i}})\delta t \end{bmatrix} γ^i+1bk=γ^ibk[121(ω^ibωi)δt]
其中 i i i表示第 i i i帧imu信息。
如果用中值积分方法,预积分更新顺序为 γ 、 α 、 β \gamma、\alpha、\beta γαβ

误差卡尔曼(ESKF)求预积分量的协方差矩阵

误差卡尔曼中状态量为包含 α 、 β 、 θ 、 b a t 、 b ω t \alpha、\beta、\theta、b_{{a}_{t}}、b_{{\omega}_{t}} αβθbatbωt共15维的状态变量。其中旋转姿态用 θ \theta θ而不再用 γ \gamma γ表示,这样做的原因是四元数表示旋转本身是过参数化的,另外用旋转向量表示求雅可比矩阵方便一些,符合平常的计算习惯,平常不用旋转向量的原因是旋转向量具有周期性,但是在误差卡尔曼中不存在周期性这个问题,误差都是小量。

于是按照误差卡尔曼的推导方式可以得到连续时间下误差状态量的导数公式:
δ x ˙ t b k = [ δ α ˙ t b k δ β ˙ t b k δ θ ˙ t b k δ b ˙ a t δ b ˙ ω t ] = [ 0 I 0 0 0 0 0 − R t b k [ a ^ t − b a t ] × − R t b k 0 0 0 − [ ω ^ − b ω 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 ω 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 ω n b a n b ω ] = F t δ x t b k + G t n t \delta \dot{x}^{b_{k}}_{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}_{\omega_{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{\omega} - b_{\omega_{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_{\omega_{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_{\omega} \\ n_{b_{a}} \\ n_{b_{\omega}} \end{bmatrix} = F_{t}\delta x^{b_{k}}_{t} + G_{t} n_{t} δx˙tbk= δα˙tbkδβ˙tbkδθ˙tbkδb˙atδb˙ωt = 00000I00000Rtbk[a^tbat]×[ω^bωt]×000Rtbk00000I00 δαtbkδβtbkδθtbkδbatδbωt + 0Rtbk00000I00000I00000I nanωnbanbω =Ftδxtbk+Gtnt
通过这个连续时间的导数公式,可以推出离散时间下的误差状态变量的递推公式
δ x i + 1 b k = δ x i b k + ( F t δ x i b k + G t n t ) δ t = ( I + F t δ t ) δ x k + G t δ t ⋅ n t \delta x^{b_{k}}_{i+1} = \delta x^{b_{k}}_{i} + (F_{t} \delta x^{b_{k}}_{i} + G_{t}n_{t})\delta t = (I+F_{t} \delta t)\delta x_{k} + G_{t} \delta t \cdot n_{t} δxi+1bk=δxibk+(Ftδxibk+Gtnt)δt=(I+Ftδt)δxk+Gtδtnt
注意!这里离散化用的是中值积分,所以 n t n_{t} nt变成了18维的向量,包含了 n a i 、 n ω i 、 n a i + 1 、 n ω i + 1 、 n b a 、 n b ω n_{a_{i}}、n_{\omega_{i}}、n_{a_{i+1}}、n_{\omega_{i+1}}、n_{b_{a}}、n_{b_{\omega}} nainωinai+1nωi+1nbanbω
最终结果太复杂了,直接截图,下面图片是cvlife课程的资料:
在这里插入图片描述
图片中和本篇博客差别在于图片中的 k k k表示第 k k k帧imu信息,本篇博客为了区别图像帧和imu帧用了 i i i表示第 i i i帧imu信息。
为了方便说明,将这个大公式写作:
δ x i + 1 = F δ x i + V n \delta x_{i+1} = F \delta x_{i} + V n δxi+1=Fδxi+Vn

重要! 误差状态量和实际状态量雅可比矩阵的关系

我们上面推出的是误差状态量 δ x = [ δ α δ β δ θ δ b a δ b ω ] \delta x = \begin{bmatrix} \delta \alpha \\ \delta \beta \\ \delta \theta \\ \delta b_{a} \\ \delta b_{\omega} \end{bmatrix} δx= δαδβδθδbaδbω 的递推公式,实际上我们也比较关心状态量 x = [ α β θ b a b ω ] x = \begin{bmatrix} \alpha \\ \beta \\ \theta \\ b_{a} \\ b_{\omega} \end{bmatrix} x= αβθbabω 对状态量 x x x的雅可比矩阵,其实二者有很大的关系。

已知状态量的递推关系:
x i + 1 t = f ( x i t ) x_{i+1}^{t} = f(x_{i}^{t}) xi+1t=f(xit)
这里上标 t t t表示实际状态变量,区分于名义状态变量。
按照误差卡尔曼的方式写成名义状态变量和误差状态变量,有:
x i + 1 + δ x i + 1 = f ( x i + δ x i ) x_{i+1} + \delta x_{i+1} = f(x_{i} + \delta x_{i}) xi+1+δxi+1=f(xi+δxi)

然后把 f ( ⋅ ) f(\cdot) f()一阶泰勒展开:
x i + 1 + δ x i + 1 = f ( x i ) + J δ x i x_{i+1} + \delta x_{i+1} = f(x_{i}) + J\delta x_{i} xi+1+δxi+1=f(xi)+Jδxi
其中, J = ∂ x i + 1 t ∂ x i J = \frac{\partial x_{i+1}^{t}}{\partial x_{i}} J=xixi+1t

又因为名义状态变量满足:
x i + 1 = f ( x i ) x_{i+1} = f(x_{i}) xi+1=f(xi)
所以有:
δ x i + 1 = J δ x i \delta x_{i+1} = J \delta x_{i} δxi+1=Jδxi
也就是说,上一部分中误差状态变量递推公式的大F矩阵就是这里实际状态变量对i时刻名义状态变量的雅可比矩阵,即:
F = J = ∂ x i + 1 t ∂ x i F = J = \frac{\partial x_{i+1}^{t}}{\partial x_{i}} F=J=xixi+1t

同时这里还有一点, x i + 1 t = x i + 1 + δ x i + 1 x_{i+1}^{t} = x_{i+1} + \delta x_{i+1} xi+1t=xi+1+δxi+1对x_{i}求偏导,其中 x i + 1 = f ( x i ) x_{i+1} = f(x_{i}) xi+1=f(xi),而 δ x i + 1 \delta x_{i + 1} δxi+1 x i x_{i} xi无关,所以 F = J = ∂ x i + 1 ∂ x i F=J= \frac{\partial x_{i+1}}{\partial x_{i}} F=J=xixi+1

然后考虑如果要求 i + 1 i+1 i+1时刻实际状态变量对初始时刻的雅可比矩阵,不妨设 J i = ∂ x i ∂ x J_{i} = \frac{\partial x_{i}}{\partial x} Ji=xxi x x x代表本次预积分初始时刻的名义状态变量,然后有:
J i + 1 = F J i J_{i+1} = F J_{i} Ji+1=FJi

维护这个雅可比矩阵的主要作用是,其中包含了实际预积分量对零偏的雅可比矩阵,当零偏更新时,可以利用这个雅可比矩阵更新预积分量。

δ x i + 1 = F δ x i + V n \delta x_{i+1} = F \delta x_{i} + V n δxi+1=Fδxi+Vn可知误差状态变量的协方差矩阵更新方式:
P i + 1 = F P i F T + V Q V T P_{i+1} = FP_{i}F^{T} + VQV^{T} Pi+1=FPiFT+VQVT
其中, P i P_{i} Pi δ x i \delta x_{i} δxi的协方差矩阵, Q Q Q n n n的协方差矩阵。
由于实际状态变量和误差状态变量的关系为:
x i t = x i + δ x i x_{i}^{t} = x_{i} + \delta x_{i} xit=xi+δxi
其中名义状态变量协方差矩阵为0,所以实际状态变量的协方差矩阵等于误差状态变量的协方差矩阵。

;