IMU模型
IMU测量值包括加速度计得到的线加速度
a
t
^
\hat{a_t}
at^和陀螺仪得到的角速度
w
t
^
\hat{w_t}
wt^.
a
t
^
=
a
t
+
b
a
t
+
R
w
t
g
w
+
n
a
w
t
^
=
w
t
+
b
w
t
+
n
w
\hat{a_t}=a_t+{b_a}_t+R_w^tg^w+n_a\\ \hat{w_t}=w_t+{b_w}_t+n_w
at^=at+bat+Rwtgw+nawt^=wt+bwt+nw
下标
t
t
t表示在IMU坐标系下,
a
t
,
w
t
a_t,w_t
at,wt表示加速度角速度真实值,
a
t
^
,
w
t
^
\hat{a_t},\hat{w_t}
at^,wt^分别代表在噪声和偏置影响下的imu加速度角速度测量值,
b
a
b_a
ba是加速度偏置,
b
w
b_w
bw是陀螺仪偏置,附加噪声为
n
a
,
n
w
n_a,n_w
na,nw.
假设附加噪声为高斯噪声:
加速度计偏置和陀螺仪偏置被建模为随机游走,其导数为高斯性的:
对于图像帧k和k+1,对应imu坐标系
b
k
和
b
k
+
1
b_k和b_{k+1}
bk和bk+1,位置、速度和方向状态值PVQ可以根据
[
t
k
,
t
k
+
1
]
[t_k,t_{k+1}]
[tk,tk+1]的IMU测量值进行积分得到,如下公式不难理解,是直接由IMU测量模型得到的。
前两条公式分别由运动学公式的位移,速度,加速度的关系得到,可以类比下匀加速运动的公式:
s
=
v
0
t
+
1
2
a
t
2
s=v_0t+\frac{1}{2}at^2
s=v0t+21at2和
v
=
v
0
+
a
t
v=v_0+at
v=v0+at。第三条四元数公式,首先要知道四元数的导数计算。
假设某个旋转运动的旋转轴为单位向量u,绕该轴旋转的角度为θ,那么它对应的单位四元数为:
q
=
[
c
o
s
θ
2
u
s
i
n
θ
2
]
q=\left[\begin{matrix}cos\frac{\theta}{2}\\ usin\frac{\theta}{2}\end{matrix}\right]
q=[cos2θusin2θ]
当旋转一段微小的时间,即旋转角度趋于零时,容易有:
然后容易有
⨂
\bigotimes
⨂代表四元数相乘,
q
=
q
1
⨂
q
2
q=q_1\bigotimes q_2
q=q1⨂q2。可以看到直接得出的
b
k
+
1
b_{k+1}
bk+1的PVQ都需要对
b
k
b_k
bk的状态进行积分,这样每次后端优化迭代后,将导致我们需要根据每次迭代后的状态量重新进行状态传递,即需要重新传递IMU状态值,这样就会存在很大的计算量。
离散形式
下面还给出基于中值积分的离散形式公式,这在void Estimator::processIMU()函数中得以实现。 作用是将IMU积分出来第j时刻PVQ作为第j帧图像的优化初始值。
这里取的是
t
t
t和
t
+
1
t+1
t+1时刻加速度角速度的测量中值
IMU预积分
IMU预积分的思想就是将参考坐标系从世界坐标系
w
w
w调整为第
k
k
k帧的体坐标系,即同时乘以
R
w
b
k
R^{b_k}_w
Rwbk
其中,需要积分的量为
有 不少参考别的大佬的,会继续补充