VIN2.IMU预积分
0.引言
搬运工+自己阅读代码理解。
pipeline如图:
本节则是阅读总结IMU(100hz)-->IMU Pre-integration
部分。processIMU()函数、integration_base.h-->IntegrationBase类
。
1.IMU测量模型
忽略地球旋转,IMU 测量方程为:
a
t
=
a
t
(
r
e
a
l
)
+
b
a
t
+
R
w
t
g
w
+
n
a
w
t
=
w
t
(
r
e
a
l
)
+
b
w
t
+
n
w
(1)
\begin{array}{c}{a_{t}=a_{t(r e a l)}+b_{a_{t}}+R_{w}^{t} g^{w}+n_{a}} \\ {w_{t}=w_{t(r e a l)}+b_{w_{t}}+n_{w}}\end{array} \tag{1}
at=at(real)+bat+Rwtgw+nawt=wt(real)+bwt+nw(1)上标
g
g
g 表示gyro,
a
a
a 表示acc,
w
w
w表示在世界坐标系world,
b
b
b 表示imu机体坐标系body.imu的真实值为 ω, a 测量值为
ω
\omega
ω、
a
a
a;测量值为
ω
~
,
a
~
\tilde{\omega}, \tilde{\mathbf{a}}
ω~,a~。
IMU 的测量信息是在body坐标系(即 IMU 坐标)中获取
,假设噪声
n
a
n_{a}
na和
n
w
n_{w}
nw服从高斯分布:
n
a
∼
N
(
0
,
σ
a
2
)
n
w
∼
N
(
0
,
σ
w
2
)
(2)
\begin{aligned} n_{a} & \sim N\left(0, \sigma_{a}^{2}\right) \\ n_{w} & \sim N\left(0, \sigma_{w}^{2}\right) \end{aligned} \tag{2}
nanw∼N(0,σa2)∼N(0,σw2)(2)
加速度偏置
b
a
t
b_{a_{t}}
bat和陀螺仪偏置
b
w
t
b_{w_{t}}
bwt被建模为随机游走:
b
˙
a
i
=
n
b
a
\dot{b}_{a_{i}}=n_{b_{a}}
b˙ai=nba
b
˙
w
i
=
n
b
w
(3)
\dot{b}_{w_{i}}=n_{b_{w}}\tag{3}
b˙wi=nbw(3)其中
n
b
a
n_{b_{a}}
nba和
n
b
w
n_{b_{w}}
nbw服从高斯分布:
n
b
a
∼
N
(
0
,
σ
b
a
2
)
n
b
w
∼
N
(
0
,
σ
b
w
2
)
(4)
\begin{aligned} n_{b_{a}} & \sim N\left(0, \sigma_{b_{a}}^{2}\right) \\ n_{b_{w}} & \sim N\left(0, \sigma_{b_{w}}^{2}\right) \end{aligned}\tag{4}
nbanbw∼N(0,σba2)∼N(0,σbw2)(4)
既是其导数服从高斯分布!
2.当前时刻 PVQ 的连续形式
对于连续两个关键帧
b
k
b_{k}
bk 和
b
k
+
1
b_{k+1}
bk+1,它们对应的时刻分别为
t
k
,
t
k
+
1
t_{k}, t_{k+1}
tk,tk+1 。可以根据
[
t
k
,
t
k
+
1
]
\left[t_{k}, t_{k+1}\right]
[tk,tk+1]时间间隔内 IMU 的测量值,对系统的位置、速度和旋转等状态进行传播(注意这里的四元数采用了实部在后,虚部在前的形式):
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
i
−
n
a
)
−
g
w
)
d
t
2
(5)
p_{b_{k+1}}^{w}=p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}+\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(a_{t}-b_{a_{i}}-n_{a}\right)-g^{w}\right) d t^{2}\tag{5}
pbk+1w=pbkw+vbkwΔtk+∬t∈[tk,tk+1](Rtw(at−bai−na)−gw)dt2(5)
v
b
k
+
1
w
=
v
b
k
w
+
∫
t
∈
[
t
k
,
t
k
+
1
]
(
R
t
w
(
a
t
−
b
a
i
−
n
a
)
−
g
w
)
d
t
(6)
v_{b_{k+1}}^{w}=v_{b_{k}}^{w}+\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{w}\left(a_{t}-b_{a_{i}}-n_{a}\right)-g^{w}\right) d t \tag{6}
vbk+1w=vbkw+∫t∈[tk,tk+1](Rtw(at−bai−na)−gw)dt(6)
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
i
−
n
w
)
d
t
q_{b_{k+1}}^{w}=q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} q_{t}^{b_{k}} \otimes\left(w_{t}-b_{w_{i}}-n_{w}\right) d t
qbk+1w=qbkw⊗∫t∈[tk,tk+1]21qtbk⊗(wt−bwi−nw)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
(7)
=q_{b_{k}}^{w} \otimes \int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(w_{t}-b_{w_{t}}-n_{w}\right) q_{t}^{b_{k}} d t \tag{7}
=qbkw⊗∫t∈[tk,tk+1]21Ω(wt−bwt−nw)qtbkdt(7)其中:
Ω
(
w
)
=
[
−
[
w
]
×
w
−
w
T
0
]
,
[
w
]
×
=
[
0
−
w
z
w
y
w
z
0
−
w
x
−
w
y
w
x
0
]
(8)
\Omega(w)=\left[\begin{array}{cc}{-[w]_{ \times}} & {w} \\ {-w^{T}} & {0}\end{array}\right],[w]_{ \times}=\left[\begin{array}{ccc}{0} & {-w_{z}} & {w_{y}} \\ {w_{z}} & {0} & {-w_{x}} \\ {-w_{y}} & {w_{x}} & {0}\end{array}\right] \tag{8}
Ω(w)=[−[w]×−wTw0],[w]×=⎣⎡0wz−wy−wz0wxwy−wx0⎦⎤(8)
Δ
t
k
\Delta t_{k}
Δtk是
[
t
k
,
t
k
+
1
]
\left[t_{k}, t_{k+1}\right]
[tk,tk+1]之间的时间间隔,
R
t
w
R_{t}^{w}
Rtw为
t
t
t 时刻从body坐标系到世界坐标系的旋转矩阵,
q
t
b
k
q_{t}^{b_{k}}
qtbk为用四元数表示的
t
t
t 时刻从body坐标系到世界坐标系的旋转。
a
t
,
w
t
a_t,w_t
at,wt为 IMU 测量的加速度和角速度,是在 Body 自身坐标系,world 坐标系是IMU 所在的惯导系。
3.当前时刻 PVQ 的中值法离散形式
前面给出的是连续时刻的相机当前 PVQ 的迭代公式,基于中值法的公式 , 即从第 i 个 IMU 时刻 到第 i+1 个 IMU 时刻的积分过程 ,这与Estimator::processIMU()函数
中的 Ps[j]、Rs[j]
和 Vs[j]
是一致的(代码中的 j
时刻即为此处的i+1
),IMU 积分出来的第 j
时刻的物理量可以作为第 j
帧图像的初始值。
p
b
i
+
1
w
=
p
b
i
w
+
v
b
i
w
δ
t
+
1
2
a
‾
i
δ
t
2
(9)
p_{b_{i+1}}^{w}=p_{b_{i}}^{w}+v_{b_{i}}^{w} \delta t+\frac{1}{2} \overline{{a}}_{i} \delta t^{2} \tag{9}
pbi+1w=pbiw+vbiwδt+21aiδt2(9)
v
b
i
+
1
w
=
v
b
i
w
+
a
‾
i
δ
t
(10)
v_{b_{i+1}}^{w}=v_{b_{i}}^{w}+\overline{{a}}_{i} \delta t\tag{10}
vbi+1w=vbiw+aiδt(10)
q
b
i
+
1
w
=
q
b
i
w
⊗
[
1
2
ω
‾
i
δ
t
]
(11)
q_{b_{i+1}}^{w}=q_{b_{i}}^{w} \otimes\left[\frac{1}{2} \overline{{\omega}}_{i} \delta t\right]\tag{11}
qbi+1w=qbiw⊗[21ωiδt](11)其中:
a
‾
i
=
1
2
[
q
i
(
a
i
−
b
a
i
)
−
g
w
+
q
i
+
1
(
a
i
+
1
−
b
a
i
)
−
g
w
]
(12)
\overline{{a}}_{i}=\frac{1}{2}\left[q_{i}\left({a}_{i}-b_{a_{i}}\right)-g^{w}+q_{i+1}\left({a}_{i+1}-b_{a_{i}}\right)-g^{w}\right]\tag{12}
ai=21[qi(ai−bai)−gw+qi+1(ai+1−bai)−gw](12)
ω
‾
i
=
1
2
(
ω
i
+
ω
i
+
1
)
−
b
ω
i
(13)
\overline{{\omega}}_{i}=\frac{1}{2}\left({\omega}_{i}+{\omega}_{i+1}\right)-b_{\omega_{i}}\tag{13}
ωi=21(ωi+ωi+1)−bωi(13)
与代码中Estimator::processIMU()
:一一对应
~ ~ ~
int j = frame_count;
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
Vs[j] += dt * un_acc;
若采用欧拉积分,其中:
a
=
q
w
b
k
(
a
b
k
−
(
b
k
a
)
−
g
w
(14)
\mathbf{a}=\mathbf{q}_{w b_{k}}\left(\mathbf{a}^{b_{k}}-\left(\mathbf{b}_{k}^{a}\right)-\mathbf{g}^{w}\right.\tag{14}
a=qwbk(abk−(bka)−gw(14)
ω
=
ω
b
k
−
b
k
g
(15)
\omega=\omega^{b_{k}}-\mathbf{b}_{k}^{g} \tag{15}
ω=ωbk−bkg(15)这两个等式表达式表达与上面不统一,不影响理解。
4.两帧之间 PVQ 增量的连续形式
IMU的数据频率一般远高于视觉,在视觉两帧k,k+1之间通常会有>10组IMU数据。IMU的数据通过积分,可以获取当前位姿(p位置,q四元数表达的姿态)、瞬时速度等参数。
在VIO中,如果参考世界坐标系对IMU进行积分,积分项中包含相对于世界坐标系的瞬时旋转矩阵,这样有几个问题:
- 相对世界坐标系的旋转矩阵有drift,如果一直以其为基准进行积分,必然造成积分误差累积;
- 在进行优化位姿调整时(通常是调整视觉KeyFrame的pose),相对于世界坐标系的pose会变化,因而优化后的瞬时旋转矩阵和积分时不同,那么积分自然也就存在问题;
- 一般这个旋转矩阵不知道。。。
因此,一般的预积分的参考坐标系为k帧的IMU参考系,这样可以解决以上问题:
- 相对k帧的IMU进行积分,不会有累积误差;
- 即使后面调整了位姿,相对位置不变,因此预积分不存在问题;
- 这个旋转矩阵为单位矩阵E,后面每出现一个IMU数据,都可以用任何一种数值积分的方法计算;同时可以将重力加速度提取到积分号外面不参加积分,相当于在重力参考系中积分,计算量也会减少。
注意这里是求的增量,即是在窗口优化过程中的产生的变化
Δ
\Delta
Δ.优化状态向量包括滑动窗口内的所有相机状态(位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw)、相机到IMU的外参、所有3D点的逆深度:
X
=
[
x
0
,
x
1
,
⋯
x
n
,
x
c
b
,
λ
0
,
λ
1
,
⋯
λ
m
]
\mathcal{X}=\left[\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots \mathbf{x}_{n}, \mathbf{x}_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right]
X=[x0,x1,⋯xn,xcb,λ0,λ1,⋯λm]
x
k
=
[
p
b
k
w
,
v
b
k
w
,
q
b
k
w
,
b
a
,
b
g
]
,
k
∈
[
0
,
n
]
\mathbf{x}_{k}=\left[\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}\right], k \in[0, n]
xk=[pbkw,vbkw,qbkw,ba,bg],k∈[0,n]
x
c
b
=
[
p
c
b
,
q
c
b
]
\mathbf{x}_{c}^{b}=\left[\mathbf{p}_{c}^{b}, \mathbf{q}_{c}^{b}\right]
xcb=[pcb,qcb]
故
q
b
k
w
\mathbf{q}_{b_{k}}^{w}
qbkw随着优化是在改变的,导致公式(5)(6)(7)的积分随着优化需重复计算,耗时耗力,故将积分项转换到body坐标系计算,减少重复。
. 通过公式(5)(6)(7)可知,IMU 的预积分需要依赖与第 k 帧的 v 和 R,当我们在后端进行非线性优化时,需要迭代更新第 k 帧的 v 和 R,这将导致我们需要根据每次迭代后值重新进行积分,这将非常耗时。因此,我们考虑将优化变量从第 k 帧到第 k+1 帧的 IMU 预积分项b中分离开来,通过对公式(1)左右两侧各乘
R
w
b
k
R_{w}^{b_{k}}
Rwbk ,可化简为:
从积分式可以看出,系统位置、速度和旋转等状态的传播需要关键帧
b
k
b_{k}
bk 时刻的位置
p
b
k
w
p_{b_{k}}^{w}
pbkw 、速度
ν
b
k
w
\nu_{b_{k}}^{w}
νbkw和旋转
q
b
k
w
q_{b_{k}}^{w}
qbkw,当这些起始状态发生改变时,就需要按照上诉积分式重新进行状态传播。在基于优化的算法中,每个关键帧时刻的状态需要频繁调整,所以就需要频繁地重新积分,这样会浪费大量的计算资源。IMU 预积分就是为了避免这种计算资源上的浪费。考虑将优化变量从第 k 帧到第 k+1 帧的 IMU 预积分项中分离开来,通过对公式(5)(6)(7)左右两侧各乘
R
w
b
k
R_{w}^{b_{k}}
Rwbk ,可化简为公式(16)(17)(18)(19)(20)(21).
IMU 预积分的思路简单来说,就是将参考坐标系从世界坐标系
w
w
w调整为第
k
k
k 个关键帧时刻的body坐标系
b
k
b_k
bk :
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
(16)
R_{w}^{b_{k}} p_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(p_{b_{k}}^{w}+v_{b_{k}}^{w} \Delta t_{k}-\frac{1}{2} g^{w} \Delta t_{k}^{2}\right)+\alpha_{b_{k+1}}^{b_{k}} \tag{16}
Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk−21gwΔtk2)+αbk+1bk(16)
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
(17)
R_{w}^{b_{k}} v_{b_{k+1}}^{w}=R_{w}^{b_{k}}\left(v_{b_{k}}^{w}-g^{w} \Delta t_{k}\right)+\beta_{b_{k+1}}^{b_{k}} \tag{17}
Rwbkvbk+1w=Rwbk(vbkw−gwΔtk)+βbk+1bk(17)
q
w
b
k
⊗
q
b
k
+
1
w
=
γ
b
k
+
1
b
k
(18)
q_{w}^{b_{k}} \otimes q_{b_{k+1}}^{w}=\gamma_{b_{k+1}}^{b_{k}} \tag{18}
qwbk⊗qbk+1w=γbk+1bk(18)其中:
α
b
k
+
1
b
k
=
∬
t
∈
[
t
k
,
t
k
+
1
]
(
R
t
b
k
(
a
t
−
b
a
i
−
n
a
)
)
d
t
2
(19)
\alpha_{b_{k+1}}^{b_{k}}=\iint_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{b_{k}}\left(a_{t}-b_{a_{i}}-n_{a}\right)\right) d t^{2} \tag{19}
αbk+1bk=∬t∈[tk,tk+1](Rtbk(at−bai−na))dt2(19)
β
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
(
R
t
b
k
(
a
t
−
b
a
t
−
n
a
)
)
d
t
(20)
\beta_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]}\left(R_{t}^{b_{k}}\left(a_{t}-b_{a_{t}}-n_{a}\right)\right) d t \tag{20}
βbk+1bk=∫t∈[tk,tk+1](Rtbk(at−bat−na))dt(20)
γ
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Ω
(
w
t
−
b
w
i
−
n
w
)
γ
t
b
k
d
t
(21)
\gamma_{b_{k+1}}^{b_{k}}=\int_{t \in\left[t_{k}, t_{k+1}\right]} \frac{1}{2} \Omega\left(w_{t}-b_{w_{i}}-n_{w}\right) \gamma_{t}^{b_{k}} d t \tag{21}
γbk+1bk=∫t∈[tk,tk+1]21Ω(wt−bwi−nw)γtbkdt(21) 新的积分项(12.13.14)中参考坐标系变成了
b
k
b_{k}
bk ,可以理解为这时的积分结果为
b
k
+
1
b_{k+1}
bk+1对于
b
k
b_{k}
bk 的相对运动量,即使在优化过程中对关键帧的位置、速度和旋转等状态进行调整,也不对新的积分项产生任何影响,从而避免了重复积分。(从world系转换为body系)
这里需要重新讨论下公式(19)~(21)的预积分公式,以
α
b
k
+
1
b
k
{\alpha}_{b_{k+1}}^{b_{k}}
αbk+1bk为例,发现它是与 IMU 的bias 相关的,而 bias 也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得到一个新的 bias,又得根据公式中重新对第 k 帧和第 k+1 帧之间的 IMU 预积分,非常耗时。这里假设预积分的变化量与 bias 是线性关系,可以写成:
α
b
k
+
1
b
k
≈
α
^
b
k
+
1
b
k
+
J
b
a
α
δ
b
a
+
J
b
ω
α
δ
b
ω
\alpha_{b_{k+1}}^{b_{k}} \approx \hat{\alpha}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\alpha} \delta b_{a}+J_{b_{\omega}}^{\alpha} \delta b_{\omega}
αbk+1bk≈α^bk+1bk+Jbaαδba+Jbωαδbω
β
b
k
+
1
b
k
≈
β
^
b
k
+
1
b
k
+
J
b
a
β
δ
b
a
+
J
b
ω
β
δ
b
ω
(22)
\beta_{b_{k+1}}^{b_{k}} \approx \hat{\beta}_{b_{k+1}}^{b_{k}}+J_{b_{a}}^{\beta} \delta b_{a}+J_{b_{\omega}}^{\beta} \delta b_{\omega}\tag{22}
βbk+1bk≈β^bk+1bk+Jbaβδba+Jbωβδbω(22)
γ
b
k
+
1
b
k
≈
γ
^
b
k
+
1
b
k
⊗
[
1
1
2
J
b
ω
γ
δ
b
ω
]
\gamma_{b_{k+1}}^{b_{k}} \approx \hat{\gamma}_{b_{k+1}}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} J_{b_{\omega}}^{\gamma} \delta b_{\omega}}\end{array}\right]
γbk+1bk≈γ^bk+1bk⊗[121Jbωγδbω]其中的Jacobi矩阵后面讲。
5.两帧之间 PVQ 增量的离散形式
欧拉法:
下面给出离散时刻的 IMU 预积分公式,首先按照论文中采用的欧拉法,给出第 i 个 IMU时刻与第 i+1 个 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}_{i+1}^{b_{k}}=\hat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t^{2}
α^i+1bk=α^ibk+β^ibkδt+21R(γ^ibk)(a^i−bai)δt2
β
^
i
+
1
b
k
=
β
^
i
b
k
+
R
(
γ
^
i
b
k
)
(
a
^
i
−
b
a
i
)
δ
t
(23)
\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+R\left(\hat{\gamma}_{i}^{b_{k}}\right)\left(\hat{a}_{i}-b_{a_{i}}\right) \delta t \tag{23}
β^i+1bk=β^ibk+R(γ^ibk)(a^i−bai)δt(23)
γ
^
i
+
1
b
k
=
γ
^
i
b
k
⊗
γ
^
i
+
1
i
=
γ
^
i
b
k
⊗
[
1
1
2
(
ω
^
i
−
b
ω
i
)
δ
t
]
\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2}\left(\widehat{\omega}_{i}-b_{\omega_{i}}\right) \delta t}\end{array}\right]
γ^i+1bk=γ^ibk⊗γ^i+1i=γ^ibk⊗[121(ω
i−bωi)δt]
中值法:
代码中采用的基于中值法的 IMU 预积分公式,这与 Estimator::processIMU()
函数中的 IntegrationBase::push_back()
上是一致的。特别注意这里跟公式(9)~(13)是不一样的,这里积分出来的是前后两帧之间的 IMU 增量信息,而公式(9)~(13)给出的当前帧时刻的物理量信息。这里相当于在求解优化过程中产生的增量,计算的是增量值。
α
^
i
+
1
b
k
=
α
^
i
b
k
+
β
^
i
b
k
δ
t
+
1
2
a
^
‾
i
′
δ
t
2
\widehat{\alpha}_{i+1}^{b_{k}}=\widehat{\alpha}_{i}^{b_{k}}+\hat{\beta}_{i}^{b_{k}} \delta t+\frac{1}{2} \overline{\widehat{a}}_{i}^{\prime} \delta t^{2}
α
i+1bk=α
ibk+β^ibkδt+21a
i′δt2
β
^
i
+
1
b
k
=
β
^
i
b
k
+
a
^
‾
i
′
δ
t
(24)
\hat{\beta}_{i+1}^{b_{k}}=\hat{\beta}_{i}^{b_{k}}+\overline{\widehat{a}}_{i}^{\prime} \delta t \tag{24}
β^i+1bk=β^ibk+a
i′δt(24)
γ
^
i
+
1
b
k
=
γ
^
i
b
k
⊗
γ
^
i
+
1
i
=
γ
^
i
b
k
⊗
[
1
1
2
ω
‾
i
′
δ
t
]
\hat{\gamma}_{i+1}^{b_{k}}=\hat{\gamma}_{i}^{b_{k}} \otimes \hat{\gamma}_{i+1}^{i}=\hat{\gamma}_{i}^{b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \overline{\omega}_{i}^{\prime} \delta t}\end{array}\right]
γ^i+1bk=γ^ibk⊗γ^i+1i=γ^ibk⊗[121ωi′δt]其中:
a
^
‾
i
′
=
1
2
[
q
i
(
a
^
i
−
b
a
i
)
+
q
i
+
1
(
a
^
i
+
1
−
b
a
i
)
]
\overline{\hat{a}}_{i}^{\prime}=\frac{1}{2}\left[q_{i}\left(\hat{a}_{i}-b_{a_{i}}\right)+q_{i+1}\left(\hat{a}_{i+1}-b_{a_{i}}\right)\right]
a^i′=21[qi(a^i−bai)+qi+1(a^i+1−bai)]
ω
^
‾
i
′
=
1
2
(
ω
^
i
+
ω
^
i
+
1
)
−
b
ω
i
(25)
\overline{\widehat{\omega}}_{i}^{\prime}=\frac{1}{2}\left(\widehat{\omega}_{i}+\widehat{\omega}_{i+1}\right)-b_{\omega_{i}}\tag{25}
ω
i′=21(ω
i+ω
i+1)−bωi(25)
对应代码integration_base.h--> midPointIntegration()
:
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
6.连续形式下 PVQ 增量的误差、协方差及 Jacobian
. 目前已经完成了IMU预积分测量值的梳理,而要将IMU预积分运用到非线性优化中,需要建立线性高斯误差状态递推方程,由线性高斯系统的协方差,推导方程协方差矩阵,并求解对应的雅可比矩阵。
IMU 在每一个时刻积分出来的值都有误差,下面对误差进行分析。预积分误差:
一段时间内 IMU 构建的预积分量作为测量值,对两时刻之间的状态量进行约束,
[ r p r q r v r b a r b g ] 15 × 1 = \left[\begin{array}{l}{\mathbf{r}_{p}} \\ {\mathbf{r}_{q}} \\ {\mathbf{r}_{v}} \\ {\mathbf{r}_{b a}} \\ {\mathbf{r}_{b g}}\end{array}\right]_{15 \times 1}= ⎣⎢⎢⎢⎢⎡rprqrvrbarbg⎦⎥⎥⎥⎥⎤15×1= [ q b i w ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j 2 [ q b j b i ⊗ ( q b i w ⊗ q w b j ) ] x y z q b i w ( v j w − v i w + g w Δ t ) − β b i b j b j a − b i a b j g − b i g ] \left[\begin{array}{c}{\mathbf{q}_{b_{i} w}\left(\mathbf{p}_{w b_{j}}-\mathbf{p}_{w b_{i}}-\mathbf{v}_{i}^{w} \Delta t+\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}\right)-\boldsymbol{\alpha}_{b_{i} b_{j}}} \\ {2\left[\mathbf{q}_{b_{j}} b_{i} \otimes\left(\mathbf{q}_{b_{i} w} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z}} \\ {\mathbf{q}_{b_{i} w}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\boldsymbol{\beta}_{b_{i} b_{j}}} \\ {\mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a}} \\ {\mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g}}\end{array}\right] ⎣⎢⎢⎢⎢⎡qbiw(pwbj−pwbi−viwΔt+21gwΔt2)−αbibj2[qbjbi⊗(qbiw⊗qwbj)]xyzqbiw(vjw−viw+gwΔt)−βbibjbja−biabjg−big⎦⎥⎥⎥⎥⎤
上面误差中位移,速度,偏置都是直接相减得到。第二项是关于四元数的旋转误差,其中 [ ⋅ ] x y z [\cdot]_{x y z} [⋅]xyz表示只取四元数的虚部 ( x , y , z ) (x, y, z) (x,y,z) 组成的三维向量。
问题的提出:一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?由高斯分布的线性组合性质:
y
=
A
x
,
x
∈
N
(
0
,
Σ
x
)
\mathbf{y}=\mathbf{A} \mathbf{x}, \mathbf{x} \in \mathcal{N}\left(0, \boldsymbol{\Sigma}_{x}\right)
y=Ax,x∈N(0,Σx)则有:
Σ
y
=
A
Σ
x
A
⊤
\boldsymbol{\Sigma}_{y}=A \boldsymbol{\Sigma}_{x} A^{\top}
Σy=AΣxA⊤
所以,要推导预积分量的协方差,我们需要知道 imu 噪声和预积分量之间的线性递推关系。类似于状态转移,回顾一下之前的卡尔曼滤波.
假设已知了相邻时刻误差的线性传递方程:
η
i
k
=
F
k
−
1
η
i
k
−
1
+
G
k
−
1
n
k
−
1
\boldsymbol{\eta}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\eta}_{i k-1}+\mathbf{G}_{k-1} \mathbf{n}_{k-1}
ηik=Fk−1ηik−1+Gk−1nk−1比如:状态量误差为
η
i
k
=
[
δ
θ
i
k
,
δ
v
i
k
,
δ
p
i
k
]
\boldsymbol{\eta}_{i k}=\left[\delta \boldsymbol{\theta}_{i k}, \delta \mathbf{v}_{i k}, \delta \mathbf{p}_{i k}\right]
ηik=[δθik,δvik,δpik]测量噪声为
n
k
=
[
n
k
g
,
n
k
a
]
\mathbf{n}_{k}=\left[\mathbf{n}_{k}^{g}, \mathbf{n}_{k}^{a}\right]
nk=[nkg,nka].误差的传递由两部分组成
:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻。
协方差矩阵可以通过递推计算得到:
Σ
i
k
=
F
k
−
1
Σ
i
k
−
1
F
k
−
1
⊤
+
G
k
−
1
Σ
n
G
k
−
1
⊤
\boldsymbol{\Sigma}_{i k}=\mathbf{F}_{k-1} \boldsymbol{\Sigma}_{i k-1} \mathbf{F}_{k-1}^{\top}+\mathbf{G}_{k-1} \mathbf{\Sigma}_{\mathbf{n}} \mathbf{G}_{k-1}^{\top}
Σik=Fk−1Σik−1Fk−1⊤+Gk−1ΣnGk−1⊤其中,
Σ
n
\boldsymbol{\Sigma}_{\mathbf{n}}
Σn是测量噪声的协方差矩阵,方差从 i 时刻开始进行递推,
Σ
i
i
=
0
\boldsymbol{\Sigma}_{i i}=\mathbf{0}
Σii=0。
6.1.基于误差随时间变化的递推方程
此处借助卡尔曼滤波过程理解。
通常对于状态量之间的递推关系是非线性的方程如
x
k
=
f
(
x
k
−
1
,
u
k
−
1
)
\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)
xk=f(xk−1,uk−1),其中状态量为 x,u 为系统的输入量。可以用两种方法来推导状态误差传递的线性递推关系:
- 一种是基于一阶泰勒展开的误差递推方程。
- 一种是基于误差随时间变化的递推方程。
非线性系统
x
k
=
f
(
x
k
−
1
,
u
k
−
1
)
\mathbf{x}_{k}=f\left(\mathbf{x}_{k-1}, \mathbf{u}_{k-1}\right)
xk=f(xk−1,uk−1)的状态误差的线性递推关系如下:
δ
x
k
=
F
δ
x
k
−
1
+
G
n
k
−
1
(26)
\delta \mathbf{x}_{k}=\mathbf{F} \delta \mathbf{x}_{k-1}+\mathbf{G} \mathbf{n}_{k-1} \tag{26}
δxk=Fδxk−1+Gnk−1(26)基于泰勒展开的误差传递应用于 EKF 的协方差预测。论文中采用基于误差随时间变化的递推方程。如果我们能够推导状态误差随时间变化的导数关系,比如:
δ
x
˙
=
A
δ
x
+
B
n
\dot{\delta \mathbf{x}}=\mathbf{A} \delta \mathbf{x}+\mathbf{B} \mathbf{n}
δx˙=Aδx+Bn则误差状态的传递方程为:
δ
x
k
=
δ
x
k
−
1
+
δ
˙
x
k
−
1
Δ
t
→
δ
x
k
=
(
I
+
A
Δ
t
)
δ
x
k
−
1
+
B
Δ
t
n
k
−
1
\begin{aligned} \delta \mathbf{x}_{k} &=\delta \mathbf{x}_{k-1}+\dot{\delta} \mathbf{x}_{k-1} \Delta t \\ \rightarrow \delta \mathbf{x}_{k} &=(\mathbf{I}+\mathbf{A} \Delta t) \delta \mathbf{x}_{k-1}+\mathbf{B} \Delta t \mathbf{n}_{k-1} \end{aligned}
δxk→δxk=δxk−1+δ˙xk−1Δt=(I+AΔt)δxk−1+BΔtnk−1可以看出有等式(26)中:
F
=
I
+
A
Δ
t
,
G
=
B
Δ
t
\mathbf{F}=\mathbf{I}+\mathbf{A} \Delta t, \mathbf{G}=\mathbf{B} \Delta t
F=I+AΔt,G=BΔt
回顾一下预积分的误差递推公式,将测量噪声也考虑进模型:
q
b
i
b
k
+
1
=
q
b
i
b
k
⊗
[
1
1
2
ω
δ
t
]
\mathbf{q}_{b_{i} b_{k+1}}=\mathbf{q}_{b_{i} b_{k}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \omega \delta t}\end{array}\right]
qbibk+1=qbibk⊗[121ωδt]
α
b
i
b
k
+
1
=
α
b
i
b
k
+
β
b
i
b
k
δ
t
+
1
2
a
δ
t
2
\boldsymbol{\alpha}_{b_{i} b_{k+1}}=\boldsymbol{\alpha}_{b_{i} b_{k}}+\beta_{b_{i} b_{k}} \delta t+\frac{1}{2} \mathbf{a} \delta t^{2}
αbibk+1=αbibk+βbibkδt+21aδt2
β
b
i
b
k
+
1
=
β
b
i
b
k
+
a
δ
t
\beta_{b_{i} b_{k+1}}=\beta_{b_{i} b_{k}}+\mathbf{a} \delta t
βbibk+1=βbibk+aδt
b
k
+
1
a
=
b
k
a
+
n
b
k
a
δ
t
b
k
+
1
g
=
b
k
g
+
n
b
k
g
δ
t
\begin{aligned} \mathbf{b}_{k+1}^{a} &=\mathbf{b}_{k}^{a}+\mathbf{n}_{\mathbf{b}_{k}^{a}} \delta t \\ \mathbf{b}_{k+1}^{g} &=\mathbf{b}_{k}^{g}+\mathbf{n}_{\mathbf{b}_{k}^{g}} \delta t \end{aligned}
bk+1abk+1g=bka+nbkaδt=bkg+nbkgδt其中:
ω
=
1
2
(
(
ω
b
k
+
n
k
g
−
b
k
g
)
+
(
ω
b
k
+
1
+
n
k
+
1
g
−
b
k
g
)
)
\boldsymbol{\omega}=\frac{1}{2}\left(\left(\boldsymbol{\omega}^{b_{k}}+\mathbf{n}_{k}^{g}-\mathbf{b}_{k}^{g}\right)+\left(\boldsymbol{\omega}^{b_{k+1}}+\mathbf{n}_{k+1}^{g}-\mathbf{b}_{k}^{g}\right)\right)
ω=21((ωbk+nkg−bkg)+(ωbk+1+nk+1g−bkg))
a
=
1
2
(
q
b
i
b
k
(
a
b
k
+
n
k
a
−
b
k
a
)
+
q
b
i
b
k
+
1
(
a
b
k
+
1
+
n
k
+
1
a
−
b
k
a
)
)
\mathbf{a}=\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}\left(\mathbf{a}^{b_{k}}+\mathbf{n}_{k}^{a}-\mathbf{b}_{k}^{a}\right)+\mathbf{q}_{b_{i} b_{k+1}}\left(\mathbf{a}^{b_{k+1}}+\mathbf{n}_{k+1}^{a}-\mathbf{b}_{k}^{a}\right)\right)
a=21(qbibk(abk+nka−bka)+qbibk+1(abk+1+nk+1a−bka))确定误差传递的状态量,噪声量,然后开始构建传递方程。
推导出如下的形式,直接给出推导结果:
[
δ
α
b
k
+
1
b
k
+
1
′
δ
θ
b
k
+
1
b
k
+
1
′
δ
β
b
k
+
1
b
k
+
1
′
δ
b
k
+
1
a
δ
b
k
+
1
g
]
=
F
[
δ
α
b
k
b
k
′
δ
θ
b
k
b
k
′
δ
β
b
k
b
k
′
δ
b
k
a
δ
b
k
g
]
+
G
[
n
k
a
n
k
g
n
k
a
n
k
+
1
a
n
b
k
g
n
b
k
g
]
(27)
\left[\begin{array}{c}{\delta \boldsymbol{\alpha}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \boldsymbol{\theta}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \boldsymbol{\beta}_{b_{k+1} b_{k+1}^{\prime}}} \\ {\delta \mathbf{b}_{k+1}^{a}} \\ {\delta \mathbf{b}_{k+1}^{g}}\end{array}\right]=\mathbf{F}\left[\begin{array}{c}{\delta \boldsymbol{\alpha}_{b_{k} b_{k}^{\prime}}} \\ {\delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}} \\ {\delta \boldsymbol{\beta}_{b_{k} b_{k}^{\prime}}} \\ {\delta \mathbf{b}_{k}^{a}} \\ {\delta \mathbf{b}_{k}^{g}}\end{array}\right]+\mathbf{G}\left[\begin{array}{c}{\mathbf{n}_{k}^{a}} \\ {\mathbf{n}_{k}^{g}} \\ {\mathbf{n}_{k}^{a}} \\ {\mathbf{n}_{k+1}^{a}} \\ {\mathbf{n}_{\mathbf{b}_{k}^{g}}} \\ {\mathbf{n}_{\mathbf{b}_{k}^{g}}}\end{array}\right] \tag{27}
⎣⎢⎢⎢⎢⎡δαbk+1bk+1′δθbk+1bk+1′δβbk+1bk+1′δbk+1aδbk+1g⎦⎥⎥⎥⎥⎤=F⎣⎢⎢⎢⎢⎡δαbkbk′δθbkbk′δβbkbk′δbkaδbkg⎦⎥⎥⎥⎥⎤+G⎣⎢⎢⎢⎢⎢⎢⎡nkankgnkank+1anbkgnbkg⎦⎥⎥⎥⎥⎥⎥⎤(27)
F , G \mathbf{F}, \mathbf{G} F,G为两个时刻间的协方差传递矩阵:
F = \mathbf{F}= F= [ I f 12 I δ t − 1 4 ( q b i b k + q b i b k + 1 ) δ t 2 f 15 0 I − [ ω ] × 0 0 − I δ t 0 f 32 I − 1 2 ( q b i b k + q b i b k + 1 ) δ t f 35 0 0 0 I 0 0 0 0 0 I ] \left[\begin{array}{cccccc}{\mathbf{I}} & {\mathbf{f}_{12}} & {\mathbf{I} \delta t} & {-\frac{1}{4}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t^{2}} & {\mathbf{f}_{15}} \\ {\mathbf{0}} & {\mathbf{I}-[\boldsymbol{\omega}]_{ \times}} & {\mathbf{0}} & {\mathbf{0}} & {-\mathbf{I} \delta t} \\ {\mathbf{0}} & {\mathbf{f}_{32}} & {\mathbf{I}} & {-\frac{1}{2}\left(\mathbf{q}_{b_{i} b_{k}}+\mathbf{q}_{b_{i} b_{k+1}}\right) \delta t} & {\mathbf{f}_{35}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I}}\end{array}\right] ⎣⎢⎢⎢⎢⎡I0000f12I−[ω]×f3200Iδt0I00−41(qbibk+qbibk+1)δt20−21(qbibk+qbibk+1)δtI0f15−Iδtf350I⎦⎥⎥⎥⎥⎤
G
=
\mathbf{G}=
G=
[
1
4
q
b
i
b
k
δ
t
2
g
12
1
4
q
b
i
b
k
+
1
δ
t
2
g
14
0
0
0
1
2
I
δ
t
0
1
2
I
δ
t
0
0
1
2
q
b
i
b
k
δ
t
g
32
1
2
q
b
i
b
k
+
1
δ
t
g
34
0
0
0
0
0
0
I
δ
t
0
0
0
0
0
0
I
δ
t
]
\left[\begin{array}{cccccc}{\frac{1}{4} \mathbf{q}_{b_{i} b_{k}} \delta t^{2}} & {\mathbf{g}_{12}} & {\frac{1}{4} \mathbf{q}_{b_{i} b_{k+1}} \delta t^{2}} & {\mathbf{g}_{14}} & {\mathbf{0}} & {\mathbf{0}} \\ {\mathbf{0}} & {\frac{1}{2} \mathbf{I} \delta t} & {\mathbf{0}} & {\frac{1}{2} \mathbf{I} \delta t} & {\mathbf{0}} & {\mathbf{0}} \\ {\frac{1}{2} \mathbf{q}_{b_{i} b_{k}} \delta t} & {\mathbf{g}_{32}} & {\frac{1}{2} \mathbf{q}_{b_{i} b_{k+1}} \delta t} & {\mathbf{g}_{34}} & {\mathbf{0}} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I} \delta t} & {\mathbf{0}} \\ {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{0}} & {\mathbf{I} \delta t}\end{array}\right]
⎣⎢⎢⎢⎢⎡41qbibkδt2021qbibkδt00g1221Iδtg320041qbibk+1δt2021qbibk+1δt00g1421Iδtg3400000Iδt00000Iδt⎦⎥⎥⎥⎥⎤
其中:
f
12
=
∂
α
b
i
b
k
+
1
∂
δ
θ
b
k
b
k
′
=
−
1
4
(
R
b
i
b
k
[
a
b
k
−
b
k
a
]
×
δ
t
2
+
R
b
i
b
k
+
1
[
(
a
b
k
+
1
−
b
k
a
)
]
×
(
I
−
[
ω
]
×
δ
t
)
δ
t
2
)
\mathbf{f}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{ \times} \delta t^{2}+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times\left(\mathbf{I}-[\boldsymbol{\omega}]_{ \times} \delta t\right) \delta t^{2}\right)
f12=∂δθbkbk′∂αbibk+1=−41(Rbibk[abk−bka]×δt2+Rbibk+1[(abk+1−bka)]×(I−[ω]×δt)δt2)
f 12 = ∂ α b i b k + 1 ∂ δ θ b k b k ′ = − 1 4 ( R b i b k [ a b k − b k a ] × δ t 2 + R b i b k + 1 [ ( a b k + 1 − b k a ) ] × ( I − [ ω ] × δ t ) δ t 2 ) \mathbf{f}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \boldsymbol{\theta}_{b_{k} b_{k}^{\prime}}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k}}\left[\mathbf{a}^{b_{k}}-\mathbf{b}_{k}^{a}\right]_{ \times} \delta t^{2}+\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times\left(\mathbf{I}-[\omega]_{ \times} \delta t\right) \delta t^{2}\right) f12=∂δθbkbk′∂αbibk+1=−41(Rbibk[abk−bka]×δt2+Rbibk+1[(abk+1−bka)]×(I−[ω]×δt)δt2)
f 15 = ∂ α b i b k + 1 ∂ δ b k g = − 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( − δ t ) \mathbf{f}_{15}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t^{2}\right)(-\delta t) f15=∂δbkg∂αbibk+1=−41(Rbibk+1[(abk+1−bka)]×δt2)(−δt)
f 35 = ∂ β b i b k + 1 ∂ δ b k g = − 1 2 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t ) ( − δ t ) \mathbf{f}_{35}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \delta \mathbf{b}_{k}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right]_{ \times} \delta t\right)(-\delta t) f35=∂δbkg∂βbibk+1=−21(Rbibk+1[(abk+1−bka)]×δt)(−δt)
g 12 = ∂ α b i b k + 1 ∂ n k g = g 14 = ∂ α b i b k + 1 ∂ n k + 1 g = 1 4 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( 1 2 δ t ) \mathbf{g}_{12}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{14}=\frac{\partial \boldsymbol{\alpha}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=\frac{1}{4}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times \delta t^{2}\right)\left(\frac{1}{2} \delta t\right) g12=∂nkg∂αbibk+1=g14=∂nk+1g∂αbibk+1=41(Rbibk+1[(abk+1−bka)]×δt2)(21δt)
g 32 = ∂ β b i b k + 1 ∂ n k g = g 34 = ∂ β b i b k + 1 ∂ n k + 1 g = − 1 2 ( R b i b k + 1 [ ( a b k + 1 − b k a ) ] × δ t 2 ) ( 1 2 δ t ) \mathbf{g}_{32}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k}^{g}}=\mathbf{g}_{34}=\frac{\partial \boldsymbol{\beta}_{b_{i} b_{k+1}}}{\partial \mathbf{n}_{k+1}^{g}}=-\frac{1}{2}\left(\mathbf{R}_{b_{i} b_{k+1}}\left[\left(\mathbf{a}^{b_{k+1}}-\mathbf{b}_{k}^{a}\right)\right] \times \delta t^{2}\right)\left(\frac{1}{2} \delta t\right) g32=∂nkg∂βbibk+1=g34=∂nk+1g∂βbibk+1=−21(Rbibk+1[(abk+1−bka)]×δt2)(21δt)
代码中 F , G \mathbf{F}, \mathbf{G} F,G为 F , V \mathbf{F}, \mathbf{V} F,V。
这里对公式(26)(27)的 IMU 误差运动方程(状态方程)再说明,将上式和 EKF 对比可知,上式恰好给出了如 EKF 一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的 IMU 测量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻的均值和协方差,而公式(26)给出的是均值预测,协方差预测公式如下(对比卡尔曼滤波的第二个公式):
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_{t+\delta t}^{b_{k}}=\left(\mathrm{I}+F_{t} \delta t\right) P_{t}^{b_{k}}\left(\mathrm{I}+F_{t} \delta t\right)^{T}+\left(G_{t} \delta t\right) Q\left(G_{t} \delta t\right)^{T}
Pt+δtbk=(I+Ftδt)Ptbk(I+Ftδt)T+(Gtδt)Q(Gtδt)T其中:
F
=
I
+
F
t
δ
t
,
V
=
G
t
δ
t
F=\mathrm{I}+F_{t} \delta t, \quad V=G_{t} \delta t
F=I+Ftδt,V=Gtδt上式给出了协方差的迭代公式,初始值
P
b
k
b
k
=
0
P_{b_{k}}^{b_{k}}=0
Pbkbk=0。其中,
Q
Q
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{array}{cccc}{\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end{array}\right]
Q12×12=⎣⎢⎢⎡σa20000σw20000σba20000σbw2⎦⎥⎥⎤误差项的 Jacobian 的迭代公式:
J
t
+
δ
t
=
(
I
+
F
t
δ
t
)
J
t
J_{t+\delta t}=\left(\mathrm{I}+F_{t} \delta t\right) J_{t}
Jt+δt=(I+Ftδt)Jt其中 Jacobian 的初始值为
J
b
k
=
I
J_{b_{k}}=I
Jbk=I
7.离散形式的 PVQ 增量误差分析
首先直接给出 PVQ 增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration()函数是一致的。(但不知为何计算的 V 中与前四个噪声项相关的差个负号?)
[
δ
α
k
+
1
δ
θ
k
+
1
δ
β
k
+
1
δ
b
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
]
\left[\begin{array}{c}{\delta \alpha_{k+1}} \\ {\delta \theta_{k+1}} \\ {\delta \beta_{k+1}} \\ {\delta b_{k+1}} \\ {\delta b_{w_{k+1}}}\end{array}\right]=\left[\begin{array}{ccccc}{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{array}\right]\left[\begin{array}{c}{\delta \alpha_{k}} \\ {\delta \theta_{k}} \\ {\delta \beta_{k}} \\ {\delta b_{a_{k}}} \\ {\delta b_{w_{k}}}\end{array}\right]
⎣⎢⎢⎢⎢⎡δαk+1δθk+1δβk+1δbk+1δbwk+1⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡I0000f01f11f2100δt0I00f030f23I0f04−δtf240I⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡δαkδθkδβkδbakδbwk⎦⎥⎥⎥⎥⎤
+
[
v
00
v
01
v
02
v
03
0
0
−
δ
t
2
0
−
δ
t
2
0
−
R
k
δ
t
2
v
21
−
R
k
+
1
δ
t
2
v
23
0
0
0
0
0
δ
t
0
0
0
0
0
0
δ
t
]
[
n
a
k
n
w
k
n
a
k
+
1
n
b
a
n
b
w
]
(28)
+\left[\begin{array}{ccccc}{v_{00}} & {v_{01}} & {v_{02}} & {v_{03}} & {0} \\ {0} & {\frac{-\delta t}{2}} & {0} & {\frac{-\delta t}{2}} & {0} \\ {-\frac{R_{k} \delta t}{2}} & {v_{21}} & {-\frac{R_{k+1} \delta t}{2}} & {v_{23}} & {0} \\ {0} & {0} & {0} & {0} & {\delta t} & {0} \\ {0}& {0} & {0} & {0} & {0} & {\delta t}\end{array}\right]\left[\begin{array}{c}{n_{a_{k}}} \\ {n_{w_{k}}} \\ {n_{a_{k+1}}} \\ {n_{b_{a}}} \\ {n_{b_{w}}}\end{array}\right]\tag{28}
+⎣⎢⎢⎢⎢⎡v000−2Rkδt00v012−δtv2100v020−2Rk+1δt00v032−δtv2300000δt00δt⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡naknwknak+1nbanbw⎦⎥⎥⎥⎥⎤(28)
其中:
f
01
=
δ
t
2
f
21
=
−
1
4
R
k
(
a
^
k
−
b
a
k
)
∧
δ
t
2
−
1
4
R
k
+
1
(
a
^
k
+
1
−
b
a
k
)
∧
[
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
]
δ
t
2
f_{01}=\frac{\delta t}{2} f_{21}=-\frac{1}{4} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t^{2}-\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t^{2}
f01=2δtf21=−41Rk(a^k−bak)∧δt2−41Rk+1(a^k+1−bak)∧[I−(2ω
k+ω
k+1−bωk)∧δt]δt2
f 03 = − 1 4 ( R k + R k + 1 ) δ t 2 f_{03}=-\frac{1}{4}\left(R_{k}+R_{k+1}\right) \delta t^{2} f03=−41(Rk+Rk+1)δt2
f 04 = δ t 2 f 24 = 1 4 R k + 1 ( a ^ k + 1 − b a k ) ∧ δ t 3 f_{04}=\frac{\delta t}{2} f_{24}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{3} f04=2δtf24=41Rk+1(a^k+1−bak)∧δt3
f
11
=
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
f_{11}=I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t
f11=I−(2ω
k+ω
k+1−bωk)∧δt
f
21
=
−
1
2
R
k
(
a
^
k
−
b
a
k
)
∧
δ
t
−
1
2
R
k
+
1
(
a
^
k
+
1
−
b
a
k
)
∧
[
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
]
δ
t
f_{21}=-\frac{1}{2} R_{k}\left(\hat{a}_{k}-b_{a_{k}}\right)^{\wedge} \delta t-\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge}\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t
f21=−21Rk(a^k−bak)∧δt−21Rk+1(a^k+1−bak)∧[I−(2ω
k+ω
k+1−bωk)∧δt]δt
f
23
=
−
1
2
(
R
k
+
R
k
+
1
)
δ
t
f_{23}=-\frac{1}{2}\left(R_{k}+R_{k+1}\right) \delta t
f23=−21(Rk+Rk+1)δt
f 24 = 1 2 R k + 1 ( a ^ k + 1 − b a k ) ∧ δ t 2 f_{24}=\frac{1}{2} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} f24=21Rk+1(a^k+1−bak)∧δt2
v 00 = − 1 4 R k δ t 2 v_{00}=-\frac{1}{4} R_{k} \delta t^{2} v00=−41Rkδt2
v 01 = v 03 = δ t 2 v 21 = 1 4 R k + 1 ( a ^ k + 1 − b a k ) ∧ δ t 2 δ t 2 v_{01}=v_{03}=\frac{\delta t}{2} v_{21}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} \frac{\delta t}{2} v01=v03=2δtv21=41Rk+1(a^k+1−bak)∧δt22δt
v 02 = − 1 4 R k + 1 δ t 2 v_{02}=-\frac{1}{4} R_{k+1} \delta t^{2} v02=−41Rk+1δt2
v 21 = v 23 = 1 4 R k + 1 ( a ^ k + 1 − b a k ) ∧ δ t 2 v_{21}=v_{23}=\frac{1}{4} R_{k+1}\left(\hat{a}_{k+1}-b_{a_{k}}\right)^{\wedge} \delta t^{2} v21=v23=41Rk+1(a^k+1−bak)∧δt2
8.离散形式的 PVQ 增量误差的 Jacobian 和协方差
将公式(28)简写为:
δ
z
k
+
1
15
×
1
=
F
15
×
15
δ
z
k
15
×
1
+
V
15
×
18
Q
18
×
1
\delta z_{k+1}^{15 \times 1}=F^{15 \times 15} \delta z_{k}^{15 \times 1}+V^{15 \times 18} Q^{18 \times 1}
δzk+115×1=F15×15δzk15×1+V15×18Q18×1
则
J
a
c
o
b
i
a
n
Jacobian
Jacobian 的迭代公式为:
J
k
+
1
15
×
15
=
F
J
k
(29)
J_{k+1}^{15 \times 15}=F J_{k} \tag{29}
Jk+115×15=FJk(29)
其中,
J
a
c
o
b
i
a
n
Jacobian
Jacobian 的初始值为
J
k
=
I
J_k = I
Jk=I。这里计算出来的
J
k
+
1
J_{k+1}
Jk+1 只是为了给后面提供对 bias 的
J
a
c
o
b
i
a
n
Jacobian
Jacobian。
协方差的迭代公式为:
P
k
+
1
15
×
15
=
F
P
k
F
T
+
V
Q
V
T
(30)
P_{k+1}^{15 \times 15}=F P_{k} F^{T}+V Q V^{T} \tag{30}
Pk+115×15=FPkFT+VQVT(30)
其中,初始值
P
k
=
0
P_{k}=0
Pk=0。
Q
Q
Q 为表示噪声项的对角协方差矩阵:
Q
18
×
18
=
[
σ
a
2
0
0
0
0
0
0
σ
w
2
0
0
0
0
0
0
σ
a
2
0
0
0
0
0
0
σ
w
2
0
0
0
0
0
0
σ
b
a
2
0
0
0
0
0
0
σ
b
w
2
]
Q^{18 \times 18}=\left[\begin{array}{cccccc}{\sigma_{a}^{2}} & {0} & {0} & {0} & {0} & {0} \\ {0} & {\sigma_{w}^{2}} & {0} & {0} & {0} & {0} \\ {0} & {0} & {\sigma_{a}^{2}} & {0} & {0} & {0} \\ {0} & {0} & {0} & {\sigma_{w}^{2}} & {0} & {0} \\ {0} & {0} & {0} & {0} & {\sigma_{b_{a}}^{2}} & {0} \\ {0} & {0} & {0} & {0} & {0} & {\sigma_{b_{w}}^{2}}\end{array}\right]
Q18×18=⎣⎢⎢⎢⎢⎢⎢⎡σa2000000σw2000000σa2000000σw2000000σba2000000σbw2⎦⎥⎥⎥⎥⎥⎥⎤
代码integration_base.h-->midPointIntegration():
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
if(update_jacobian)
{
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
Vector3d a_0_x = _acc_0 - linearized_ba;
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
//构造F、V矩阵
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = Matrix3d::Identity();
F.block<3, 3>(0, 3) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +
-0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;
F.block<3, 3>(0, 6) = MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(0, 9) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;
F.block<3, 3>(3, 3) = Matrix3d::Identity() - R_w_x * _dt;
F.block<3, 3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;
F.block<3, 3>(6, 3) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(6, 6) = Matrix3d::Identity();
F.block<3, 3>(6, 9) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(12, 12) = Matrix3d::Identity();
//cout<<"A"<<endl<<A<<endl;
MatrixXd V = MatrixXd::Zero(15,18);
V.block<3, 3>(0, 0) = 0.25 * delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * 0.5 * _dt;
V.block<3, 3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 9) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(6, 0) = 0.5 * delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x * _dt * 0.5 * _dt;
V.block<3, 3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 9) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 12) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(12, 15) = MatrixXd::Identity(3,3) * _dt;
//step_jacobian = F;
//step_V = V;
jacobian = F * jacobian; \\公式(29)
covariance = F * covariance * F.transpose() + V * noise * V.transpose();\\公式(30)
}
}
总述:基于滑动窗口的 VIO Bundle Adjustment,总的loss funcution如下所示:
min X ρ ( ∥ r p − J p X ∥ Σ p 2 ) ⏟ prior + ∑ i ∈ B ρ ( ∥ r b ( z b i b i + 1 , X ) ∥ Σ b i b i + 1 2 ) ⏟ IMU error \min _{\mathcal{X}} \underbrace{\rho\left(\left\|\mathbf{r}_{p}-\mathbf{J}_{p} \mathcal{X}\right\|_{\Sigma_{p}}^{2}\right)}_{\text { prior }}+\underbrace{\sum_{i \in B} \rho\left(\left\|\mathbf{r}_{b}\left(\mathbf{z}_{b_{i} b_{i+1}}, \mathcal{X}\right)\right\|_{\Sigma_{b_{i} b_{i+1}}}^{2}\right)}_{\text { IMU error }} minX prior ρ(∥rp−JpX∥Σp2)+ IMU error i∈B∑ρ(∥rb(zbibi+1,X)∥Σbibi+12) + ∑ ( i , j ) ∈ F ρ ( ∥ r f ( z f j c i , X ) ∥ Σ f j c i 2 ) ⏟ image error +\underbrace{\sum_{(i, j) \in F} \rho\left(\left\|\mathbf{r}_{f}\left(\mathbf{z}_{\mathbf{f}_{j}}^{c_{i}}, \mathcal{X}\right)\right\|_{\Sigma_{\mathbf{f}_{j}}^{c_{i}}}^{2}\right)}_{\text { image error }} + image error (i,j)∈F∑ρ(∥∥∥rf(zfjci,X)∥∥∥Σfjci2)
故,6、7、8小节实际是在求的IMU error项将会用到的量,为后端优化做准备!