文章目录
- IMU的测量值误差模型
- IMU预积分真实模型
- IMU预积分估计模型
- 误差模型
- 普通增量积分
- 中值积分法
- 参考文献
IMU的测量值误差模型
IMU的测量值误差模型：
a
^
t
=
a
t
+
R
w
t
g
w
+
b
a
t
+
n
a
t
ω
^
t
=
ω
t
+
b
ω
t
+
n
ω
t
\begin{array}{} {{{\hat a}_t} = {a_t} + R_w^t{g^w} + {b_{{a_t}}} + {n_{a_t}}}\\ {{{\hat \omega }_t} = {\omega _t} + {b_{{\omega _t}}} + {n_{\omega _t} }} \end{array}
a^t=at+Rwtgw+bat+natω^t=ωt+bωt+nωt
其中， a ^ t , ω ^ t {\hat a}_t, {\hat \omega }_t a^t,ω^t分别表示加速度计计陀螺仪的测量值； b a t , b ω t b_{a_t}, b_{\omega_t} bat,bωt分别为加速度计，陀螺仪的零偏，该零偏是随机游走的，零偏的导数服从高斯分布，设为：
b ˙ a t = n b a 和 b ˙ ω t = n b ω {\dot{b}}_{a_t}=n_{b_a}和 {\dot{b}}_{\omega_t}=n_{b_\omega} b˙at=nba和b˙ωt=nbω
b
^
a
t
,
b
^
ω
t
{\hat{b}}_{a_t},{\hat{b}}_{\omega_t}
b^at,b^ωt表示额定的零偏估计值。真实的零偏是包含了游走偏差的，而加速度计和陀螺仪需要体现实际的物理量，要去除噪声所带来的影响。
此外，
n
a
t
,
n
ω
t
n_{a_t}, n_{\omega _t}
nat,nωt 分别为加速度计，陀螺仪的高斯噪声。
g
w
g^w
gw为重力加速度在世界坐标系下的表示。下标
t
t
t表示t时刻。
据上述公式可知，IMU的测量误差主要由加速度计及陀螺仪的噪声及零偏的随机游走所构成。使用IMU的积分数据量来估计位姿，很难准确测量出随机偏差的真实值，从而影响IMU积分量。参考文献《Quaternion kinematics for the error-state Kalman filter》同样我们建立两套模型，一套真实模型(考虑随机噪声)，一套为估计模型(未考虑噪声，表示实际积分情况，用 ⋅ ^ \widehat \cdot ⋅ 表示)。
VINS预积分发生在两帧图像(关键)帧之间(如，
[
k
,
k
+
1
]
[k,k+1]
[k,k+1])，即将两帧图像发布时间内，IMU的数据进行积分(将[k,k+1]时刻内的IMU数据进行积分，从而得到IMU两帧图像之间的位姿变化量)。
因此状态的传递，误差的传递均是指两个图像帧之间的IMU积分值的传递，以下的预积分均以
[
k
,
k
+
1
]
[k,k+1]
[k,k+1]为例。
IMU预积分真实模型
p b k + 1 w = p b k w + v b k w Δ t k + ∫ ∫ t ∈ [ k , k + 1 ] [ R t w ( a ^ t − b a t − n a t ) − g w ] d t 2 v b k + 1 w = v b k w + ∫ t ∈ [ k , k + 1 ] [ R t w ( a ^ t − b a t − n a t ) − g w ] d t q b k + 1 w = q b k w ⊗ ∫ t ∈ [ k , k + 1 ] 1 2 Ω ( ω ^ t − b ω t − n ω t ) q t b k d t \begin{array}{} {p_{{b_{k + 1}}}^w = p_{{b_k}}^w + v_{{b_k}}^w{\rm{\Delta }}{t_k} +\int\!\!\!\int \nolimits_{t \in [k,k + 1]} [R_t^w({{\hat a}_t} - {b_{{a_t}}} - {n_{{a_t}}}) - {g^w}]d{t^2}}\\ {v_{{b_{k + 1}}}^w = v_{{b_k}}^w + \smallint \nolimits_{t \in [k,k + 1]} [R_t^w({{\hat a}_t} - {b_{{a_t}}} - {n_{{a_t}}}) - {g^w}]dt}\\ {q_{{b_{k + 1}}}^w = q_{{b_k}}^w \otimes \smallint \nolimits_{t \in [k,k + 1]} \frac{1}{2}{\rm{\Omega }}({{\hat \omega }_t} - {b_{{\omega _t}}} - {n_{{\omega _t}}})q_t^{{b_k}}dt} \end{array} pbk+1w=pbkw+vbkwΔtk+∫∫t∈[k,k+1][Rtw(a^t−bat−nat)−gw]dt2vbk+1w=vbkw+∫t∈[k,k+1][Rtw(a^t−bat−nat)−gw]dtqbk+1w=qbkw⊗∫t∈[k,k+1]21Ω(ω^t−bωt−nωt)qtbkdt
转换到体坐标系下，其PVQ形式如下：
其中：
α
b
k
+
1
b
k
=
∫
∫
t
∈
[
t
k
,
t
k
+
1
]
R
t
b
k
(
a
^
t
−
b
a
t
−
n
a
t
)
d
t
2
β
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
R
t
b
k
(
a
^
t
−
b
a
t
−
n
a
t
)
d
t
γ
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Ω
(
ω
^
t
−
b
ω
t
−
n
ω
t
)
γ
t
b
k
d
t
\begin{array}{} {\alpha _{{b_{k + 1}}}^{{b_k}} = \int\!\!\!\int \nolimits_{t \in [{t_k},{t_{k + 1}}]} R_t^{{b_k}}({{\hat a}_t} - {b_{{a_t}}} - {n_{{a_t}}})d{t^2}}\\ {\beta _{{b_{k + 1}}}^{{b_k}} = \smallint \nolimits_{t \in [{t_k},{t_{k + 1}}]} R_t^{{b_k}}({{\hat a}_t} - {b_{{a_t}}} - {n_{{a_t}}})dt}\\ {\gamma _{{b_{k + 1}}}^{{b_k}} = \smallint \nolimits_{t \in [{t_k},{t_{k + 1}}]} \frac{1}{2}{\rm{\Omega }}({{\hat \omega }_t} - {b_{{\omega _t}}} - {n_{{\omega _t}}})\gamma _t^{{b_k}}dt} \end{array}
αbk+1bk=∫∫t∈[tk,tk+1]Rtbk(a^t−bat−nat)dt2βbk+1bk=∫t∈[tk,tk+1]Rtbk(a^t−bat−nat)dtγbk+1bk=∫t∈[tk,tk+1]21Ω(ω^t−bωt−nωt)γtbkdt
在VIO系统中，IMU的发布频率通常是高于图像的，VINS系统中，使用k来表示对应的图像帧时刻，通过积分两个图像帧之间的IMU数据，来与视觉估计的位姿进行对比，来得到更加精确的位姿。
在连续积分模型时，时间连续，用
t
t
t作为标识，而当使用实际离散积分形式时，
i
i
i表示
k
k
k到
k
+
1
k+1
k+1图像帧之间的IMU发布时间。
b
k
,
b
k
+
1
b_k, b_{k+1}
bk,bk+1为
k
,
k
+
1
k,k+1
k,k+1时刻对应的IMU坐标系。
α
i
+
1
b
k
=
α
i
b
k
+
β
i
b
k
δ
t
+
1
2
α
‾
i
′
δ
t
2
β
i
+
1
b
k
=
β
i
b
k
+
α
‾
i
′
δ
t
γ
i
+
1
b
k
=
γ
i
b
k
⊗
γ
i
+
1
i
=
γ
i
b
k
⊗
[
1
1
2
ω
‾
i
′
δ
t
]
\begin{array}{c} & {\alpha_{i + 1}^{b_{k}} = \alpha_{i}^{b_{k}} + \beta_{i}^{b_{k}}\delta t + \frac{1}{2}{\overline{\alpha}}_{i}^{'}\delta t^{2}} \\ & {\beta_{i + 1}^{b_{k}} = \beta_{i}^{b_{k}} + {\overline{\alpha}}_{i}^{'}\delta t} \\ & {\gamma_{i + 1}^{b_{k}} = \gamma_{i}^{b_{k}} \otimes \gamma_{i + 1}^{i} = \gamma_{i}^{b_{k}} \otimes \left\lbrack \begin{array}{r} 1 \\ {\frac{1}{2}{\overline{\omega}}_{i}^{'}\delta t} \end{array} \right\rbrack} \end{array}
αi+1bk=αibk+βibkδt+21αi′δt2βi+1bk=βibk+αi′δtγi+1bk=γibk⊗γi+1i=γibk⊗[121ωi′δt]
当使用普通离散增量形式时：
a
ˉ
i
′
=
a
^
i
−
b
a
i
−
n
a
i
,
ω
ˉ
i
′
=
ω
^
i
−
b
ω
i
−
n
ω
i
{\bar a_i}^\prime = {\hat a_i} - {b_{{a_i}}} - {n_{{a_i}}},{\bar \omega _i}^\prime = {\hat \omega _i} - {b_{{\omega _i}}} - {n_{{\omega _i}}}
aˉi′=a^i−bai−nai,ωˉi′=ω^i−bωi−nωi
利用中值积分时，
a
ˉ
i
′
=
1
2
[
q
i
(
a
^
i
−
b
a
i
−
n
a
i
)
+
q
i
+
1
(
a
^
i
+
1
−
b
a
i
−
n
a
i
)
]
{\bar a_i}^\prime = \frac{1}{2}[{q_i}({\hat a_i} - {b_{{a_i}}} - {n_{{a_i}}}) + {q_{i + 1}}({\hat a_{i + 1}} - {b_{{a_i}}} - {n_{{a_i}}})]
aˉi′=21[qi(a^i−bai−nai)+qi+1(a^i+1−bai−nai)]
ω
ˉ
i
′
=
1
2
(
ω
^
i
+
ω
^
i
+
1
)
−
b
^
ω
{\bar \omega _i}^\prime = \frac{1}{2}({\hat \omega _i} + {\hat \omega _{i + 1}}) - {\hat b_\omega }
ωˉi′=21(ω^i+ω^i+1)−b^ω
IMU预积分估计模型
首先，不考虑高斯噪声，世界坐标系下的PVQ积分形式如下：
p
^
b
k
+
1
w
=
p
^
b
k
w
+
v
b
k
w
Δ
t
k
+
∫
∫
t
∈
[
k
,
k
+
1
]
[
R
t
w
(
a
^
t
−
b
^
a
t
)
−
g
w
]
d
t
2
v
^
b
k
+
1
w
=
v
^
b
k
w
+
∫
t
∈
[
k
,
k
+
1
]
[
R
t
w
(
a
^
t
−
b
^
a
t
)
−
g
w
]
d
t
q
^
b
k
+
1
w
=
q
^
b
k
w
⊗
∫
t
∈
[
k
,
k
+
1
]
1
2
Ω
(
ω
^
t
−
b
^
ω
t
)
q
^
t
b
k
d
t
\begin{array}{} {\hat p_{{b_{k + 1}}}^w = \hat p_{{b_k}}^w + v_{{b_k}}^w{\rm{\Delta }}{t_k} + \int\!\!\!\int \nolimits_{t \in [k,k + 1]} [R_t^w({{\hat a}_t} - {{\hat b}_{{a_t}}}) - {g^w}]d{t^2}}\\ {\hat v_{{b_{k + 1}}}^w = \hat v_{{b_k}}^w + \smallint \nolimits_{t \in [k,k + 1]} [R_t^w({{\hat a}_t} - {{\hat b}_{{a_t}}}) - {g^w}]dt}\\ {\hat q_{{b_{k + 1}}}^w = \hat q_{{b_k}}^w \otimes \smallint \nolimits_{t \in [k,k + 1]} \frac{1}{2}{\rm{\Omega }}({{\hat \omega }_t} - {{\hat b}_{{\omega _t}}})\hat q_t^{{b_k}}dt} \end{array}
p^bk+1w=p^bkw+vbkwΔtk+∫∫t∈[k,k+1][Rtw(a^t−b^at)−gw]dt2v^bk+1w=v^bkw+∫t∈[k,k+1][Rtw(a^t−b^at)−gw]dtq^bk+1w=q^bkw⊗∫t∈[k,k+1]21Ω(ω^t−b^ωt)q^tbkdt
转换到体坐标系下，其PVQ形式如下：
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
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
q
^
w
b
k
⊗
q
^
b
k
+
1
w
=
γ
^
b
k
+
1
b
k
\begin{array}{} {\hat R_w^{{b_k}}\hat p_{{b_{k + 1}}}^w = \hat R_w^{{b_k}}\left( {\hat p_{{b_k}}^w + \hat v_{{b_k}}^w{\rm{\Delta }}{t_k} - \frac{1}{2}{g^w}{\rm{\Delta }}t_k^2} \right) + \hat \alpha _{{b_{k + 1}}}^{{b_k}}}\\ {\hat R_w^{{b_k}}\hat v_{{b_{k + 1}}}^w = \hat R_w^{{b_k}}(\hat v_{{b_k}}^w - {g^w}{\rm{\Delta }}{t_k}) + \hat \beta _{{b_{k + 1}}}^{{b_k}}}\\ {\hat q_w^{{b_k}} \otimes \hat q_{{b_{k + 1}}}^w = \hat \gamma _{{b_{k + 1}}}^{{b_k}}} \end{array}
R^wbkp^bk+1w=R^wbk(p^bkw+v^bkwΔtk−21gwΔtk2)+α^bk+1bkR^wbkv^bk+1w=R^wbk(v^bkw−gwΔtk)+β^bk+1bkq^wbk⊗q^bk+1w=γ^bk+1bk
其中：
α
^
b
k
+
1
b
k
=
∫
∫
t
∈
[
t
k
,
t
k
+
1
]
R
^
t
b
k
(
a
^
t
−
b
^
a
t
)
d
t
2
β
^
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
R
^
t
b
k
(
a
^
t
−
b
^
a
t
)
d
t
γ
^
b
k
+
1
b
k
=
∫
t
∈
[
t
k
,
t
k
+
1
]
1
2
Ω
(
ω
^
t
−
b
^
ω
t
)
γ
t
b
k
d
t
\begin{array}{} {\hat \alpha _{{b_{k + 1}}}^{{b_k}} = \int\!\!\!\int \nolimits_{t \in [{t_k},{t_{k + 1}}]} \hat R_t^{{b_k}}({{\hat a}_t} - {{\hat b}_{{a_t}}})d{t^2}}\\ {\hat \beta _{{b_{k + 1}}}^{{b_k}} = \smallint \nolimits_{t \in [{t_k},{t_{k + 1}}]} \hat R_t^{{b_k}}({{\hat a}_t} - {{\hat b}_{{a_t}}})dt}\\ {\hat \gamma _{{b_{k + 1}}}^{{b_k}} = \smallint \nolimits_{t \in [{t_k},{t_{k + 1}}]} \frac{1}{2}{\rm{\Omega }}({{\hat \omega }_t} - {{\hat b}_{{\omega _t}}})\gamma _t^{{b_k}}dt} \end{array}
α^bk+1bk=∫∫t∈[tk,tk+1]R^tbk(a^t−b^at)dt2β^bk+1bk=∫t∈[tk,tk+1]R^tbk(a^t−b^at)dtγ^bk+1bk=∫t∈[tk,tk+1]21Ω(ω^t−b^ωt)γtbkdt
其对应的实际离散积分形式：
当使用普通离散增量形式时：
a
^
ˉ
i
′
=
a
^
i
−
b
^
a
i
,
ω
^
ˉ
i
′
=
ω
^
i
−
b
^
ω
i
{{\bar {\hat a}}_i}^\prime = {\hat a_i} - {\hat b_{a_i}},{{\bar {\hat \omega}_i}^\prime = {\hat \omega _i} - {\hat b_{\omega _i}}}
a^ˉi′=a^i−b^ai,ω^ˉi′=ω^i−b^ωi
利用中值积分时，
a
^
ˉ
i
′
=
1
2
[
q
^
i
(
a
^
i
−
b
^
a
i
)
+
q
^
i
+
1
(
a
^
i
+
1
−
b
^
a
i
)
]
{{\bar {\hat a}}_i}^\prime = \frac{1}{2}[{\hat q_i}({\hat a_i} - {\hat b_{{a_i}}}) + {\hat q_{i + 1}}({\hat a_{i + 1}} - {\hat b_{{a_i}}})]
a^ˉi′=21[q^i(a^i−b^ai)+q^i+1(a^i+1−b^ai)]
ω
^
ˉ
i
′
=
1
2
(
ω
^
i
+
ω
^
i
+
1
)
−
b
^
ω
i
{\bar {\hat \omega} _i}^\prime = \frac{1}{2}({\hat \omega _i} + {\hat \omega _{i + 1}}) - {\hat b_{\omega _i}}
ω^ˉi′=21(ω^i+ω^i+1)−b^ωi
误差模型
在使用马氏距离构成最小二乘问题时，我们需要知道估计量随着时间的推移，其误差的一个变化情况，因而需要对误差模型进行评估，进而对其各变量的协方差进行评估。VINS用于构造滑窗约束方程。
设估计为
z
^
\hat{z}
z^，真值为
z
z
z，则其误差定义为：
δ
z
=
z
−
z
^
\delta z=z-\hat{z}
δz=z−z^
当选用 z t b k = [ α t b k , β t b k , γ t b k , b a t , b ω t ] T z_t^{{b_k}} = {[\alpha _t^{{b_k}},\beta _t^{{b_k}},\gamma _t^{{b_k}},{b_{{a_t}}},{b_{{\omega _t}}}]^T} ztbk=[αtbk,βtbk,γtbk,bat,bωt]T作为预积分参数，
普通增量积分
- 其对应的普通增量积分的误差状态方程如下：
δ z ˙ t b k = ∂ δ z t b k ∂ δ t = J t \delta \dot z_t^{{b_k}} = \frac{{\partial \delta z_t^{{b_k}}}}{{\partial \delta t}} = {J_t} δz˙tbk=∂δt∂δztbk=Jt
通过计算，可得 J t = {J_t}= Jt=
从而得，当 δ t \delta t δt足够小时，
在积分初始状态，设 J 0 = I , P 0 = 0 J_0=I, P_0 = 0 J0=I,P0=0，可以计算出雅可比矩阵 J t + δ t {J_{t+\delta t}} Jt+δt及协方差矩阵 P t + δ t {P_{t+\delta t}} Pt+δt随时间推移的传递方程。
雅可比矩阵 J t + δ t {J_{t+\delta t}} Jt+δt的传递方程：
J t + δ t = ∂ δ z t + δ t b k ∂ δ t = δ z ˙ t + δ t b k = ∂ δ z t + δ t b k ∂ δ z t b k ∂ δ z t b k ∂ δ t = F J t = ( I + F t δ t ) J t {J_{t + \delta t}} = \frac{{\partial \delta z_{t + \delta t}^{{b_k}}}}{{\partial \delta t}} = \delta \dot z_{t + \delta t}^{{b_k}} = \frac{{\partial \delta z_{t + \delta t}^{{b_k}}}}{{\partial \delta z_t^{{b_k}}}}\frac{{\partial \delta z_t^{{b_k}}}}{{\partial \delta t}} = F{J_t} = (I + {F_t}\delta t){J_t} Jt+δt=∂δt∂δzt+δtbk=δz˙t+δtbk=∂δztbk∂δzt+δtbk∂δt∂δztbk=FJt=(I+Ftδt)Jt
其中：
∂ δ z t + δ t b k ∂ δ z t b k = F = ( I + F t δ t ) \frac{{\partial \delta z_{t + \delta t}^{{b_k}}}}{{\partial \delta z_t^{{b_k}}}} = F = (I + {F_t}\delta t) ∂δztbk∂δzt+δtbk=F=(I+Ftδt)
设：变量 a ∼ N ( μ a , Λ a ) , b ∼ N ( μ b , Λ b ) a\sim\mathcal{N}\left(\mu_a,\Lambda_a\right),b\sim\mathcal{N}\left(\mu_b,\Lambda_b\right) a∼N(μa,Λa),b∼N(μb,Λb)
则可知: F a ∼ N ( F μ a , F Λ a F T ) , G b ∼ N ( G μ b , G Λ b G T ) Fa\sim\mathcal{N}\left(F\mu_a,F\Lambda_aF^T\right),Gb\sim\mathcal{N}\left(G\mu_b,G\Lambda_bG^T\right) Fa∼N(Fμa,FΛaFT),Gb∼N(Gμb,GΛbGT)
从而得到协方差矩阵的传递公式：
设误差量 δ z b t b k ∼ N ( 0 , P t b k ) \delta z_{b_t}^{b_k}\sim\mathcal{N}(0,P_t^{b_k}) δzbtbk∼N(0,Ptbk),
δ z b t + δ t b k ∼ N ( 0 , P t + δ t b k ) \delta z_{b_{t+\delta t}}^{b_k}\sim\mathcal{N}\left(0,P_{t+\delta t}^{b_k}\right) δzbt+δtbk∼N(0,Pt+δtbk) 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
当我们使用迭代法估计零偏时，零偏值会在每次迭代时，进行更新，从而我们的IMU积分量则需要根据新的零偏进行重新的计算，比较耗时，因而，VINS作者提出，通过更新IMU积分量对零偏误差的雅可比矩阵，来迭代零偏更新量到IMU的积分值，从而简化计算步骤。
由 δ z = z − z ^ \delta z=z-\hat{z} δz=z−z^得：
∂ z ∂ b = ∂ δ z ∂ b \frac{\partial z}{\partial b}=\frac{\partial\delta z}{\partial b} ∂b∂z=∂b∂δz
从而可以从 ∂ δ z t + δ t b k ∂ δ z t b k \frac{{\partial \delta z_{t + \delta t}^{{b_k}}}}{{\partial \delta z_t^{{b_k}}}} ∂δztbk∂δzt+δtbk对应的雅可比矩阵中拿出关于零偏的雅可比矩阵。
注意： δ z ˙ t + δ t b k \delta \dot z_{t + \delta t}^{{b_k}} δz˙t+δtbk指的是对时间间隔 δ t \delta t δt的导数，而不是对 δ z ˙ t b k \delta \dot z_{t}^{{b_k}} δz˙tbk的导数，这里与VINS代码有所不同(代码中使用的 J t J_t Jt矩阵取的对零偏的雅可比矩阵)
中值积分法
- 使用中值积分法时，得到的状态方程传递方程如下：
以上图片来自《VINS 论文推导及代码解析 崔华坤》
参考文献
《VINS 论文推导及代码解析 崔华坤》
《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》
《Quaternion kinematics for the error-state Kalman filter》
![Python中批量提取[]括号内第一个元素的四种方法](https://img-blog.csdnimg.cn/direct/b4ac8fe970da4c6c92ff1b60a01c0124.png)
