MSCKF(二)——预测部分
文章目录
Reference
- A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. MSCKF1.0的论文;
- Quaternion Kinematics for Error-State KF. 关于四元数以及ESKF的论文;
- Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight. S-MSCKF对应的论文;
- https://github.com/KumarRobotics/msckf_vio S-MSCKF的工程;
- https://zhuanlan.zhihu.com/p/76341809 知乎上大佬对MSCKF的总结,本文没有过多的程序方面的讲解,都是从理论上推导的,也是个人的一些解惑过程;
- https://blog.csdn.net/wubaobao1993/article/details/109297640 笔者写的关于两种四元数的表示;
PS: MSCKF的工程算是我见过最贴近论文的工程了,基本上工程中的数学部分在论文里面都给了出来,所以对于MSCKF1.0来说,论文是一定要弄懂的。本篇将参考[1]和参考[3]放在一起进行梳理,因为两者确实很相似,不过在参考[3]中作者对能观性进行了一定程度的补偿。
EKF的状态变量构建
坐标系的表示问题
论文中{G}为世界(global)坐标系,{I}为IMU坐标系(或者称为机体坐标系)。
位姿表示方式
关于位姿的表示,在大多数的SLAM系统中,位姿表示一般为 T = { q I G ∣ t I G } T=\{q^{G}_{I}|t^{G}_{I}\} T={qIG∣tIG},其中旋转部分为Hamilton的思源书表示法;但是MSCKF(或者大部分的EKF-base的VO/VIO系统)中的位姿表示变为了 T = { G I q ∣ p I G } T=\{{}^{I}_{G}q|p^{G}_{I}\} T={GIq∣pIG},其中旋转部分为JPL的四元数表示法,这两种表示方法的区别和联系可以看笔者之前写的参考[6],简单的说就是大佬们为了四元数乘法的一致性,自己又搞了一个表示方法出来,借用参考[2]中的区别图如下:
这里在本文的最后笔者会把两者的具体区别简单推导一下,这里先说比较影响推导的:
-
四元数的微分方程发生了变化,区别如下:
- Hamilton表示法中,四元数的微分方程表示为$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes\Omega(^Lw) $;
- JPL表示法中,四元数的微分方程表示为 q ˙ = 1 2 Ω ( L w ) ⊗ q \dot{\mathbf{q}}=\frac{1}{2}\Omega(^Lw)\otimes \mathbf{q} q˙=21Ω(Lw)⊗q
其中 Ω ( L w ) \Omega(^Lw) Ω(Lw)均用一个纯四元数(pure quaternions)的右乘矩阵形式(这个右乘矩阵是在Hamilton四元数表示法中的):
Ω ( w ) = [ − [ w ] × w − w T 0 ] \Omega(w)=\begin{bmatrix}-[w]_{\times} & w \\ -w^T & 0 \end{bmatrix} Ω(w)=[−[w]×−wTw0]
-
四元数的扰动展开发生了变化:
-
在Hamilton表示法中,扰动四元数表示为
R ( δ q ) ≈ R ( [ 1 , 1 2 δ θ ] ) = I 3 x 3 + [ δ θ ] × \mathrm{R}(\mathbf{\delta{q}}) \approx \mathrm{R}(\left[1, \frac{1}{2}\mathrm{\delta{\theta}}\right])=\mathrm{I_{3x3}}+[\delta{\theta}]_{\times} R(δq)≈R([1,21δθ])=I3x3+[δθ]× -
在JPL表示法中,扰动四元数表示为:
R ( δ q ) ≈ R ( [ 1 , 1 2 δ θ ] ) = I 3 x 3 − [ δ θ ] × \mathrm{R}(\mathbf{\delta{q}}) \approx \mathrm{R}(\left[1, \frac{1}{2}\mathrm{\delta{\theta}}\right])=\mathrm{I_{3x3}}-[\delta{\theta}]_{\times} R(δq)≈R([1,21δθ])=I3x3−[δθ]×
-
-
四元数参数的摄动坐标系的选择:
- 在Hamilton表示法中,因为旋转是PBTW的,所以扰动量为 δ θ L ′ L \delta{\theta}_{L'}^{L} δθL′L,参考系为{L};
- 在JPL表示法中,因为旋转是PWTB的,所以扰动量为 L L ′ δ θ {}_{L}^{L'}\delta{\theta} LL′δθ,参考系也为{L};
IMU状态变量的表示方式
Notation:
以下的参数表示中:
- 若字母头上什么都没有,形如 X \mathbf{X} X的形式,表示该变量为truth-state,这个值为理想值,永远不可能求得;
- 若字母头上有尖号,形如 X ^ \widehat{\mathbf{X}} X ,表示该变量为normal-state,也就是算法的估计值;
- 若字母头上有飘号,形如 X ~ \widetilde{\mathbf{X}} X ,表示该变量为error-state,也就是估计值与真值之间的差值;
- 特别的,对于四元数表示旋转而言,头上都有一个横线,形如 q ‾ \overline{\mathbf{q}} q,表示该四元数是一个单位四元数;
IMU状态变量一如既往的还是用5个变量表示,这里同时加上了相机与IMU的外参,如下:
X
I
M
U
=
[
G
I
q
‾
T
b
g
T
G
v
I
T
b
a
T
G
p
I
T
I
C
q
‾
I
p
C
]
T
(1)
\mathrm{X}_{IMU}=\begin{bmatrix} ^{I}_{G}\overline{q}^T & b_g^{T} & ^{G}v_{I}^{T} & b_a^{T} & ^{G}p_{I}^{T} & ^{C}_{I}\overline{q} & ^{I}p_{C} \end{bmatrix}^{T} \tag{1}
XIMU=[GIqTbgTGvITbaTGpITICqIpC]T(1)
IMU的error-state向量表示如下:
X
~
I
M
U
=
[
G
I
δ
θ
T
b
~
g
T
G
v
~
I
T
b
~
a
T
G
p
~
I
T
I
C
δ
θ
T
I
p
C
T
]
T
(2)
\tilde{\mathrm{X}}_{IMU}=\begin{bmatrix} ^{I}_{G}\delta{\theta^T} & \tilde{b}_g^T & ^{G}\tilde{v}_{I}^T & \tilde{b}_{a}^T & ^{G}\tilde{p}_{I}^{T} & _{I}^{C}\delta{\theta}^T & ^{I}p_{C}^T \end{bmatrix}^{T} \tag{2}
X~IMU=[GIδθTb~gTGv~ITb~aTGp~ITICδθTIpCT]T(2)
这边稍微对Notation进行一下说明:
- 对于旋转的误差向量 δ θ \delta{\theta} δθ来说,我们通常认为其表示绕一个轴向旋转一个角度,这个轴向其实是有参考系的,一般情况下,算法求解的旋转向量的参考系均为估计的机体坐标系(也可以说是摄动坐标系),即 L ^ \mathbf{\hat{L}} L^或者 I ^ \mathbf{\hat{I}} I^,而公式(2)中没有实际的体现出来,误差旋转向量前的字母完全是为了区别开IMU位姿和IMU与Camera外参;
- 对于非旋转的误差向量来说,他们均是在世界坐标系下表示的,所以前面的字母{G}、{I}和{C}均是有意义的,表示它的参考坐标系;
- 公式(2)中的IMU和Camera的相机外参中旋转的定义方向是不同的(实际上,如果按照参考3中的定义,那么它后面AppendixB中的Jacobian是不对的,且S-MSCKF的工程中,它的变量也是用IMU到Camera的旋转方向表示的);
带相机位姿的状态变量表示方式
k时刻的整个滤波器的位姿为:
X
^
k
=
[
X
^
I
M
U
G
C
1
q
‾
^
T
G
p
^
C
1
T
.
.
.
G
C
N
q
‾
^
T
G
p
^
C
N
T
]
T
(3)
\mathrm{\widehat{X}_{k}}=\begin{bmatrix}\mathrm{\widehat{X}_{IMU}} & ^{C_1}_{G}\widehat{\overline{q}}^T & ^{G}\widehat{p}_{C_1}^T & ... & ^{C_N}_{G}\widehat{\overline{q}}^T & ^{G}\widehat{p}_{C_N}^T \end{bmatrix}^T \tag{3}
X
k=[X
IMUGC1q
TGp
C1T...GCNq
TGp
CNT]T(3)
对应的error-state为:
X
~
k
=
[
X
~
I
M
U
δ
θ
C
1
G
p
~
C
1
T
.
.
.
δ
θ
C
N
G
p
~
C
N
T
]
T
(4)
\mathrm{\tilde{X}_{k}}=\begin{bmatrix}\mathrm{\tilde{X}_{IMU}} & \delta{\theta}_{C_1} & ^{G}\tilde{p}_{C_1}^T & ... & \delta{\theta}_{C_N} & ^{G}\tilde{p}_{C_N}^T \end{bmatrix}^T \tag{4}
X~k=[X~IMUδθC1Gp~C1T...δθCNGp~CNT]T(4)
MSCKF位姿估计——估计的变量是什么?
这一小节主要搞清楚MSCKF中的状态量是什么,这个也是理解MSCKF的一个重要的部分。
相比于EKF-Base的方法,MSCKF使用的是ESKF的方式,也就是对误差变量error-state进行估计,而不是对表示位姿的状态变量normal-state进行估计。与Graph-base的方法求解的量是一样的。
使用ESKF的好处是不言而喻的,引用参考2中的原话:
- The orientation error-state is minimal (i.e., it has the same number of parameters as degrees of freedom), avoiding issues related to over-parametrization (or redundancy) and the consequent risk of singularity of the involved covariances matrices, resulting typically from enforcing constraints.
- The error-state system is always operating close to the origin, and therefore far from possible parameter singularities, gimbal lock issues, or the like, providing a guarantee that the linearization validity holds at all times.
- The error-state is always small, meaning that all second-order products are negligible. This makes the computation of Jacobians very easy and fast. Some Jacobians may even be constant or equal to available state magnitudes.
- The error dynamics are slow because all the large-signal dynamics have been inte- grated in the nominal-state. This means that we can apply KF corrections (which are the only means to observe the errors) at a lower rate than the predictions.
MSCKF位姿估计——状态变量的估计
搞清楚了估计的变量是什么之后(一定注意是error-state),下面就可以开始构建整个滤波问题了,但是因为本质上算法还是在求解状态变量(也就是 X ^ \widehat{\mathbf{X}} X ),因此这里先看一下状态变量的递推过程;
IMU姿态的传导(Propagation)
IMU的运动方程
对于IMU而言,姿态传导部分基本上依赖于IMU的运动方程,使用IMU的测量进行最新时刻的位姿的推导,并以此作为机体位姿转移到相机位姿上,在连续时域上,IMU的运动方程如下:
{
G
I
q
ˉ
˙
(
t
)
=
1
2
Ω
(
ω
(
t
)
)
G
I
q
ˉ
(
t
)
b
˙
g
(
t
)
=
n
w
g
(
t
)
G
v
˙
(
t
)
=
G
a
(
t
)
b
˙
a
(
t
)
=
n
w
a
(
t
)
G
p
˙
(
t
)
=
G
v
(
t
)
(5)
\begin{aligned} \begin{cases} ^{I}_{G}\dot{\bar{q}}(t) &=\frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\omega}(t))_{G}^{I} \bar{q}(t) \\ \dot{\mathbf{b}}_{g}(t) &=\mathbf{n}_{w g}(t) \\ ^{G}\dot{\mathbf{v}}(t) &= ^{G}\mathbf{a}(t) \\ \dot{\mathbf{b}}_{a}(t) &=\mathbf{n}_{w a}(t) \\ ^{G}\dot{\mathbf{p}}(t) &= ^{G}\mathbf{v}(t) \end{cases} \end{aligned} \tag{5}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧GIqˉ˙(t)b˙g(t)Gv˙(t)b˙a(t)Gp˙(t)=21Ω(ω(t))GIqˉ(t)=nwg(t)=Ga(t)=nwa(t)=Gv(t)(5)
其中四元数为JPL表示法,
Ω
\Omega
Ω为上面的纯四元数右乘矩阵形式。
公式(5)中的参数均为理想情况下的参数,实际过程中的运动方程为:
{
G
I
q
ˉ
^
˙
(
t
)
=
1
2
Ω
(
ω
^
(
t
)
)
G
I
q
ˉ
(
t
)
b
^
˙
g
(
t
)
=
0
G
v
^
˙
(
t
)
=
C
q
^
T
a
^
−
2
⌊
ω
G
×
⌋
G
v
^
I
−
⌊
ω
G
×
⌋
2
G
p
^
I
+
G
g
b
^
˙
a
(
t
)
=
0
G
p
˙
(
t
)
=
G
v
^
(
t
)
(6)
\begin{aligned} \begin{cases} ^{I}_{G}\dot{\widehat{\bar{q}}}(t) &=\frac{1}{2} \boldsymbol{\Omega}(\boldsymbol{\widehat\omega}(t))_{G}^{I} \bar{q}(t) \\ \dot{\mathbf{\widehat b}}_{g}(t) &=0 \\ ^{G}\dot{\mathbf{\widehat v}}(t) &= \mathbf{C}_{\hat{q}}^{T} \hat{\mathbf{a}}-2\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{G} \hat{\mathbf{v}}_{I}-\left\lfloor\boldsymbol{\omega}_{G} \times\right\rfloor^{2} G_{\hat{\mathbf{p}}_{I}}+{ }^{G} \mathbf{g} \\ \dot{\mathbf{\widehat b}}_{a}(t) &= 0 \\ ^{G}\dot{\mathbf{p}}(t) &= ^{G}\mathbf{\widehat v}(t) \end{cases} \end{aligned} \tag{6}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧GIqˉ
˙(t)b
˙g(t)Gv
˙(t)b
˙a(t)Gp˙(t)=21Ω(ω
(t))GIqˉ(t)=0=Cq^Ta^−2⌊ωG×⌋Gv^I−⌊ωG×⌋2Gp^I+Gg=0=Gv
(t)(6)
其中:
- ω ^ ( t ) = ω m − b ^ g − R w b ω G \widehat \omega(t)=\omega_{m}-\widehat b_{g}-R^{b}_{w}\omega_{G} ω (t)=ωm−b g−RwbωG,表示实际过程中通过测量减去零偏的值,同时作者这里考虑了地球转动的影响;
- a ^ = a m − b ^ a \widehat a=a_m-\widehat b_a a =am−b a,表示实际过程中通过测量减去零偏的值,作者在计算速度的微分时,也考虑了地球转动对于测量值的影响;
- 以上均在连续时域中进行的分析,对连续函数进行离散化时需要在位置上考虑加速度的影响,这样会更加的准确一些(在开源的S-MSCKF代码中是没有考虑地球自传的影响的);
IMU状态积分
由IMU的运动方程可以得到一个非线性的微分方程,如下:
X
˙
I
M
U
=
f
(
t
,
X
I
M
U
)
(7)
\mathrm{\dot{X}_{IMU}}=\mathbf{f}(t, \mathrm{X_{IMU}}) \tag{7}
X˙IMU=f(t,XIMU)(7)
可以使用参考2中的Appendix A中的方法对状态量进行更新,在S-MSCKF中采用的是RK4的方法。
Camera姿态的传导
以上均是IMU位姿的递推过程,因为这个过程中并没有观测,因此相机的位姿都保持不变;
当有新的图像到来的时候,直接用IMU的最新位姿并经过IMU与相机的外参作为相机的最新位姿,公式如下:
G
C
q
^
=
I
C
q
^
⊗
G
I
q
^
,
G
p
^
C
=
G
p
^
C
+
C
(
G
I
q
^
)
⊤
I
p
^
C
(8)
{ }_{G}^{C} \hat{\mathbf{q}}={ }_{I}^{C} \hat{\mathbf{q}} \otimes_{G}^{I} \hat{\mathbf{q}}, \quad{ }^{G} \hat{\mathbf{p}}_{C}={ }^{G} \hat{\mathbf{p}}_{C}+C\left(^{I}_{G}{\hat{q}}\right)^{\top}{ }^{I} \hat{\mathbf{p}}_{C} \tag{8}
GCq^=ICq^⊗GIq^,Gp^C=Gp^C+C(GIq^)⊤Ip^C(8)
MSCKF位姿估计——预测阶段(predict)
这部分着重推导以误差状态变量为参数的EKF(也叫作ESKF)的预测阶段:
IMU误差状态的微分方程
这部分请读者参考参考2第五章的内容,这里直接给出结论,如下,其中考虑了相机与IMU的外参变量:
X
~
˙
I
M
U
=
F
X
~
I
M
U
+
G
n
I
M
U
(9)
\dot{\tilde{\mathbf{X}}}_{\mathrm{IMU}}=\mathbf{F} \tilde{\mathbf{X}}_{\mathrm{IMU}}+\mathbf{G} \mathbf{n}_{\mathrm{IMU}} \tag{9}
X~˙IMU=FX~IMU+GnIMU(9)
其中:
-
F = [ − ⌊ ω ^ × ⌋ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T ⌊ a ^ × ⌋ 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] \mathbf{F}=\left[\begin{array}{ccccc} -\lfloor\hat{\boldsymbol{\omega}} \times\rfloor & -\mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ -\mathbf{C}_{\hat{q}}^{T}\lfloor\hat{\mathbf{a}} \times\rfloor & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{-} \mathbf{C}_{\hat{q}}^{T} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] F=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡−⌊ω^×⌋03×3−Cq^T⌊a^×⌋03×303×303×303×3−I303×303×303×303×303×303×303×303×303×303×3I3030303×303×3−Cq^T03×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×3⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
-
G = [ − I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 − C q ^ T 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 I 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 ] \mathbf{G}=\left[\begin{array}{cccc} -\mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & -\mathbf{C}_{\hat{q}}^{T} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{I}_{3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \\ \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} & \mathbf{0}_{3 \times 3} \end{array}\right] G=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡−I303×303×303×303×303×303×303×3I303×303×303×303×303×303×303×3−Cq^T03×303×303×303×303×303×303×3I303×303×303×3⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤
稍微不同的一点是参考2中考虑了重力,而这里并没有考虑重力。
Notation
公式(6)中考虑了地球的自转(MSCKF1.0论文中的做法),但是公式(9)中没有考虑(S-MSCKF论文中的做法)。
IMU误差状态传递过程的推导
根据MSCKF2.0中的表述,在MSCK1.0中使用的是数值推导而非理论推导,这里先按照1.0中的思路来,之后总结2.0的时候再按照2.0的方法进行推导。
公式(9)表示了误差状态的微分关系,根据线性系统的离散化知识,可以得到误差状态的递推方程为:
X
~
(
t
k
+
1
)
=
Φ
(
t
k
+
1
,
t
k
)
X
~
(
t
k
)
+
∫
t
k
t
k
+
1
Φ
(
t
k
+
1
,
τ
)
G
(
τ
)
n
(
τ
)
d
τ
(10)
\boldsymbol{\tilde{X}}\left(t_{k+1}\right)=\boldsymbol{\Phi}\left(t_{k+1}, t_{k}\right) \boldsymbol{\tilde{X}}\left(t_{k}\right)+\int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, \tau\right) \boldsymbol{G}(\tau) \boldsymbol{n}(\tau) \mathrm{d} \tau \tag{10}
X~(tk+1)=Φ(tk+1,tk)X~(tk)+∫tktk+1Φ(tk+1,τ)G(τ)n(τ)dτ(10)
其中状态传递矩阵
Φ
˙
(
t
k
+
1
,
t
k
)
=
F
(
t
)
Φ
(
t
k
+
1
)
\dot\Phi(t_{k+1}, t_k)=F(t)\Phi(t_{k+1})
Φ˙(tk+1,tk)=F(t)Φ(tk+1),可以明显看到,该状态转移矩阵的闭式解是指数函数,形式为:
Φ
(
t
k
+
1
,
t
k
)
=
exp
∫
t
k
t
k
+
1
F
(
t
)
d
t
(11)
\boldsymbol{\Phi}\left(t_{k+1}, t_{k}\right)=\exp \int_{t_{k}}^{t_{k+1}} \boldsymbol{F}(t) \mathrm{d} t \tag{11}
Φ(tk+1,tk)=exp∫tktk+1F(t)dt(11)
针对公式(8),对协方差矩阵进行推导的话可以得到:
E
[
W
k
W
j
T
]
=
E
[
∫
t
k
t
k
+
1
Φ
(
t
k
+
1
,
t
)
G
(
t
)
w
(
t
)
d
t
⋅
∫
t
j
t
j
+
1
w
T
(
τ
)
G
T
(
τ
)
Φ
T
(
t
k
+
1
,
τ
)
d
τ
]
=
∫
t
k
t
k
+
1
Φ
(
t
k
+
1
,
t
)
G
(
t
)
[
∫
t
j
t
j
+
1
E
[
w
(
t
)
w
T
(
τ
)
]
⋅
G
T
(
τ
)
Φ
T
(
t
k
+
1
,
τ
)
d
τ
]
d
t
=
∫
t
k
t
k
+
1
Φ
(
t
k
+
1
,
t
)
G
(
t
)
[
∫
t
j
t
j
+
1
q
δ
(
t
−
τ
)
G
T
(
τ
)
Φ
(
t
k
+
1
,
τ
)
d
τ
]
d
t
(12)
\begin{aligned} E\left[\boldsymbol{W}_{k} \boldsymbol{W}_{j}^{\mathrm{T}}\right]=& E\left[\int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, t\right) \boldsymbol{G}(t) \boldsymbol{w}(t) \mathrm{d} t \cdot \int_{t_{j}}^{t_{j+1}} \boldsymbol{w}^{\mathrm{T}}(\tau) \boldsymbol{G}^{\mathrm{T}}(\tau) \boldsymbol{\Phi}^{\mathrm{T}}\left(t_{k+1}, \tau\right) \mathrm{d} \tau\right] \\ &= \int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, t\right) \boldsymbol{G}(t)\left[\int_{t_{j}}^{t_{j+1}} E\left[\boldsymbol{w}(t) \boldsymbol{w}^{\mathrm{T}}(\tau)\right] \cdot \boldsymbol{G}^{\mathrm{T}}(\tau) \boldsymbol{\Phi}^{\mathrm{T}}\left(t_{k+1}, \tau\right) \mathrm{d} \tau\right] \mathrm{d} t\\ &= \int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, t\right) \boldsymbol{G}(t)\left[\int_{t_{j}}^{t_{j+1}} \boldsymbol{q} \delta(t-\tau) \boldsymbol{G}^{\mathrm{T}}(\tau) \boldsymbol{\Phi}\left(t_{k+1}, \tau\right) \mathrm{d} \tau\right] \mathrm{d} t \end{aligned} \tag{12}
E[WkWjT]=E[∫tktk+1Φ(tk+1,t)G(t)w(t)dt⋅∫tjtj+1wT(τ)GT(τ)ΦT(tk+1,τ)dτ]=∫tktk+1Φ(tk+1,t)G(t)[∫tjtj+1E[w(t)wT(τ)]⋅GT(τ)ΦT(tk+1,τ)dτ]dt=∫tktk+1Φ(tk+1,t)G(t)[∫tjtj+1qδ(t−τ)GT(τ)Φ(tk+1,τ)dτ]dt(12)
于是看到,当
t
!
=
τ
t!=\tau
t!=τ的时候,因为狄更斯函数的关系导致积分值为0;而当
t
=
=
τ
t==\tau
t==τ的时候,整个积分不为0:
E
[
W
k
W
j
T
]
∣
j
=
k
=
∫
t
k
t
k
+
1
Φ
(
t
k
+
1
,
t
)
G
(
t
)
q
G
T
(
t
)
Φ
T
(
t
k
+
1
,
t
)
d
t
(13)
\left.E\left[\boldsymbol{W}_{k} \boldsymbol{W}_{j}^{\mathrm{T}}\right]\right|_{j=k}=\int_{t_{k}}^{t_{k+1}} \boldsymbol{\Phi}\left(t_{k+1}, t\right) \boldsymbol{G}(t) \boldsymbol{q} \boldsymbol{G}^{\mathrm{T}}(t) \boldsymbol{\Phi}^{\mathrm{T}}\left(t_{k+1}, t\right) \mathrm{d} t \tag{13}
E[WkWjT]∣∣∣j=k=∫tktk+1Φ(tk+1,t)G(t)qGT(t)ΦT(tk+1,t)dt(13)
对于离散情况而言,如果采样周期足够短,我们可以简单的假设在
t
∈
[
t
k
,
t
k
+
1
]
t \in [t_k, t_{k+1}]
t∈[tk,tk+1]这段时间内,转移矩阵
Φ
\Phi
Φ和驱动矩阵
G
G
G均不变化,于是整个误差状态传递过程就比较明了了:
- 通过公式(9)获得误差状态微分方程的传递矩阵 F ( t ) \mathbf{F(t)} F(t);
- 通过公式(11)获得误差状态的转移矩阵 Φ ( t k + 1 , t k ) \mathbf{\Phi(t_{k+1}, t_k)} Φ(tk+1,tk),这里通常使用泰勒展开,论文中保留到了三阶;
- 通过公式(13)获得误差状态的系统协方差矩阵 Q \mathrm{Q} Q;
IMU误差状态的预测部分
由上面的推导可以得到以IMU误差状态为估计量的KF的预测过程(这里直接在离散时域上推导):
{
x
~
k
+
1
I
M
U
=
Φ
(
k
+
1
,
k
)
x
~
k
I
M
U
P
k
+
1
∣
k
I
M
U
=
Φ
(
k
+
1
,
k
)
P
k
∣
k
I
M
U
Φ
(
k
+
1
,
k
)
T
+
Φ
(
k
+
1
,
k
)
G
N
G
T
Φ
(
k
+
1
,
k
)
T
(14)
\begin{aligned} \begin{cases} \mathrm{\tilde{x}_{k+1}^{IMU}} &= \Phi(\mathrm{k+1},\mathrm{k})\mathrm{\tilde{x}_k^{IMU}} \\ \mathrm{P_{k+1|k}^{IMU}} &=\Phi(\mathrm{k+1},\mathrm{k})\mathrm{P_{k|k}^{IMU}}\Phi(\mathrm{k+1},\mathrm{k})^T + \Phi(\mathrm{k+1},\mathrm{k})\mathrm{G}\mathrm{N}\mathrm{G}^T\Phi(\mathrm{k+1},\mathrm{k})^T \end{cases} \end{aligned} \tag{14}
{x~k+1IMUPk+1∣kIMU=Φ(k+1,k)x~kIMU=Φ(k+1,k)Pk∣kIMUΦ(k+1,k)T+Φ(k+1,k)GNGTΦ(k+1,k)T(14)
其中:
- 转移矩阵 Φ ( k + 1 , k ) \Phi(k+1, k) Φ(k+1,k)由公式(11)算的的闭式解表示,程序中采用了泰勒展开的前三阶表示;
- 矩阵G为微分方程的噪声驱动矩阵,这里假设在k到k+1时刻,该矩阵恒定不变;
- 噪声协方差矩阵N是IMU的固有属性,这些值可以由数据手册获得,也可以根据实际数据估计一下;
Camera误差状态的预测部分
因为在预测阶段并没有相机姿态的信息,因此对于k+1时刻之前的误差状态其实都是不变的,对于最新时刻的相机位姿的误差状态,算法直接给0就可以了,而且其实对于ESKF而言,每次使用观测更新完误差变量之后,当次的误差变量是要reset为0的,所以其实每次的预测方程我们仅仅关注协方差矩阵的部分就足够了,所以下面主要分三个部分推导一下协方差的更新:
k+1帧之前的Camera自身的协方差更新
由于Camera这部分没有任何的信息,因此对于预测部分,可以认为其状态转移矩阵为单位阵,噪声驱动矩阵为全零矩阵,所以Camera的协方差不做变化;
P
k
+
1
∣
k
C
C
=
P
k
C
C
(15)
\mathrm{P_{k+1|k}^{CC}}=\mathrm{P_{k}^{CC}} \tag{15}
Pk+1∣kCC=PkCC(15)
k+1帧之前的IMU与Camera的协方差更新
该部分因为IMU的状态更新了,因此IMU方差(这里说是方差是想把IMU与Camera看做两个简单的变量)发生变化,导致IMU与Camera的协方差变化,如下:
P
k
+
1
∣
k
I
C
=
E
(
(
x
~
k
+
1
∣
k
I
M
U
−
x
~
^
k
+
1
∣
k
I
M
U
)
(
x
~
k
+
1
∣
k
C
A
M
−
x
~
^
k
+
1
∣
k
C
A
M
)
)
=
E
(
Φ
(
k
+
1
,
k
)
(
x
~
k
I
M
U
−
x
~
^
k
I
M
U
)
(
x
~
k
C
A
M
−
x
~
^
k
C
A
M
)
)
=
Φ
(
k
+
1
,
k
)
E
(
(
x
~
k
I
M
U
−
x
~
^
k
I
M
U
)
(
x
~
k
C
A
M
−
x
~
^
k
C
A
M
)
)
=
Φ
(
k
+
1
,
k
)
P
k
I
C
(16)
\begin{aligned} \mathrm{P_{k+1|k}^{IC}}&=\mathrm{E}((\mathrm{\tilde{x}^{IMU}_{k+1|k}-\hat{\tilde{x}}^{IMU}_{k+1|k}})(\mathrm{\tilde{x}^{CAM}_{k+1|k}-\hat{\tilde{x}}^{CAM}_{k+1|k}})) \\ &=\mathrm{E}(\Phi(k+1,k)(\mathrm{\tilde{x}^{IMU}_{k}-\hat{\tilde{x}}^{IMU}_{k}})(\mathrm{\tilde{x}^{CAM}_{k}-\hat{\tilde{x}}^{CAM}_{k}})) \\ &=\Phi(k+1, k)\mathrm{E}((\mathrm{\tilde{x}^{IMU}_{k}-\hat{\tilde{x}}^{IMU}_{k}})(\mathrm{\tilde{x}^{CAM}_{k}-\hat{\tilde{x}}^{CAM}_{k}})) \\ &=\Phi(k+1,k)\mathrm{P_{k}^{IC}} \end{aligned} \tag{16}
Pk+1∣kIC=E((x~k+1∣kIMU−x~^k+1∣kIMU)(x~k+1∣kCAM−x~^k+1∣kCAM))=E(Φ(k+1,k)(x~kIMU−x~^kIMU)(x~kCAM−x~^kCAM))=Φ(k+1,k)E((x~kIMU−x~^kIMU)(x~kCAM−x~^kCAM))=Φ(k+1,k)PkIC(16)
k+1帧的协方差初值
对于k+1帧来说,因为是从IMU的位姿直接拿过来使用的,因此k+1帧的误差向量初值与当前IMU的误差向量的协方差息息相关,具体关系主要由公式(8)得到:
设k+1帧的误差向量为 G C δ θ ^{C}_{G}\delta{\theta} GCδθ和 G p ~ C \mathrm{^{G}{\tilde{p}}_{C}} Gp~C,该误差向量与IMU的误差向量关系如下:
旋转的误差向量
四元数的推导因为涉及到左乘矩阵和右乘矩阵等等,推导有些繁琐,这里使用旋转矩阵的方法:
R
(
G
C
δ
θ
)
=
I
3
x
3
−
[
G
C
δ
θ
]
×
=
R
G
C
R
G
C
^
T
=
R
(
I
C
δ
θ
)
R
^
I
C
R
(
G
I
δ
θ
)
R
^
G
I
⏟
R
G
C
(
R
^
I
C
R
^
G
I
⏟
R
G
C
^
)
T
=
(
I
3
x
3
−
[
I
C
δ
θ
]
×
)
R
^
I
C
(
I
3
x
3
−
[
G
I
δ
θ
]
×
)
(
R
^
I
C
)
T
=
I
3
x
3
−
[
I
C
δ
θ
]
×
−
R
^
I
C
[
G
I
δ
θ
]
×
(
R
^
I
C
)
T
=
I
3
x
3
−
[
I
C
δ
θ
]
×
−
[
R
^
I
C
G
I
δ
θ
]
×
+
o
(
2
)
\begin{aligned} \mathrm{R(_{G}^{C}\delta{\theta})} = \mathrm{I_{3x3}}-[_{G}^{C}\delta{\theta}]_{\times} &= \mathrm{R_{G}^{C}}\mathrm{\hat{R_{G}^{C}}}^T \\ &=\underbrace{\mathrm{R(_{I}^{C}\delta{\theta})}\mathrm{\hat{R}_{I}^{C}}\mathrm{R(_{G}^{I}\delta{\theta})\mathrm{\hat{R}_{G}^{I}}}}_{\mathrm{R_{G}^{C}}} (\underbrace{\mathrm{\hat{R}_{I}^{C}}\mathrm{\hat{R}_{G}^{I}}}_{\mathrm{\hat{R_{G}^{C}}}})^T \\ &=(\mathrm{I_{3x3}}-[_{I}^{C}\delta{\theta}]_{\times})\mathrm{\hat{R}_{I}^{C}}(\mathrm{I_{3x3}}-[_{G}^{I}\delta{\theta}]_{\times})(\mathrm{\hat{R}_{I}^{C}})^T \\ &=\mathrm{I_{3x3}}-[_{I}^{C}\delta{\theta}]_{\times}-\mathrm{\hat{R}_{I}^{C}}[_{G}^{I}\delta{\theta}]_{\times}(\mathrm{\hat{R}_{I}^{C}})^T \\ &= \mathrm{I_{3x3}}-[_{I}^{C}\delta{\theta}]_{\times}-[\mathrm{\hat{R}_{I}^{C}}_{G}^{I}\delta{\theta}]_{\times} + o(2) \end{aligned}
R(GCδθ)=I3x3−[GCδθ]×=RGCRGC^T=RGC
R(ICδθ)R^ICR(GIδθ)R^GI(RGC^
R^ICR^GI)T=(I3x3−[ICδθ]×)R^IC(I3x3−[GIδθ]×)(R^IC)T=I3x3−[ICδθ]×−R^IC[GIδθ]×(R^IC)T=I3x3−[ICδθ]×−[R^ICGIδθ]×+o(2)
省略最后的二阶无穷小,并且等号左右两边消元,将反对称矩阵映射回向量空间可以推得:
G
C
δ
θ
=
R
^
I
C
G
I
δ
θ
+
I
C
δ
θ
(17)
_{G}^{C}\delta{\theta} = \mathrm{\hat{R}_{I}^{C}} {}_{G}^{I}\delta{\theta} + {}_{I}^{C}\delta{\theta} \tag{17}
GCδθ=R^ICGIδθ+ICδθ(17)
位置的误差向量
G p ~ C = G p C − G p ^ C = G p ^ I + G p ~ I + ( R ( G I δ θ ) R ^ G I ) T ( I p ^ C + I p ~ C ) ⏟ G p C − ( G p ^ I + ( R ^ G I ) T I p ^ C ⏟ G p ^ C ) = G p ~ I + R ^ G I T ( I 3 x 3 + [ G I δ θ ] × ) I p ^ C + R ^ G I T ( I 3 x 3 + [ G I δ θ ] × ) I p ~ C − ( R ^ G I ) T I p ^ C = G p ~ I + R ^ G I T ( [ G I δ θ ] × ) I p ^ C + R ^ G I T p ~ C + o ( 2 ) \begin{aligned} ^{G}\mathrm{\tilde{p}_{C}} &= ^{G}\mathrm{{p}_{C}}-^{G}\mathrm{\hat{p}_{C}} \\ &= \underbrace{^{G}\mathrm{\hat{p}_{I}}+^{G}\mathrm{\tilde{p}_{I}}+(\mathrm{R(_{G}^{I}\delta{\theta})\hat{R}_{G}^{I})^{T}(^{I}\mathrm{\hat{p}_{C}}+^{I}\mathrm{\tilde{p}_{C}})}}_{^{G}\mathrm{{p}_{C}}}-(\underbrace{^{G}\mathrm{\hat{p}_{I}}+(\mathrm{\hat{R}_{G}^{I})^{T} {}^{I}\mathrm{\hat{p}_{C}}}}_{^{G}\mathrm{\hat{p}_{C}}}) \\ &= ^{G}\mathrm{\tilde{p}_{I}}+\mathrm{\hat{R}_{G}^{I}}^{T}(\mathrm{I_{3x3}}+[_{G}^{I}\delta{\theta}]_{\times})^{I}\mathrm{\hat{p}_{C}}+\mathrm{\hat{R}_{G}^{I}}^{T}(\mathrm{I_{3x3}}+[_{G}^{I}\delta{\theta}]_{\times})^{I}\mathrm{\tilde{p}_{C}}-(\mathrm{\hat{R}_{G}^{I})^{T} {}^{I}\mathrm{\hat{p}_{C}}} \\ &= ^{G}\mathrm{\tilde{p}_{I}}+\mathrm{\hat{R}_{G}^{I}}^{T}([_{G}^{I}\delta{\theta}]_{\times})^{I}\mathrm{\hat{p}_{C}}+\mathrm{\hat{R}_{G}^{I}}^{T}\mathrm{\tilde{p}_{C}}+o(2) \end{aligned} Gp~C=GpC−Gp^C=GpC Gp^I+Gp~I+(R(GIδθ)R^GI)T(Ip^C+Ip~C)−(Gp^C Gp^I+(R^GI)TIp^C)=Gp~I+R^GIT(I3x3+[GIδθ]×)Ip^C+R^GIT(I3x3+[GIδθ]×)Ip~C−(R^GI)TIp^C=Gp~I+R^GIT([GIδθ]×)Ip^C+R^GITp~C+o(2)
省略二阶无穷小并根据反对称矩阵相乘的性质得:
G
p
~
C
=
−
R
^
G
I
T
(
[
I
p
^
C
]
×
)
G
I
δ
θ
+
G
p
~
I
+
R
^
G
I
T
p
~
C
(18)
^{G}\mathrm{\tilde{p}_{C}} = -\mathrm{\hat{R}_{G}^{I}}^{T}([{}^{I}\mathrm{\hat{p}_{C}}]_{\times}) {}_{G}^{I}\delta{\theta} +^{G}\mathrm{\tilde{p}_{I}}+\mathrm{\hat{R}_{G}^{I}}^{T}\mathrm{\tilde{p}_{C}} \tag{18}
Gp~C=−R^GIT([Ip^C]×)GIδθ+Gp~I+R^GITp~C(18)
Camera与IMU的状态转移方程
根据公式(17)和(18)很容易得到Camera的误差向量与IMU的误差向量的关系:
X
~
k
+
1
∣
k
C
A
M
=
[
G
C
δ
θ
G
p
~
C
]
=
[
R
^
I
C
0
3
x
3
0
3
x
3
0
3
x
3
0
3
x
3
I
3
x
3
0
3
x
3
−
R
^
G
I
T
(
[
I
p
^
C
]
×
)
0
3
x
3
0
3
x
3
0
3
x
3
I
3
x
3
0
3
x
3
R
^
G
I
T
]
[
G
I
δ
θ
b
~
g
G
v
~
I
b
~
a
G
p
~
I
I
C
δ
θ
I
p
C
]
=
J
I
M
U
C
A
M
X
~
k
+
1
∣
k
I
M
U
(19)
\mathrm{\tilde{X}^{CAM}_{k+1|k}}=\begin{bmatrix} {}_{G}^{C}\delta{\theta} \\ ^{G}\tilde{p}_{C} \end{bmatrix}=\begin{bmatrix}\mathrm{\hat{R}_{I}^{C}} & \mathbf{0}_{3x3} & \mathbf{0}_{3x3} & \mathbf{0}_{3x3} & \mathbf{0}_{3x3} & \mathbf{I}_{3x3} & \mathbf{0}_{3x3} \\ -\mathrm{\hat{R}_{G}^{I}}^{T}([{}^{I}\mathrm{\hat{p}_{C}}]_{\times}) & \mathbf{0}_{3x3} & \mathbf{0}_{3x3} & \mathbf{0}_{3x3} & \mathbf{I}_{3x3} & \mathbf{0}_{3x3} & \mathrm{\hat{R}_{G}^{I}}^{T} \end{bmatrix} \begin{bmatrix} ^{I}_{G}\delta{\theta} \\ \tilde{b}_g \\ ^{G}\tilde{v}_{I} \\ \tilde{b}_{a} \\ ^{G}\tilde{p}_{I} \\ _{I}^{C}\delta{\theta} \\ ^{I}p_{C} \end{bmatrix} = \mathbf{J}^{CAM}_{IMU} \mathrm{\tilde{X}^{IMU}_{k+1|k}} \tag{19}
X~k+1∣kCAM=[GCδθGp~C]=[R^IC−R^GIT([Ip^C]×)03x303x303x303x303x303x303x3I3x3I3x303x303x3R^GIT]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡GIδθb~gGv~Ib~aGp~IICδθIpC⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤=JIMUCAMX~k+1∣kIMU(19)
于是根据线性系统的理论,k+1时刻Camera的协方差为:
P
k
+
1
∣
k
C
A
M
=
J
I
M
U
C
A
M
P
k
+
1
∣
k
I
M
U
(
J
I
M
U
C
A
M
)
T
(20)
\mathrm{P}^{CAM}_{k+1|k}=\mathbf{J}_{IMU}^{CAM} \mathrm{P}^{IMU}_{k+1|k} (\mathbf{J}_{IMU}^{CAM})^T \tag{20}
Pk+1∣kCAM=JIMUCAMPk+1∣kIMU(JIMUCAM)T(20)
预测部分总结
结合公式(14)(15)(16)(20),容易得出,在预测阶段,协方差的传导(propagation)可以分为两步:
-
最新帧没有到来的更新:
P k + 1 ∣ k = [ P k + 1 ∣ k I M U Φ ( k + 1 , k ) P k ∣ k I C ( Φ ( k + 1 , k ) P k ∣ k I C ) T P k ∣ k C A M ] (21) \mathrm{P}_{k+1|k}=\begin{bmatrix} \mathrm{P}^{IMU}_{k+1|k} & \Phi(k+1, k)\mathrm{P}^{IC}_{k|k} \\ (\Phi(k+1, k)\mathrm{P}^{IC}_{k|k})^T & \mathrm{P}^{CAM}_{k|k} \end{bmatrix} \tag{21} Pk+1∣k=[Pk+1∣kIMU(Φ(k+1,k)Pk∣kIC)TΦ(k+1,k)Pk∣kICPk∣kCAM](21) -
最新帧到来,进行状态扩展(state augmentation):
P k + 1 ∣ k ← [ I 6 N + 21 J ] P k + 1 ∣ k [ I 6 N + 21 J ] T (22) \mathbf{P}_{k+1 \mid k} \leftarrow\left[\begin{array}{c} \mathbf{I}_{6 N+21} \\ \mathbf{J} \end{array}\right] \mathbf{P}_{k+1 \mid k}\left[\begin{array}{c} \mathbf{I}_{6 N+21} \\ \mathbf{J} \end{array}\right]^{T} \tag{22} Pk+1∣k←[I6N+21J]Pk+1∣k[I6N+21J]T(22)
这个地方和论文中也不一样,论文中的协方差均是 P k ∣ k \mathbf{P}_{k|k} Pk∣k,但是实际上应该是经过预测步骤更新过的协方差矩阵进行扩充;
总结
本文主要介绍了MSCKF的预测部分,重点主要是:
- 对于MSCKF而言,待估计量为Error-State;
- 由IMU的运动方程可以对Normal-State进行积分;
- 由Error-State的微分方程可以推导得到状态转移方程的闭式解,进而构成了整个Kalman Filter的预测部分;