深蓝学院从零开始手写VIO(一)——IMU模型

深蓝学院从零开始手写VIO(一)——IMU模型

  • 旋转运动学
    • 速度合成公式
    • 加速度合成公式
  • IMU测量原理
    • 加速度计
    • 陀螺仪
  • IMU误差模型
    • 确定性误差
    • 随机误差
    • 离散量测下的误差模型
      • 高斯白噪声的离散积分
      • 零偏的离散积分
    • Allan方差标定
  • IMU误差的数学模型

声明:本专栏文章为深蓝学院《从零开始手写VIO》课程个人学习笔记,更多学习资源请咨询深蓝学院相关课程。

旋转运动学

速度合成公式

深蓝学院从零开始手写VIO(一)——IMU模型_第1张图片
假设有如上图所示的一静一动两个坐标系,按照习惯,我们记动坐标系为体系 b \bm{b} b,静坐标系为惯性系 i \bm{i} i。此时有一个物体在空间运动,记其在两个坐标系下的坐标分别为 r i \bm{r}^i ri r b \bm{r}^b rb,两者之间的关系为:
r i = r i b i + R i b r b \bm{r}^i=\bm{r}_{ib}^i+\bm{R}_{ib}\bm{r}^b ri=ribi+Ribrb
式中上标表示矢量所在的坐标系, r i b i \bm{r}_{ib}^i ribi表示 b \bm{b} b系到 i \bm{i} i系的平移变换矢量, R i b \bm{R}_{ib} Rib表示 b \bm{b} b系到 i \bm{i} i系的旋转变换矩阵。对上式求解时间导数有:
d r i d t = d ( r i b i + R i b r b ) d t = d r i b d t + d R i b d t r b + R i b d r b d t = v i b i + R i b [ ω i b b ] × r b + R i b v b \begin{aligned} \frac{d\bm{r}^i}{dt}&=\frac{d(\bm{r}_{ib}^i+\bm{R}_{ib}\bm{r}^b)}{dt}\\ &=\frac{d\bm{r}_{ib}}{dt}+\frac{d\bm{R}_{ib}}{dt}\bm{r}^b+\bm{R}_{ib}\frac{d\bm{r}^b}{dt}\\ &=\bm{v}_{ib}^i+\bm{R}_{ib}[\bm{\omega}_{ib}^b]_\times\bm{r}^b+\bm{R}_{ib}\bm{v}^b\end{aligned} dtdri=dtd(ribi+Ribrb)=dtdrib+dtdRibrb+Ribdtdrb=vibi+Rib[ωibb]×rb+Ribvb

这里我们引入矢量叉积的一个重要几何性质:
定理1:矢量叉积的旋转不变性。若矩阵 R \bm{R} R为旋转矩阵,则对于矢量叉积 a × b \bm{a}\times\bm{b} a×b ( R a ) × ( R b ) = R ( a × b ) (\bm{Ra})\times(\bm{Rb})=\bm{R}(\bm{a}\times\bm{b}) (Ra)×(Rb)=R(a×b)
利用这一性质,上式可以重写为:
d r i d t = v i b i + R i b [ ω i b b ] × r b + R i b v b = v i b i + [ R i b ω i b b ] × [ R i b r b ] + R i b v b v i = v i b i + ω i b i × r i + R i b v b (1) \begin{aligned} \frac{d\bm{r}^i}{dt}&=\bm{v}_{ib}^i+\bm{R}_{ib}[\bm{\omega}_{ib}^b]_\times\bm{r}^b+\bm{R}_{ib}\bm{v}^b\\ &=\bm{v}_{ib}^i+[\bm{R}_{ib}\bm{\omega}_{ib}^b]_\times[\bm{R}_{ib}\bm{r}^b]+\bm{R}_{ib}\bm{v}^b\\ \bm{v}^i&=\bm{v}_{ib}^i+\bm{\omega}_{ib}^i\times\bm{r}^i+\bm{R}_{ib}\bm{v}^b \end{aligned} \tag{1} dtdrivi=vibi+Rib[ωibb]×rb+Ribvb=vibi+[Ribωibb]×[Ribrb]+Ribvb=vibi+ωibi×ri+Ribvb(1)

式(1)描述了物体的速度在动坐标系和静坐标系之间的转换关系,又称为速度合成公式科里奥利方程。由于坐标系原点之间的位移矢量与旋转是相互独立,互不相应的,因此在后续讨论中,我们将省略式(1)中的 v i b i \bm{v}_{ib}^i vibi项,只讨论旋转相关部分。

加速度合成公式

进一步地,我们对式(1)量测继续求时间导数有:
d v i d t = d ( ω i b i × r i ) d t + d ( R i b v b ) d t = d ω i b i d t × r i + ω i b i × d r i d t + d R i b d t v b + R i b d v b d t a i = ω ˙ i b i × r i + ω i b i × v i + ω i b i × r i + R i b a b \begin{aligned} \frac{d\bm{v}^i}{dt}&=\frac{d(\bm{\omega}_{ib}^i\times\bm{r}^i)}{dt}+\frac{d(\bm{R}_{ib}\bm{v}^b)}{dt}\\ &=\frac{d\bm{\omega}_{ib}^i}{dt}\times\bm{r}^i+\bm{\omega}_{ib}^i\times\frac{d\bm{r}^i}{dt}+\frac{d\bm{R}_{ib}}{dt}\bm{v}^b+\bm{R}_{ib}\frac{d\bm{v}^b}{dt}\\ \bm{a}^i&=\dot{\bm{\omega}}_{ib}^i\times\bm{r}^i+\bm{\omega}_{ib}^i\times\bm{v}^i+\bm{\omega}_{ib}^i\times\bm{r}^i+\bm{R}_{ib}\bm{a}^b \end{aligned} dtdviai=dtd(ωibi×ri)+dtd(Ribvb)=dtdωibi×ri+ωibi×dtdri+dtdRibvb+Ribdtdvb=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab
将式(1)带入上式有:
a i = ω ˙ i b i × r i + ω i b i × v i + ω i b i × r i + R i b a b = ω ˙ i b i × r i + ω i b i × ( ω i b i × r i + R i b v b ) + ω i b i × r i + R i b a b = ω ˙ i b i × r i ⏟ E u l e r + ω i b i × ( ω i b i × r i ) ⏟ C e n t r i f u g a l + 2 ω i b i × r i ⏟ C o r i o l i s + R i b a b (2) \begin{aligned} \bm{a}^i&=\dot{\bm{\omega}}_{ib}^i\times\bm{r}^i+\bm{\omega}_{ib}^i\times\bm{v}^i+\bm{\omega}_{ib}^i\times\bm{r}^i+\bm{R}_{ib}\bm{a}^b\\ &=\dot{\bm{\omega}}_{ib}^i\times\bm{r}^i+\bm{\omega}_{ib}^i\times(\bm{\omega}_{ib}^i\times\bm{r}^i+\bm{R}_{ib}\bm{v}^b)+\bm{\omega}_{ib}^i\times\bm{r}^i+\bm{R}_{ib}\bm{a}^b\\ &=\underbrace{\dot{\bm{\omega}}_{ib}^i\times\bm{r}^i}_{Euler}+\underbrace{\bm{\omega}_{ib}^i\times(\bm{\omega}_{ib}^i\times\bm{r}^i)}_{Centrifugal}+\underbrace{2\bm{\omega}_{ib}^i\times\bm{r}^i}_{Coriolis}+\bm{R}_{ib}\bm{a}^b\\ \end{aligned} \tag{2} ai=ω˙ibi×ri+ωibi×vi+ωibi×ri+Ribab=ω˙ibi×ri+ωibi×(ωibi×ri+Ribvb)+ωibi×ri+Ribab=Euler ω˙ibi×ri+Centrifugal ωibi×(ωibi×ri)+Coriolis 2ωibi×ri+Ribab(2)
式(2)前三项依次为欧拉力离心力科氏力,该式表征了运动物体在静坐标系和动坐标系下的加速度变换关系,称为加速度合成公式

IMU测量原理

加速度计

深蓝学院从零开始手写VIO(一)——IMU模型_第2张图片单个加速度计的测量原理如上图所示,可以认为是一种弹簧振子模型,对于三轴IMU来说,其内部的三个加速度计正交安装。由于在地面运动的物体处在地球的重力场中,如果我们假设加速度计所在的测量坐标系和当地地理坐标系重合,那么当加速度计静止时,弹簧振子受重力影响,在重力垂线方向的输出并不为零。而是重力矢量 g \bm{g} g,即:
a = g = [ 0 , 0 , − g ] \bm{a}=\bm{g}=[0,0,-g] a=g=[0,0,g]

陀螺仪

陀螺仪的测量原理包振动陀螺、光纤陀螺、冷原子陀螺等,在低端的MEMS通常采用的为震动陀螺。震动陀螺的本质是通过测量式(2)中Coriolis力 f c = 2 ω × v \bm{f}_c=2\bm{\omega}\times\bm{v} fc=2ω×v,进而利用Coriolis力 f c \bm{f}_c fc和线速度 v \bm{v} v求解出角速度 ω \bm{\omega} ω
深蓝学院从零开始手写VIO(一)——IMU模型_第3张图片
上图为MEMS中所采用的典型音叉振动陀螺的原理图,根据音叉的特性,左右音叉臂上的质量元永远做同速反向振动,此时若音叉以角速度 ω \bm{\omega} ω做旋转运动,同时音叉还以外部加速度 f a \bm{f}_a fa运动,将两个音叉的测量量相减可以消去 f a \bm{f}_a fa的影响而只留下由于角运动而产生的科氏力 f a \bm{f}_a fa,最终我们可以得到:
2 f a = 4 ω × v 2\bm{f}_a=4\bm{\omega}\times\bm{v} 2fa=4ω×v
当然上述假设是在音叉的左右质量元质量完全相等的前提下得到的,在实际应用中,左右质量元不可避免的会存在质量差,从而导致外部加速度引起的力 f a \bm{f}_a fa无法完全消除,这一由外部加速度残差引起的角速度误差的比力称为该陀螺的G-敏感性(G-Sentivity)

IMU误差模型

IMU传感器的误差主要有确定性误差和随机误差两部分组成,其模型和标定方法如下。

确定性误差

在MEMS IMU传感器中,我们需要考虑的确定性误差主要包括以下三项

  • 零偏(Bias):IMU传感器在完全静止状态下存在的非零输出值,属于加性误差;
  • 尺度因子(Scale): IMU传感器自身尺度不准引起的测量误差,属于乘性误差;
  • 不垂直度(Nonorothogonality):IMU传感器中由于各轴安装不垂直引起的误差,属于乘性误差。

上述三个确定性误差通常可以表示为统一的线性代数形式(矩阵中对角线为尺度因子,其余为不垂直度),以加速度计为例:
[ a x ′ a y ′ a z ′ ] = [ s x x m x y m x z m y x s y y s y z m z x m z y s z z ] [ a x a y a z ] + [ b a x b a y b a z ] (3) \left[\begin{array}{c}a_x'\\ a_y'\\ a_z'\end{array}\right]=\begin{bmatrix} s_{xx} & m_{xy} & m_{xz}\\ m_{yx} & s_{yy} & s_{yz}\\ m_{zx} & m_{zy} & s_{zz} \end{bmatrix} \left[\begin{array}{c}a_x\\ a_y\\ a_z\end{array}\right]+\left[\begin{array}{c}b_{ax}\\ b_{ay}\\ b_{az}\end{array}\right] \tag{3} axayaz=sxxmyxmzxmxysyymzymxzsyzszzaxayaz+baxbaybaz(3)
确定性误差的标定通常采用六面法,即将三轴传感器的分别按照正反轴方向放置(对于加速度计为朝上和朝下,参考值为重力矢量;对于陀螺仪为正转和反转,参考值由标定转台提供),并将测量值带入式(4)中进行最小二乘求解。

此外,温度误差也属于确定性误差,可采用soak或ramp方法标定,这里略过。

随机误差

IMU器件的随机误差主要包括高斯白噪声和随机游走两部分:

  • 高斯白噪声:均值为 0 0 0,方差为 σ \sigma σ的独立高斯过程,且该高斯过程的任意两个时刻之间相互独立。
  • 随机游走:实习器件中的零偏并不是固定不变的,而是符合维纳过程(wiener process),这使得零偏的导数符合高斯白噪声过程,即:
    b ˙ ( t ) = σ b w ( t ) \dot{\bm{b}}(t)=\bm{\sigma}_b\bm{w}(t) b˙(t)=σbw(t)

离散量测下的误差模型

上述误差模型是在连续时间下给出的,而实际器件的量测是离散的,因此需要推导离散时间下的IMU误差模型。按照我们上面给出的误差模型,一个单轴陀螺的量测如下:
ω ~ ( t ) = ω ( t ) + b ( t ) + n ( t ) b ˙ ( t ) = n b ( t ) \begin{aligned} \tilde{\omega}(t)&=\omega(t)+b(t)+n(t)\\ \dot{b}(t)&=n_b(t) \end{aligned} ω~(t)b˙(t)=ω(t)+b(t)+n(t)=nb(t)

式中 n ( t ) \bm{n}(t) n(t) n b ( t ) \bm{n}_b(t) nb(t)均为高斯白噪声。实际传感器的信号采集是通过对一定时间内的信号求平均获得的,因此我们可以假设在信号采集时间内,真实的信号 w ( t ) \bm{w}(t) w(t)保持不变,而只有零偏和噪声在变化,即:
w ~ ( t 0 + Δ t ) = w ( t 0 ) + 1 Δ t ∫ t 0 t 0 + Δ t [ b ( t ) + n ( t ) ] d t (4) \tilde{w}(t_0+\Delta t)=w(t_0)+\frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}[b(t)+n(t)]dt \tag{4} w~(t0+Δt)=w(t0)+Δt1t0t0+Δt[b(t)+n(t)]dt(4)
只要知道高斯白噪声和零偏的离散分布即可求得IMU在离散量测下的误差模型,这里我们先看高斯白噪声。

高斯白噪声的离散积分

对于高斯白噪声 n ( t ) \bm{n}(t) n(t)来说,根据式(4),其在 Δ t \Delta t Δt时间内的值为:
n d [ k ] ≜ n ( t 0 + Δ t ) = 1 Δ t ∫ t 0 t 0 + Δ t n ( t ) d t n_d[k]\triangleq n(t_0+\Delta t)=\frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}n(t)dt nd[k]n(t0+Δt)=Δt1t0t0+Δtn(t)dt
由于 n d [ k ] n_d[k] nd[k]的均值为零,我们可以求解离散情况下 n d [ k ] n_d[k] nd[k]的方差为:
V a r [ n d [ k ] ] = E [ ( n d [ k ] − E ( n d [ k ] ) ( n d [ k ] − E ( n d [ k ] ) ) T ] = E [ n d [ k ] 2 ] = E [ 1 Δ t 2 ∫ t 0 t 0 + Δ t ∫ t 0 t 0 + Δ t n ( τ ) n ( t ) d τ d t ] = E [ 1 Δ t 2 ∫ t 0 t 0 + Δ t n ( τ ) d τ ∫ t 0 t 0 + Δ t n ( t ) d t ] \begin{aligned} Var[n_d[k]]&=E[(n_d[k]-E(n_d[k])(n_d[k]-E(n_d[k]))^T]\\ &=E[n_d[k]^2]\\ &=E\left[\frac{1}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}\int_{t_0}^{t_0+\Delta t}n(\tau)n(t)d\tau dt\right]\\ &=E\left[\frac{1}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}n(\tau)d\tau\int_{t_0}^{t_0+\Delta t}n(t) dt\right] \end{aligned} Var[nd[k]]=E[(nd[k]E(nd[k])(nd[k]E(nd[k]))T]=E[nd[k]2]=E[Δt21t0t0+Δtt0t0+Δtn(τ)n(t)dτdt]=E[Δt21t0t0+Δtn(τ)dτt0t0+Δtn(t)dt]

由在离散条件下,连续积分可以写为有限状态的累加和形式,即:
∫ t 0 t 0 + Δ t n ( t ) d t = n ( t 0 ) d t + n ( t 0 + d t ) d t + ⋯ + n ( t 0 + k d t ) d t = ∑ i = 0 k n ( t 0 + i d t ) d t \begin{aligned} \int_{t_0}^{t_0+\Delta t}n(t) dt&=n(t_0)dt+n(t_0+dt)dt+\cdots+n(t_0+kdt)dt\\ &=\sum\limits_{i=0}^kn(t_0+idt)dt \end{aligned} t0t0+Δtn(t)dt=n(t0)dt+n(t0+dt)dt++n(t0+kdt)dt=i=0kn(t0+idt)dt

带入 V a r ( n d [ k ] ) Var(n_d[k]) Var(nd[k])的表达式中有:
V a r ( n d [ k ] ) = E [ 1 Δ t 2 ∫ t 0 t 0 + Δ t n ( τ ) d τ ∫ t 0 t 0 + Δ t n ( t ) d t ] = 1 Δ t 2 E [ ∑ i = 0 k n ( t 0 + i d τ ) d τ ∑ i = 0 k n ( t 0 + i d t ) d t ] \begin{aligned} Var(n_d[k])&=E\left[\frac{1}{\Delta t^2}\int_{t_0}^{t_0+\Delta t}n(\tau)d\tau\int_{t_0}^{t_0+\Delta t}n(t) dt\right]\\ &=\frac{1}{\Delta t^2}E\left[\sum\limits_{i=0}^kn(t_0+id\tau)d\tau\sum\limits_{i=0}^kn(t_0+idt)dt\right] \end{aligned} Var(nd[k])=E[Δt21t0t0+Δtn(τ)dτt0t0+Δtn(t)dt]=Δt21E[i=0kn(t0+idτ)dτi=0kn(t0+idt)dt]

由于高斯白噪声之间是不相关的,即:
E [ n ( τ ) n ( t ) ] = { σ n 2 , τ = t 0 , τ ≠ t E[n(\tau)n(t)]=\left\{\begin{aligned} &\sigma^2_n,\quad\tau=t\\ &0,\quad\tau\ne t \end{aligned}\right. E[n(τ)n(t)]={σn2,τ=t0,τ=t

因此有:
V a r [ n d [ k ] ] = 1 Δ t 2 E [ ∑ i = 0 k n ( t 0 + i d τ ) d τ ∑ i = 0 k n ( t 0 + i d t ) d t ] = σ n 2 Δ t Δ t 2 = σ n 2 Δ t \begin{aligned} Var[n_d[k]]&= \frac{1}{\Delta t^2}E\left[\sum\limits_{i=0}^kn(t_0+id\tau)d\tau\sum\limits_{i=0}^kn(t_0+idt)dt\right]\\ &=\frac{\sigma^2_n\Delta t}{\Delta t^2}=\frac{\sigma^2_n}{\Delta t} \end{aligned} Var[nd[k]]=Δt21E[i=0kn(t0+idτ)dτi=0kn(t0+idt)dt]=Δt2σn2Δt=Δtσn2
因此我们可以获得离散状态下的高斯白噪声服从如下分布:
n d [ k ] ∼ N ( 0 , σ n 2 Δ t ) (5) n_d[k]\sim N(0,\frac{\sigma_n^2}{\Delta t}) \tag{5} nd[k]N(0,Δtσn2)(5)

零偏的离散积分

按照同样的方法,我们现在来看离散状态下的零偏积分方差。首先我们已知零偏的时间导数是一个高斯白噪声 b ˙ ( t ) = n b ( t ) \dot{b}(t)=n_b(t) b˙(t)=nb(t),将式(4)中的零偏积分项取出有:
1 Δ t ∫ t 0 t 0 + Δ t b ( t ) d t = 1 Δ t ∫ t 0 t 0 + Δ t ( b ( t 0 ) + ∫ t 0 t 0 + Δ t n b ( τ ) d τ ) d t = ( b ( t 0 ) + ∫ t 0 t 0 + Δ t n b ( τ ) d τ ) Δ t ∫ t 0 t 0 + Δ t 1 d t b ( t 0 + Δ t ) = b ( t 0 ) + ∫ t 0 t 0 + Δ t n b ( τ ) d τ \begin{aligned} \frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}b(t)dt&=\frac{1}{\Delta t}\int_{t_0}^{t_0+\Delta t}\left(b(t_0)+\int_{t_0}^{t_0+\Delta t}n_b(\tau)d\tau\right)dt\\ &=\frac{\left(b(t_0)+\int_{t_0}^{t_0+\Delta t}n_b(\tau)d\tau\right)}{\Delta t}\int_{t_0}^{t_0+\Delta t}1dt\\ b(t_0+\Delta t)&=b(t_0)+\int_{t_0}^{t_0+\Delta t}n_b(\tau)d\tau \end{aligned} Δt1t0t0+Δtb(t)dtb(t0+Δt)=Δt1t0t0+Δt(b(t0)+t0t0+Δtnb(τ)dτ)dt=Δt(b(t0)+t0t0+Δtnb(τ)dτ)t0t0+Δt1dt=b(t0)+t0t0+Δtnb(τ)dτ

该积分项的期望和方差分别为:
E [ b ( t 0 + Δ t ) ] = E [ b ( t 0 ) + ∫ t 0 t 0 + Δ t n b ( τ ) d τ ] = E [ b ( t 0 ) ] + ∫ t 0 t 0 + Δ t E [ n b ( τ ) ] d τ = b ( t 0 ) V a r [ b ( t 0 + Δ t ) ] = E [ ( b ( t 0 + Δ t ) − E [ b ( t 0 + Δ t ) ] ) ( b ( t 0 + Δ t ) − E [ b ( t 0 + Δ t ) ] ) T ] = E [ ∫ t 0 t 0 + Δ t n b ( τ ) d τ ∫ t 0 t 0 + Δ t n b ( t ) d t ] = Δ t σ b 2 \begin{aligned} E[b(t_0+\Delta t)]&=E[b(t_0)+\int_{t_0}^{t_0+\Delta t}n_b(\tau)d\tau]\\ &=E[b(t_0)]+\int_{t_0}^{t_0+\Delta t}E[n_b(\tau)]d\tau\\ &=b(t_0)\\ Var[b(t_0+\Delta t)]&=E\left[(b(t_0+\Delta t)-E[b(t_0+\Delta t)])(b(t_0+\Delta t)-E[b(t_0+\Delta t)])^T\right]\\ &=E\left[\int_{t_0}^{t_0+\Delta t}n_b(\tau)d\tau\int_{t_0}^{t_0+\Delta t}n_b(t)dt\right]\\ &=\Delta t\sigma_b^2 \end{aligned} E[b(t0+Δt)]Var[b(t0+Δt)]=E[b(t0)+t0t0+Δtnb(τ)dτ]=E[b(t0)]+t0t0+ΔtE[nb(τ)]dτ=b(t0)=E[(b(t0+Δt)E[b(t0+Δt)])(b(t0+Δt)E[b(t0+Δt)])T]=E[t0t0+Δtnb(τ)dτt0t0+Δtnb(t)dt]=Δtσb2
因此我们可以获得离散状态下的零偏符合如下分布:
b d [ k ] ∼ N ( b d [ k − 1 ] , Δ t σ b 2 ) (6) b_d[k]\sim N(b_d[k-1],\Delta t\sigma_b^2) \tag{6} bd[k]N(bd[k1],Δtσb2)(6)

Allan方差标定

参考以下两个IMU Allan方差标定工具:

  • imu_utils
  • kalibar_allan

IMU误差的数学模型

在连续时间下,IMU的量测微分方程为:
{ p ˙ w = v w v ˙ w = a w q ˙ w b = q w b ⊗ [ 0 1 2 ω w b b ] \left\{\begin{aligned} \dot{\bm{p}}^w&=\bm{v}^w\\ \dot{\bm{v}}^w&=\bm{a}^w\\ \dot{\bm{q}}_{wb}&=\bm{q}_{wb}\otimes\left[\begin{array}{c}0\\ \frac{1}{2}\bm{\omega}^b_{wb}\end{array}\right] \end{aligned}\right. p˙wv˙wq˙wb=vw=aw=qwb[021ωwbb]

同样在实际应用中,我们需要使用其离散形式,即:
{ p { k + 1 } w = p { k + 1 } w + v { k } w Δ t + 1 2 a { k } w Δ t 2 v { k + 1 } w = v { k } w + a { k } w Δ t q w b { k + 1 } = q w b { k } ⊗ [ 1 1 2 ω w b b Δ t ] \left\{\begin{aligned} \bm{p}^w_{\{k+1\}}&=\bm{p}^w_{\{k+1\}}+\bm{v}^w_{\{k\}}\Delta t+\frac{1}{2}\bm{a}^w_{\{k\}}\Delta t^2\\ \bm{v}^w_{\{k+1\}}&=\bm{v}^w_{\{k\}}+\bm{a}^w_{\{k\}}\Delta t\\ \bm{q}_{wb\{k+1\}}&=\bm{q}_{wb\{k\}}\otimes\left[\begin{array}{c}1\\ \frac{1}{2}\bm{\omega}^b_{wb}\Delta t\end{array}\right] \end{aligned}\right. p{k+1}wv{k+1}wqwb{k+1}=p{k+1}w+v{k}wΔt+21a{k}wΔt2=v{k}w+a{k}wΔt=qwb{k}[121ωwbbΔt]

其中四元数的离散更新用到了一阶毕卡近似算法。

你可能感兴趣的:(从零开始手写VIO)