VINS-Mono融合轮式编码器和GPS(一):预积分
开篇
最近在学习多传感器融合技术,本着重复造轮子的精神,做了一个VINS-Mono融合轮式编码器和GPS数据的小项目,轮式编码器数据在融合时做了紧耦合,主要参考了论文[1];GPS数据做了松耦合,主要是借用了VINS-Fusion的方式,当然也可以用紧耦合的方式(挖个坑,后续再加),项目地址VINS-GPS-Wheel,附视频。项目仍在进行中,欢迎交流学习。
在此记录一下主要的几个方面(水几篇博客):
VINS-Mono融合轮式编码器和GPS(一):预积分
VINS-Mono融合轮式编码器和GPS(二):鲁棒初始化
VINS-Mono融合轮式编码器和GPS(三):后端优化
VINS-Mono融合轮式编码器和GPS(四):融合GPS
理论
VIO系统本身具有4自由度的不可观性,但是有文献[2]表明,对于汽车的行驶方式,例如恒速运动、圆周运动,会造成尺度无法精确估计,融合轮式编码器可以提高VIO系统的精确性。在自动驾驶的室外场景下,回环比较困难,GNSS数据可以提供一个全局的定位,需要加进去。
预积分公式推导
不同资料记法可能不太一样,推公式之前先说一下这里使用的记法, ( ⋅ ) w {\left ( \cdot \right )}^{w} (⋅)w 、 ( ⋅ ) b k {\left ( \cdot \right )}^{{b}_k} (⋅)bk、 ( ⋅ ) c k {\left ( \cdot \right )}^{{c}_k} (⋅)ck、 ( ⋅ ) o k {\left ( \cdot \right )}^{{o}_k} (⋅)ok分别表示物理量的世界坐标系、IMU坐标系、相机坐标系和轮式编码器坐标系在图像第k帧的表示。 R o b {R}_{o}^{b} Rob和 q o b {q}_{o}^{b} qob表示向量从 o o o坐标系到 b b b坐标系下的旋转变换, p o b {p}_{o}^{b} pob表示 b b b坐标系下 o o o坐标系原点的坐标。
单独写轮式编码器的预积分太单薄,有的地方会和IMU预积分公式放到一起,不过IMU相关的公式说明就不给出了。
1. 预积分形式
α
^
i
+
1
b
k
=
α
^
i
b
k
+
β
^
i
b
k
δ
t
+
1
2
R
(
γ
^
i
b
k
)
(
a
^
−
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(\hat{\gamma}_{i}^{b_{k}})(\hat{a}-b_{a_i}){\delta t}^{2}
α^i+1bk=α^ibk+β^ibkδt+21R(γ^ibk)(a^−bai)δt2
β
^
i
+
1
b
k
=
β
^
i
b
k
+
R
(
γ
^
i
b
k
)
(
a
^
−
b
a
i
)
δ
t
\hat{\beta}_{i+1}^{b_{k}} = \hat{\beta}_{i}^{b_{k}} +R(\hat{\gamma}_{i}^{b_{k}})(\hat{a}-b_{a_i}){\delta t}
β^i+1bk=β^ibk+R(γ^ibk)(a^−bai)δt
γ
^
i
+
1
b
k
=
γ
^
i
b
k
⊗
γ
^
i
+
1
i
=
γ
^
i
b
k
⊗
[
1
1
2
(
ω
^
−
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\begin{bmatrix} 1\\ \frac{1}{2}(\hat{\omega}-b_{\omega_i})\delta t \end{bmatrix}
γ^i+1bk=γ^ibk⊗γ^i+1i=γ^ibk⊗[121(ω^−bωi)δt]
η
^
i
+
1
b
k
=
η
^
i
b
k
+
R
(
γ
^
i
b
k
)
(
R
^
o
b
v
^
i
)
δ
t
\hat{\eta}_{i+1}^{b_{k}} = \hat{\eta}_{i}^{b_{k}} +R(\hat{\gamma}_{i}^{b_{k}})(\hat{R}_{o}^{b}\hat{v}_i){\delta t}
η^i+1bk=η^ibk+R(γ^ibk)(R^obv^i)δt
η
\eta
η表示轮式编码器预积分量,
R
o
b
{R}_{o}^{b}
Rob为轮式编码器的外参旋转矩阵,
v
^
i
\hat{v}_i
v^i为轮式编码器的线速度数据,当然这里采用的不是编码器的原始数据,需要自己算出线速度。注意,这里的
i
i
i表示IMU时刻,虽然作者没有说明,本项目在代码实现过程中还是做了一下时间戳对齐。
2. 中值积分
η ^ i + 1 b k = η ^ i b k + 1 2 [ R ( γ ^ i b k ) ( R ^ o b v ^ i ) + R ( γ ^ i + 1 b k ) ( R ^ o b v ^ i + 1 ) ] δ t \hat{\eta}_{i+1}^{b_{k}} = \hat{\eta}_{i}^{b_{k}} +\frac{1}{2}\left [ R(\hat{\gamma}_{i}^{b_{k}})(\hat{R}_{o}^{b}\hat{v}_i) + R(\hat{\gamma}_{i+1}^{b_{k}})(\hat{R}_{o}^{b}\hat{v}_{i+1})\right]{\delta t} η^i+1bk=η^ibk+21[R(γ^ibk)(R^obv^i)+R(γ^i+1bk)(R^obv^i+1)]δt
3. 连续形式下的预积分误差、协方差和Jacobian
导数形式:
δ
z
˙
t
b
k
=
F
t
δ
z
t
b
k
+
G
t
n
t
\delta\dot{z}_t^{b_k} = F_t\delta z_t^{b_k} + G_tn_t
δz˙tbk=Ftδztbk+Gtnt
根据导数定义,可写出
δ
z
˙
t
+
δ
t
b
k
=
(
I
+
F
t
δ
t
)
δ
z
t
b
k
+
(
G
t
δ
t
)
n
t
\delta\dot{z}_{t+\delta t}^{b_k} = (I + F_t\delta t)\delta z_t^{b_k} + (G_t\delta t)n_t
δz˙t+δtbk=(I+Ftδt)δztbk+(Gtδt)nt,进而写出展开形式(为了和代码一致,按照
p
q
v
pqv
pqv而不是
p
v
q
pvq
pvq的形式)
[
δ
α
^
t
+
δ
t
b
k
δ
θ
^
t
+
δ
t
b
k
δ
β
^
t
+
δ
t
b
k
δ
η
^
t
+
δ
t
b
k
δ
b
^
a
t
+
δ
t
δ
b
^
ω
t
+
δ
t
]
=
[
I
0
I
δ
t
0
0
0
0
I
−
(
ω
^
t
−
b
ω
t
)
∧
δ
t
0
0
0
−
I
δ
t
0
−
R
t
b
k
(
a
^
t
−
b
a
t
)
∧
δ
t
I
0
−
R
t
b
k
δ
t
0
0
−
R
t
b
k
(
R
^
b
o
v
^
t
)
∧
δ
0
I
0
0
0
0
0
0
I
0
0
0
0
0
0
I
]
[
δ
α
^
t
b
k
δ
θ
^
t
b
k
δ
β
^
t
b
k
δ
η
^
t
b
k
δ
b
^
a
t
δ
b
^
ω
t
]
+
[
0
0
0
0
0
0
−
I
δ
t
0
0
0
−
R
t
b
k
δ
t
0
0
0
0
0
0
−
R
t
b
k
R
o
b
δ
t
0
0
0
0
0
I
δ
t
0
0
0
0
0
I
δ
t
]
[
n
a
n
ω
n
v
n
b
a
n
b
ω
]
\begin{bmatrix} \delta\hat{\alpha}_{t+\delta t}^{b_k} \\ \delta\hat{\theta}_{t+\delta t}^{b_k}\\ \delta\hat{\beta}_{t+\delta t}^{b_k}\\ \delta\hat{\eta}_{t+\delta t}^{b_k}\\ \delta\hat{b}_{a_{t+\delta t}}\\ \delta\hat{b}_{\omega_{t+\delta t}} \end{bmatrix}= \begin{bmatrix} I& 0& I\delta t& 0& 0& 0\\ 0& I-{(\hat{\omega}_t-b_{\omega_t})}^{\wedge}\delta t & 0& 0& 0& -I\delta t\\ 0& -R_{t}^{b_k}{(\hat{a}_t-b_{a_t})}^{\wedge}\delta t& I& 0& -R_{t}^{b_k}\delta t& 0\\ 0& -R_{t}^{b_k}{(\hat{R}_b^o\hat{v}_t)}^{\wedge}\delta& 0& I& 0& 0\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I \end{bmatrix}\begin{bmatrix} \delta\hat{\alpha}_{t}^{b_k} \\ \delta\hat{\theta}_{t}^{b_k}\\ \delta\hat{\beta}_{t}^{b_k}\\ \delta\hat{\eta}_{t}^{b_k}\\ \delta\hat{b}_{a_{t}}\\ \delta\hat{b}_{\omega_{t}} \end{bmatrix}+\begin{bmatrix} 0& 0& 0& 0& 0\\ 0& -I\delta t& 0& 0& 0\\ -R_t^{b_k}\delta t& 0& 0& 0& 0\\ 0& 0& -R_t^{b_k}R_o^b\delta t& 0& 0\\ 0& 0& 0& I\delta t& 0\\ 0& 0& 0& 0& I\delta t \end{bmatrix}\begin{bmatrix} n_a\\ n_\omega \\ n_v\\ n_{b_a}\\ n_{b_\omega } \end{bmatrix}
⎣⎢⎢⎢⎢⎢⎢⎢⎡δα^t+δtbkδθ^t+δtbkδβ^t+δtbkδη^t+δtbkδb^at+δtδb^ωt+δt⎦⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡I000000I−(ω^t−bωt)∧δt−Rtbk(a^t−bat)∧δt−Rtbk(R^bov^t)∧δ00Iδt0I000000I0000−Rtbkδt0I00−Iδt000I⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎡δα^tbkδθ^tbkδβ^tbkδη^tbkδb^atδb^ωt⎦⎥⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡00−Rtbkδt0000−Iδt0000000−RtbkRobδt000000Iδt000000Iδt⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡nanωnvnbanbω⎦⎥⎥⎥⎥⎤
4. 离散形式下的预积分误差、协方差和Jacobian(和代码一致)
这里直接给出离散形式下的预积分误差公式
[
δ
α
k
+
1
δ
θ
k
+
1
δ
β
k
+
1
δ
η
k
+
1
δ
b
a
k
+
1
δ
b
ω
k
+
1
]
=
[
I
f
01
δ
t
0
f
04
f
05
0
f
11
0
0
0
−
δ
t
0
f
21
I
0
f
24
f
25
0
f
31
0
I
0
f
35
0
0
0
0
I
0
0
0
0
0
0
I
]
+
[
v
00
v
01
0
v
03
v
04
0
0
0
0
−
δ
t
2
0
0
−
δ
t
2
0
0
0
−
R
k
δ
t
2
v
21
0
−
R
k
+
1
δ
t
2
v
24
0
0
0
0
v
31
v
32
0
v
34
v
35
0
0
0
0
0
0
0
0
δ
t
0
0
0
0
0
0
0
0
δ
t
]
[
n
a
k
n
ω
k
n
v
k
n
a
k
+
1
n
ω
k
+
1
n
v
k
+
1
n
b
a
n
b
ω
]
\begin{bmatrix} \delta \alpha_{k+1}\\ \delta \theta_{k+1}\\ \delta \beta_{k+1}\\ \delta \eta_{k+1}\\ \delta b_{a_{k+1}}\\ \delta b_{\omega_{k+1}} \end{bmatrix}=\begin{bmatrix} I& f_{01}& \delta t& 0& f_{04}& f_{05}\\ 0& f_{11}& 0& 0& 0& -\delta t\\ 0& f_{21}& I& 0& f_{24}& f_{25}\\ 0& f_{31}& 0& I& 0& f_{35}\\ 0& 0& 0& 0& I& 0\\ 0& 0& 0& 0& 0& I \end{bmatrix}+\begin{bmatrix} v_{00}& v_{01}& 0& v_{03}& v_{04}& 0& 0& 0\\ 0& \frac{-\delta t}{2} & 0& 0& \frac{-\delta t}{2}& 0& 0& 0\\ \frac{-R_k\delta t}{2}& v_{21}& 0& \frac{-R_{k+1}\delta t}{2}& v_{24}& 0& 0& 0\\ 0& v_{31}& v_{32}& 0& v_{34}& v_{35}& 0& 0\\ 0& 0& 0& 0& 0& 0& \delta t& 0\\ 0& 0& 0& 0& 0& 0& 0& \delta t \end{bmatrix}\begin{bmatrix} n_{a_k}\\ n_{\omega_k}\\ n_{v_k}\\ n_{a_{k+1}}\\ n_{\omega_{k+1}}\\ n_{v_{k+1}}\\ n_{b_a}\\ n_{b_\omega } \end{bmatrix}
⎣⎢⎢⎢⎢⎢⎢⎡δαk+1δθk+1δβk+1δηk+1δbak+1δbωk+1⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎡I00000f01f11f21f3100δt0I000000I00f040f240I0f05−δtf25f350I⎦⎥⎥⎥⎥⎥⎥⎤+⎣⎢⎢⎢⎢⎢⎢⎡v0002−Rkδt000v012−δtv21v3100000v3200v0302−Rk+1δt000v042−δtv24v3400000v35000000δt000000δt⎦⎥⎥⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡naknωknvknak+1nωk+1nvk+1nbanbω⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
IMU相关推导过程省略,只给出轮式编码器的。用true表示有噪声真实测量值,nominal表示无噪声的理论值
δ
η
˙
=
η
˙
t
r
u
e
−
η
˙
n
o
m
i
n
a
l
η
˙
t
r
u
e
=
R
t
b
k
t
r
u
e
R
o
b
v
^
t
t
r
u
e
=
R
t
b
k
e
x
p
(
δ
θ
∧
)
R
o
b
(
v
^
t
−
n
e
)
=
R
t
b
k
(
I
+
δ
θ
∧
)
R
o
b
(
v
^
t
−
n
e
)
=
R
t
b
k
[
R
o
b
v
^
t
−
R
o
b
n
e
−
δ
θ
∧
R
o
b
(
v
^
t
−
n
e
)
]
=
R
t
b
k
[
R
o
b
v
^
t
−
R
o
b
n
e
−
[
R
o
b
(
v
^
t
−
n
e
)
]
∧
δ
θ
]
η
˙
n
o
m
i
n
a
l
=
R
t
b
k
R
o
b
v
^
t
\begin{aligned} & \delta \dot{\eta} = \dot{\eta}_{true}-\dot{\eta}_{nominal} \\ &\begin{aligned} \dot{\eta}_{true}&={R_t^{b_k}}_{true} {R}_{o}^{b} {{\hat{v}_t}_{true} } \\ & = {R_t^{b_k}}exp(\delta \theta ^{\wedge}){R}_{o}^{b} ({\hat{v}_{t}-n_{e}}) \\ & = {R_t^{b_k}}(I+\delta \theta ^\wedge ){R}_{o}^{b} ({\hat{v}_{t}-n_{e}}) \\ & = {R_t^{b_k}}\left [ {R}_{o}^{b}\hat{v}_{t} - {R}_{o}^{b}n_{e}-\delta \theta ^{\wedge} {R}_{o}^{b}({\hat{v}_{t}-n_{e}}) \right ] \\ & = {R_t^{b_k}}\left [ {R}_{o}^{b}\hat{v}_{t} - {R}_{o}^{b}n_{e}-[{R}_{o}^{b}({\hat{v}_{t}-n_{e}})]^{\wedge} \delta \theta \right ] \\ \end{aligned} \\ & \dot{\eta}_{nominal} = {R_t^{b_k}} {R}_{o}^{b} {{\hat{v}_t} } \end{aligned}
δη˙=η˙true−η˙nominalη˙true=RtbktrueRobv^ttrue=Rtbkexp(δθ∧)Rob(v^t−ne)=Rtbk(I+δθ∧)Rob(v^t−ne)=Rtbk[Robv^t−Robne−δθ∧Rob(v^t−ne)]=Rtbk[Robv^t−Robne−[Rob(v^t−ne)]∧δθ]η˙nominal=RtbkRobv^t
δ
η
˙
=
η
˙
t
r
u
e
−
η
˙
n
o
m
i
n
a
l
=
−
R
t
b
k
R
o
b
n
e
−
R
t
b
k
[
R
o
b
(
v
^
t
−
n
e
)
]
∧
δ
θ
\delta \dot{\eta} = \dot{\eta}_{true}-\dot{\eta}_{nominal} = - {R_t^{b_k}}{R}_{o}^{b}n_{e}-{R_t^{b_k}}[{R}_{o}^{b}({\hat{v}_{t}-n_{e}})]^{\wedge} \delta \theta
δη˙=η˙true−η˙nominal=−RtbkRobne−Rtbk[Rob(v^t−ne)]∧δθ
δ
η
˙
=
−
1
2
R
k
b
k
R
o
b
n
e
k
−
1
2
R
k
+
1
b
k
+
1
R
o
b
n
e
k
+
1
−
1
2
R
k
b
k
[
R
o
b
(
v
^
k
−
n
e
k
)
]
∧
δ
θ
k
−
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
−
n
e
k
+
1
)
]
∧
δ
θ
k
+
1
=
−
1
2
R
k
b
k
R
o
b
n
e
k
−
1
2
R
k
+
1
b
k
+
1
R
o
b
n
e
k
+
1
−
1
2
R
k
b
k
[
R
o
b
(
v
^
k
−
n
e
k
)
]
∧
δ
θ
k
−
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
−
n
e
k
+
1
)
]
∧
{
[
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
]
δ
θ
k
−
n
ω
k
+
n
ω
k
+
1
2
δ
t
−
δ
t
δ
b
ω
k
}
=
{
−
1
2
R
k
b
k
[
R
o
b
(
v
^
k
−
n
e
k
)
]
∧
−
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
−
n
e
k
+
1
)
]
∧
[
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
]
}
δ
θ
k
+
1
4
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
)
]
∧
n
ω
k
δ
t
+
1
4
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
)
]
∧
n
ω
k
+
1
δ
t
+
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
)
]
∧
δ
b
ω
k
δ
t
−
1
2
R
k
b
k
R
o
b
n
e
k
−
1
2
R
k
+
1
b
k
+
1
R
o
b
n
e
k
+
1
\begin{aligned} \delta \dot{\eta} = & - \frac{1}{2} {R_k^{b_k}}{R}_{o}^{b}{n_{e}}_{k} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}{R}_{o}^{b}{n_{e}}_{k+1} - \frac{1}{2}{R_{k}^{b_k}}[{R}_{o}^{b}({\hat{v}_{k}-{n_{e}}_{k}})]^{\wedge} \delta \theta_{k}- \frac{1}{2}{R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}-{n_{e}}_{k+1}})]^{\wedge} \delta \theta_{k+1} \\ = &- \frac{1}{2} {R_k^{b_k}}{R}_{o}^{b}{n_{e}}_{k} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}{R}_{o}^{b}{n_{e}}_{k+1} - \frac{1}{2}{R_{k}^{b_k}}[{R}_{o}^{b}({\hat{v}_{k}-{n_{e}}_{k}})]^{\wedge} \delta \theta_{k} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}-{n_{e}}_{k+1}})]^{\wedge} \left\{\left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta \theta_{k}-\frac{{n_{\omega}}_{k}+{n_{\omega}}_{k+1}}{2} \delta t-\delta t \delta b_{\omega_{k}}\right\} \\ = &\left \{- \frac{1}{2} {R_{k}^{b_k}}[{R}_{o}^{b}({\hat{v}_{k}-{n_{e}}_{k}})]^{\wedge} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}-{n_{e}}_{k+1}})]^{\wedge} \left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \right \} \delta \theta_{k} \\ & + \frac{1}{4} {R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}})]^{\wedge} {n_{\omega}}_{k}\delta t + \frac{1}{4} {R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}})]^{\wedge}{n_{\omega}}_{k+1}\delta t \\ & + \frac{1}{2} {R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}})]^{\wedge} \delta b_{\omega_{k}}\delta t - \frac{1}{2} {R_k^{b_k}}{R}_{o}^{b}{n_{e}}_{k} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}{R}_{o}^{b}{n_{e}}_{k+1} \\ \end{aligned}
δη˙===−21RkbkRobnek−21Rk+1bk+1Robnek+1−21Rkbk[Rob(v^k−nek)]∧δθk−21Rk+1bk+1[Rob(v^k+1−nek+1)]∧δθk+1−21RkbkRobnek−21Rk+1bk+1Robnek+1−21Rkbk[Rob(v^k−nek)]∧δθk−21Rk+1bk+1[Rob(v^k+1−nek+1)]∧{[I−(2ω
k+ω
k+1−bωk)∧δt]δθk−2nωk+nωk+1δt−δtδbωk}{−21Rkbk[Rob(v^k−nek)]∧−21Rk+1bk+1[Rob(v^k+1−nek+1)]∧[I−(2ω
k+ω
k+1−bωk)∧δt]}δθk+41Rk+1bk+1[Rob(v^k+1)]∧nωkδt+41Rk+1bk+1[Rob(v^k+1)]∧nωk+1δt+21Rk+1bk+1[Rob(v^k+1)]∧δbωkδt−21RkbkRobnek−21Rk+1bk+1Robnek+1
δ
η
k
+
1
=
δ
η
k
+
δ
η
˙
δ
t
=
f
31
δ
θ
k
+
f
35
δ
b
ω
k
+
v
31
n
ω
k
+
v
32
n
e
k
+
v
34
n
ω
k
+
1
+
v
35
n
e
k
+
1
\begin{aligned} \delta \eta _{k+1} & = \delta \eta _{k} + \delta \dot{\eta }\delta t \\ & = f_{31}\delta \theta _{k} + f_{35}\delta b_{\omega _{k}} + v_{31}{n_{\omega }}_{k} + v_{32}{n_{e}}_{k} + v_{34}{n_{\omega }}_{k+1} + v_{35}{n_{e}}_{k+1} \end{aligned}
δηk+1=δηk+δη˙δt=f31δθk+f35δbωk+v31nωk+v32nek+v34nωk+1+v35nek+1
其中:
f
31
=
−
1
2
R
k
b
k
[
R
o
b
(
v
^
k
−
n
e
k
)
]
∧
−
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
−
n
e
k
+
1
)
]
∧
[
I
−
(
ω
^
k
+
ω
^
k
+
1
2
−
b
ω
k
)
∧
δ
t
]
δ
t
f
35
=
1
2
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
)
]
∧
δ
t
2
v
31
=
v
34
=
1
4
R
k
+
1
b
k
+
1
[
R
o
b
(
v
^
k
+
1
)
]
∧
δ
t
2
v
32
=
−
1
2
R
k
b
k
R
o
b
δ
t
v
35
=
−
1
2
R
k
+
1
b
k
R
o
b
δ
t
\begin{aligned} &f_{31}=- \frac{1}{2} {R_{k}^{b_k}}[{R}_{o}^{b}({\hat{v}_{k}-{n_{e}}_{k}})]^{\wedge} - \frac{1}{2}{R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}-{n_{e}}_{k+1}})]^{\wedge} \left[I-\left(\frac{\widehat{\omega}_{k}+\widehat{\omega}_{k+1}}{2}-b_{\omega_{k}}\right)^{\wedge} \delta t\right] \delta t \\ &f_{35}=\frac{1}{2} {R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}})]^{\wedge} \delta t^{2} \\ &v_{31}=v_{34}=\frac{1}{4} {R_{k+1}^{b_{k+1}}}[{R}_{o}^{b}({\hat{v}_{k+1}})]^{\wedge} \delta t^{2} \\ &v_{32}=- \frac{1}{2} {R_k^{b_k}}{R}_{o}^{b}\delta t \\ &v_{35}=- \frac{1}{2} {R_{k+1}^{b_k}}{R}_{o}^{b} \delta t \\ \end{aligned}
f31=−21Rkbk[Rob(v^k−nek)]∧−21Rk+1bk+1[Rob(v^k+1−nek+1)]∧[I−(2ω
k+ω
k+1−bωk)∧δt]δtf35=21Rk+1bk+1[Rob(v^k+1)]∧δt2v31=v34=41Rk+1bk+1[Rob(v^k+1)]∧δt2v32=−21RkbkRobδtv35=−21Rk+1bkRobδt
将预积分误差简写成
δ
z
k
+
1
=
F
δ
z
k
+
V
Q
\delta z_{k+1} = F\delta z_{k} + VQ
δzk+1=Fδzk+VQ
则Jacobian的迭代公式为
J
k
+
1
=
F
J
k
J_{k+1}=FJ_{k}
Jk+1=FJk,协方差的迭代公式为
P
k
+
1
=
F
P
k
F
T
+
V
Q
V
T
P_{k+1} = FP_{k}F^{T} + VQV^{T}
Pk+1=FPkFT+VQVT
Q
=
[
σ
a
2
0
0
0
0
0
0
0
0
σ
w
2
0
0
0
0
0
0
0
0
σ
e
2
0
0
0
0
0
0
0
0
σ
a
2
0
0
0
0
0
0
0
0
σ
w
2
0
0
0
0
0
0
0
0
σ
e
2
0
0
0
0
0
0
0
0
σ
b
a
2
0
0
0
0
0
0
0
0
σ
b
w
2
]
Q=\begin{bmatrix} \sigma_{a}^{2} & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & \sigma_{w}^{2} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & \sigma_{e}^{2} & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & \sigma_{a}^{2} & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & \sigma_{w}^2 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & \sigma_{e}^{2} & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{b_{a}}^{2} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \sigma_{b_{w}}^{2} \end{bmatrix}
Q=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡σa200000000σw200000000σe200000000σa200000000σw200000000σe200000000σba200000000σbw2⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤
公式纯手打,若发现有错误,请指出
实践
公式可能看起来多一点,其实是和IMU预积分的公式类似的,对应的代码比较简洁。
配合代码查看
对应代码在预积分文件中 integration_base.h
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0, const Eigen::Vector3d &_enc_v_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1, const Eigen::Vector3d &_enc_v_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Vector3d &delta_v,
const Eigen::Vector3d &delta_eta, 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_delta_eta, Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg,
bool update_jacobian)
{
// 中值法
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;
Vector3d un_enc_0 = delta_q * RIO * _enc_v_0; // encoder
Vector3d un_enc_1 = result_delta_q * RIO * _enc_v_1; // encoder
result_delta_eta = delta_eta + 0.5 * (un_enc_0 + un_enc_1) * _dt; // encoder
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;
Vector3d e_0_x = RIO * _enc_v_0;
Vector3d e_1_x = RIO * _enc_v_1;
Matrix3d R_w_x, R_a_0_x, R_a_1_x, R_e_0_x, R_e_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;
R_e_0_x << 0, -e_0_x(2), e_0_x(1),
e_0_x(2), 0, -e_0_x(0),
-e_0_x(1), e_0_x(0), 0;
R_e_1_x << 0, -e_1_x(2), e_1_x(1),
e_1_x(2), 0, -e_1_x(0),
-e_1_x(1), e_1_x(0), 0;
MatrixXd F = MatrixXd::Zero(18, 18);
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, 12) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;
F.block<3, 3>(0, 15) = -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, 15) = -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, 12) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;
F.block<3, 3>(6, 15) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;
F.block<3, 3>(9, 3) = -0.5 * delta_q.toRotationMatrix() * R_e_0_x * _dt +
-0.5 * result_delta_q.toRotationMatrix() * R_e_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;
F.block<3, 3>(9, 9) = Matrix3d::Identity();
F.block<3, 3>(9, 15) = 0.5 * result_delta_q.toRotationMatrix() * R_e_1_x * _dt * _dt;
F.block<3, 3>(12, 12) = Matrix3d::Identity();
F.block<3, 3>(15, 15) = Matrix3d::Identity();
MatrixXd V = MatrixXd::Zero(18, 24);
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, 9) = 0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;
V.block<3, 3>(0, 12) = V.block<3, 3>(0, 3);
V.block<3, 3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(3, 12) = 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, 9) = 0.5 * result_delta_q.toRotationMatrix() * _dt;
V.block<3, 3>(6, 12) = V.block<3, 3>(6, 3);
V.block<3, 3>(9, 3) = 0.25 * -result_delta_q.toRotationMatrix() * R_e_1_x * _dt * _dt; // 相差-
V.block<3, 3>(9, 6) = 0.5 * delta_q.toRotationMatrix() * RIO * _dt;
V.block<3, 3>(9, 12) = V.block<3, 3>(9, 3);
V.block<3, 3>(9, 15) = 0.5 * result_delta_q.toRotationMatrix() * RIO * _dt;
V.block<3, 3>(12, 18) = MatrixXd::Identity(3,3) * _dt;
V.block<3, 3>(15, 21) = MatrixXd::Identity(3,3) * _dt;
// step_jacobian_enc = F;
// step_V_enc = V;
jacobian_enc = F * jacobian_enc;
covariance_enc = F * covariance_enc * F.transpose() + V * noise_enc * V.transpose();
}
}
参考
- J. Liu, W. Gao and Z. Hu, “Visual-Inertial Odometry Tightly Coupled with Wheel Encoder Adopting Robust Initialization and Online Extrinsic Calibration.”
- K. J. Wu, C. X. Guo, G. Georgiou and S. I. Roumeliotis, “VINS on wheels.”
- 崔华坤:VINS 论文推导及代码解析